git.maemo.org Git - mardrone/blob - mardrone/ARDrone_SDK_Version_1_8_20110726/Examples/Android/ardrone/project/jni/navdata.c

   1 #include "navdata.h"
   2
   3 #include "app.h"
   4 #include <control_states.h>
   5 #include <ardrone_tool/Navdata/ardrone_navdata_file.h>
   6 #include <ardrone_tool/Navdata/ardrone_navdata_client.h>
   7 #include <VP_Os/vp_os_signal.h>
   8
   9 instance_navdata_t inst_nav;
  10 vp_os_mutex_t inst_nav_mutex;
  11
  12 static inline C_RESULT ardrone_navdata_init( void* data )
  13 {
  14         ardrone_navdata_reset_data(&inst_nav);
  15
  16         vp_os_mutex_init( &inst_nav_mutex );
  17
  18         return C_OK;
  19 }
  20
  21 static inline C_RESULT ardrone_navdata_process( const navdata_unpacked_t* const navdata )
  22 {
  23         vp_os_mutex_lock( &inst_nav_mutex );
  24         inst_nav.flyingState = ((navdata->navdata_demo.ctrl_state >> 16) == CTRL_FLYING)
  25                                                 || ((navdata->navdata_demo.ctrl_state >> 16) == CTRL_HOVERING)
  26                                                 || ((navdata->navdata_demo.ctrl_state >> 16) == CTRL_TRANS_GOTOFIX);
  27
  28         inst_nav.pitch = navdata->navdata_demo.theta / 1000;
  29         inst_nav.yaw = navdata->navdata_demo.psi / 1000;
  30         inst_nav.roll = navdata->navdata_demo.phi / 1000;
  31         inst_nav.trimPitch = navdata->navdata_trims.euler_angles_trim_theta;
  32         inst_nav.trimYaw = navdata->navdata_trims.angular_rates_trim_r;
  33         inst_nav.trimRoll = navdata->navdata_trims.euler_angles_trim_phi;
  34         inst_nav.altitude = navdata->navdata_demo.altitude;
  35         inst_nav.vx = navdata->navdata_demo.vx;
  36         inst_nav.vy = navdata->navdata_demo.vy;
  37         inst_nav.vz = navdata->navdata_demo.vz;
  38         inst_nav.videoNumFrames = navdata->navdata_demo.num_frames;
  39         inst_nav.nbDetectedOpponent = navdata->navdata_vision_detect.nb_detected;
  40         vp_os_memcpy(inst_nav.xOpponent, navdata->navdata_vision_detect.xc, sizeof(int) * 4);
  41         vp_os_memcpy(inst_nav.yOpponent, navdata->navdata_vision_detect.yc, sizeof(int) * 4);
  42         vp_os_memcpy(inst_nav.widthOpponent, navdata->navdata_vision_detect.width, sizeof(int) * 4);
  43         vp_os_memcpy(inst_nav.heightOpponent, navdata->navdata_vision_detect.height, sizeof(int) * 4);
  44         vp_os_memcpy(inst_nav.distOpponent, navdata->navdata_vision_detect.dist, sizeof(int) * 4);
  45         vp_os_memcpy(inst_nav.detectCameraRot, &navdata->navdata_demo.detection_camera_rot, sizeof(float) * 9);
  46         vp_os_memcpy(inst_nav.detectCameraTrans, &navdata->navdata_demo.detection_camera_trans, sizeof(float) * 3);
  47         vp_os_memcpy(inst_nav.droneCameraRot, &navdata->navdata_demo.drone_camera_rot, sizeof(float) * 9);
  48         vp_os_memcpy(inst_nav.droneCameraTrans, &navdata->navdata_demo.drone_camera_trans, sizeof(float) * 3);
  49
  50         if(inst_nav.detectionType != navdata->navdata_demo.detection_camera_type)
  51                 printf("%s : detection type %d changed to %d\n", __FUNCTION__, inst_nav.detectionType,  navdata->navdata_demo.detection_camera_type);
  52
  53         inst_nav.detectionType = navdata->navdata_demo.detection_camera_type;
  54         inst_nav.detectTagIndex = navdata->navdata_demo.detection_tag_index;
  55         inst_nav.ardrone_state = navdata->ardrone_state;
  56         inst_nav.commandState = (navdata->ardrone_state & ARDRONE_COMMAND_MASK);
  57         inst_nav.comWatchdog = (navdata->ardrone_state & ARDRONE_COM_WATCHDOG_MASK);
  58         inst_nav.emergencyLanding = (navdata->ardrone_state & ARDRONE_EMERGENCY_MASK);
  59         inst_nav.startPressed = (navdata->ardrone_state & ARDRONE_USER_FEEDBACK_START);
  60         inst_nav.cutoutProblem = (navdata->ardrone_state & ARDRONE_CUTOUT_MASK);
  61         inst_nav.motorsProblem = (navdata->ardrone_state & ARDRONE_MOTORS_MASK);
  62         inst_nav.cameraProblem = !(navdata->ardrone_state & ARDRONE_VIDEO_THREAD_ON);
  63         inst_nav.adcVersionProblem = !(navdata->ardrone_state & ARDRONE_PIC_VERSION_MASK);
  64         inst_nav.adcProblem = (navdata->ardrone_state & ARDRONE_ADC_WATCHDOG_MASK);
  65         inst_nav.ultrasoundProblem = (navdata->ardrone_state & ARDRONE_ULTRASOUND_MASK);
  66         inst_nav.visionProblem = !(navdata->ardrone_state & ARDRONE_VISION_MASK);
  67         inst_nav.anglesProblem = (navdata->ardrone_state & ARDRONE_ANGLES_OUT_OF_RANGE);
  68         inst_nav.vbatLowProblem = (navdata->ardrone_state & ARDRONE_VBAT_LOW);
  69         inst_nav.userEmergency = (navdata->ardrone_state & ARDRONE_USER_EL);
  70         inst_nav.timerElapsed = (navdata->ardrone_state & ARDRONE_TIMER_ELAPSED);
  71         inst_nav.videoThreadOn = (navdata->ardrone_state & ARDRONE_VIDEO_THREAD_ON);
  72         inst_nav.navdataThreadOn = (navdata->ardrone_state & ARDRONE_NAVDATA_THREAD_ON);
  73         inst_nav.bootstrap = (navdata->ardrone_state & ARDRONE_NAVDATA_BOOTSTRAP);
  74         inst_nav.remainingBattery = navdata->navdata_demo.vbat_flying_percentage;
  75
  76         //PRINT( "command navdata demo:%d   ", (navdata->ardrone_state & ARDRONE_NAVDATA_DEMO_MASK) );
  77
  78
  79         vp_os_mutex_unlock( &inst_nav_mutex );
  80
  81         return C_OK;
  82 }
  83
  84 static inline C_RESULT ardrone_navdata_release( void )
  85 {
  86         return C_OK;
  87 }
  88
  89 C_RESULT ardrone_navdata_write_to_file(bool_t enable)
  90 {
  91         return C_OK;
  92 }
  93
  94 C_RESULT ardrone_navdata_reset_data(instance_navdata_t *nav)
  95 {
  96         nav->ardrone_state = 0;
  97         nav->flyingState = FALSE;
  98         nav->commandState = FALSE;
  99         nav->comWatchdog = FALSE;
 100         nav->bootstrap = TRUE;
 101         nav->emergencyLanding = FALSE;
 102         nav->startPressed = FALSE;
 103         nav->cutoutProblem = FALSE;
 104         nav->motorsProblem = FALSE;
 105         nav->cameraProblem = FALSE;
 106         nav->adcVersionProblem =FALSE;
 107         nav->adcProblem = FALSE;
 108         nav->ultrasoundProblem = FALSE;
 109         nav->visionProblem = TRUE;
 110         nav->anglesProblem = FALSE;
 111         nav->vbatLowProblem = FALSE;
 112         nav->userEmergency = FALSE;
 113         nav->pitch = 0.0f;
 114         nav->yaw = 0.0f;
 115         nav->roll = 0.0f;
 116         nav->trimPitch = 0.0f;
 117         nav->trimYaw = 0.0f;
 118         nav->trimRoll = 0.0f;
 119         nav->altitude = 0.0f;
 120         nav->vx = 0.0f;
 121         nav->vy = 0.0f;
 122         nav->vz = 0.0f;
 123         nav->remainingBattery = -1;
 124         nav->nbDetectedOpponent = 0;
 125         nav->videoNumFrames = 0;
 126         vp_os_memset(nav->xOpponent, 0x0, (sizeof(int) * 4));
 127         vp_os_memset(nav->yOpponent, 0x0, (sizeof(int) * 4));
 128         vp_os_memset(nav->widthOpponent, 0x0, (sizeof(int) * 4));
 129         vp_os_memset(nav->heightOpponent, 0x0, (sizeof(int) * 4));
 130         vp_os_memset(nav->distOpponent, 0x0, (sizeof(int) * 4));
 131         vp_os_memset(nav->detectCameraRot, 0x0, (sizeof(float) * 9));
 132         vp_os_memset(nav->detectCameraHomo, 0x0, (sizeof(float) * 9));
 133         vp_os_memset(nav->detectCameraTrans, 0x0, (sizeof(float) * 3));
 134         vp_os_memset(nav->droneCameraRot, 0x0, (sizeof(float) * 9));
 135         vp_os_memset(nav->droneCameraTrans, 0x0, (sizeof(float) * 3));
 136
 137         nav->detectionType = CAD_TYPE_NUM;
 138         nav->detectTagIndex = 0;
 139         nav->timerElapsed = FALSE;
 140         nav->videoThreadOn = FALSE;
 141         nav->navdataThreadOn = FALSE;
 142
 143         return C_OK;
 144 }
 145
 146 C_RESULT ardrone_navdata_get_data(instance_navdata_t *data)
 147 {
 148         C_RESULT result = C_FAIL;
 149
 150         if(data)
 151         {
 152                 vp_os_mutex_lock( &inst_nav_mutex );
 153                 vp_os_memcpy(data, &inst_nav, sizeof(instance_navdata_t));
 154                 vp_os_mutex_unlock( &inst_nav_mutex );
 155                 result = C_OK;
 156         }
 157
 158         return result;
 159 }
 160
 161 BEGIN_NAVDATA_HANDLER_TABLE
 162 NAVDATA_HANDLER_TABLE_ENTRY(ardrone_navdata_init, ardrone_navdata_process, ardrone_navdata_release, NULL)
 163 END_NAVDATA_HANDLER_TABLE