vcs.maemo.org Git - mardrone/blob - mardrone/ARDrone_SDK_Version_1_8_20110726/ControlEngine/iPhone/Classes/Navdata/navdata.c

   1 #include "navdata.h"
   2 #include "ARDroneTypes.h"
   3 #include <control_states.h>
   4 #include <ardrone_tool/Navdata/ardrone_navdata_file.h>
   5 #include <ardrone_tool/Navdata/ardrone_navdata_client.h>
   6 navdata_unpacked_t inst_nav;
   7 vp_os_mutex_t inst_nav_mutex;
   8 extern char root_dir[];
   9 static bool_t writeToFile = FALSE;
  10
  11 static inline C_RESULT ardrone_navdata_init( void* data )
  12 {
  13         vp_os_mutex_init( &inst_nav_mutex );
  14
  15         vp_os_mutex_lock( &inst_nav_mutex);
  16         ardrone_navdata_reset_data(&inst_nav);
  17         vp_os_mutex_unlock( &inst_nav_mutex);
  18
  19         writeToFile = FALSE;
  20
  21         return C_OK;
  22 }
  23
  24 static inline C_RESULT ardrone_navdata_process( const navdata_unpacked_t* const navdata )
  25 {
  26         if( writeToFile )
  27         {
  28                 if( navdata_file == NULL )
  29                 {
  30                         ardrone_navdata_file_init(root_dir);
  31
  32                         PRINT("Saving in %s file\n", root_dir);
  33                 }
  34                 ardrone_navdata_file_process( navdata );
  35         }
  36         else
  37         {
  38                 if(navdata_file != NULL)
  39                         ardrone_navdata_file_release();
  40         }
  41
  42         vp_os_mutex_lock( &inst_nav_mutex);
  43 /*      inst_nav.ardrone_state = navdata->ardrone_state;
  44         inst_nav.vision_defined = navdata->vision_defined;
  45         vp_os_memcpy(&inst_nav.navdata_demo, &navdata->navdata_demo, sizeof(navdata_demo_t));
  46         vp_os_memcpy(&inst_nav.navdata_vision_detect, &navdata->navdata_vision_detect, sizeof(navdata_vision_detect_t));
  47 */
  48         vp_os_memcpy(&inst_nav, navdata, sizeof(navdata_unpacked_t));
  49         vp_os_mutex_unlock( &inst_nav_mutex );
  50
  51         return C_OK;
  52 }
  53
  54 static inline C_RESULT ardrone_navdata_release( void )
  55 {
  56         ardrone_navdata_file_release();
  57         return C_OK;
  58 }
  59
  60 C_RESULT ardrone_navdata_write_to_file(bool_t enable)
  61 {
  62         writeToFile = enable;
  63         return C_OK;
  64 }
  65
  66 C_RESULT ardrone_navdata_reset_data(navdata_unpacked_t *nav)
  67 {
  68         C_RESULT result = C_FAIL;
  69
  70         if(nav)
  71         {
  72                 vp_os_memset(nav, 0x0, sizeof(navdata_unpacked_t));
  73                 result = C_OK;
  74         }
  75
  76         return result;
  77 }
  78
  79 C_RESULT ardrone_navdata_get_data(navdata_unpacked_t *data)
  80 {
  81         C_RESULT result = C_FAIL;
  82
  83         if(data)
  84         {
  85                 vp_os_mutex_lock( &inst_nav_mutex );
  86 /*              data->ardrone_state = inst_nav.ardrone_state;
  87                 data->vision_defined = inst_nav.vision_defined;
  88                 vp_os_memcpy(&data->navdata_demo, &inst_nav.navdata_demo, sizeof(navdata_demo_t));
  89                 vp_os_memcpy(&data->navdata_vision_detect, &inst_nav.navdata_vision_detect, sizeof(navdata_vision_detect_t));
  90 */
  91                 vp_os_memcpy(data, &inst_nav, sizeof(navdata_unpacked_t));
  92                 vp_os_mutex_unlock( &inst_nav_mutex );
  93                 result = C_OK;
  94         }
  95
  96         return result;
  97 }
  98
  99 ARDRONE_FLYING_STATE ardrone_navdata_get_flying_state(navdata_unpacked_t data)
 100 {
 101         ARDRONE_FLYING_STATE tmp_state;
 102         switch ((data.navdata_demo.ctrl_state >> 16))
 103         {
 104                 case CTRL_FLYING:
 105                 case CTRL_HOVERING:
 106                 case CTRL_TRANS_GOTOFIX:
 107                         tmp_state = ARDRONE_FLYING_STATE_FLYING;
 108                         break;
 109
 110                 case CTRL_TRANS_TAKEOFF:
 111                         tmp_state = ARDRONE_FLYING_STATE_TAKING_OFF;
 112                         break;
 113
 114                 case CTRL_TRANS_LANDING:
 115                         tmp_state = ARDRONE_FLYING_STATE_LANDING;
 116                         break;
 117
 118                 case CTRL_DEFAULT:
 119                 case CTRL_LANDED:
 120                 default:
 121                         tmp_state = ARDRONE_FLYING_STATE_LANDED;
 122                         break;
 123         }
 124
 125         return tmp_state;
 126 }
 127
 128 BEGIN_NAVDATA_HANDLER_TABLE
 129 NAVDATA_HANDLER_TABLE_ENTRY(ardrone_navdata_init, ardrone_navdata_process, ardrone_navdata_release, NULL)
 130 END_NAVDATA_HANDLER_TABLE