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

   1 /*
   2  *  ControlData.m
   3  *  ARDroneEngine
   4  *
   5  *  Created by Frederic D'HAEYER on 14/01/10.
   6  *  Copyright 2010 Parrot SA. All rights reserved.
   7  *
   8  */
   9 #include "ConstantsAndMacros.h"
  10 #include "ControlData.h"
  11
  12 //#define DEBUG_CONTROL
  13
  14 ControlData ctrldata = { 0 };
  15 navdata_unpacked_t ctrlnavdata;
  16 extern char iphone_mac_address[];
  17
  18 void setApplicationDefaultConfig(void)
  19 {
  20         ardrone_application_default_config.navdata_demo = TRUE;
  21         ardrone_application_default_config.navdata_options = (NAVDATA_OPTION_MASK(NAVDATA_DEMO_TAG) | NAVDATA_OPTION_MASK(NAVDATA_VISION_DETECT_TAG) | NAVDATA_OPTION_MASK(NAVDATA_GAMES_TAG));
  22         ardrone_application_default_config.video_codec = P264_CODEC;
  23         ardrone_application_default_config.bitrate_ctrl_mode = ARDRONE_VARIABLE_BITRATE_MODE_DYNAMIC;
  24         ctrldata.applicationDefaultConfigState = CONFIG_STATE_IDLE;
  25 }
  26
  27 void config_callback(bool_t result)
  28 {
  29         if(result)
  30                 ctrldata.configurationState = CONFIG_STATE_IDLE;
  31 }
  32
  33 void initControlData(void)
  34 {
  35         ctrldata.framecounter = 0;
  36
  37         ctrldata.needSetEmergency = FALSE;
  38         ctrldata.needSetTakeOff = FALSE;
  39         ctrldata.isInEmergency = FALSE;
  40         ctrldata.navdata_connected = FALSE;
  41
  42         ctrldata.needAnimation = FALSE;
  43         vp_os_memset(ctrldata.needAnimationParam, 0, sizeof(ctrldata.needAnimationParam));
  44
  45         ctrldata.needVideoSwitch = -1;
  46
  47         ctrldata.needLedAnimation = FALSE;
  48         vp_os_memset(ctrldata.needLedAnimationParam, 0, sizeof(ctrldata.needLedAnimationParam));
  49
  50         ctrldata.wifiReachabled = FALSE;
  51
  52         strcpy(ctrldata.error_msg, "");
  53         strcpy(ctrldata.takeoff_msg, "take_off");
  54         strcpy(ctrldata.emergency_msg, "emergency");
  55
  56         initNavdataControlData();
  57         resetControlData();
  58         ardrone_tool_start_reset();
  59
  60         ctrldata.configurationState = CONFIG_STATE_NEEDED;
  61
  62         ardrone_navdata_write_to_file(FALSE);
  63
  64         /* Setting default values for ControlEngine */
  65         ctrldata.applicationDefaultConfigState = CONFIG_STATE_NEEDED;
  66 }
  67
  68 void initNavdataControlData(void)
  69 {
  70         //drone data
  71         ardrone_navdata_reset_data(&ctrlnavdata);
  72 }
  73
  74 void resetControlData(void)
  75 {
  76         //printf("reset control data\n");
  77         ctrldata.accelero_flag = 0;
  78         inputPitch(0.0);
  79         inputRoll(0.0);
  80         inputYaw(0.0);
  81         inputGaz(0.0);
  82         initNavdataControlData();
  83 }
  84
  85 void configuration_get(void)
  86 {
  87         if(ctrldata.configurationState == CONFIG_STATE_IDLE)
  88                 ctrldata.configurationState = CONFIG_STATE_NEEDED;
  89 }
  90
  91 void switchTakeOff(void)
  92 {
  93 #ifdef DEBUG_CONTROL
  94         PRINT("%s\n", __FUNCTION__);
  95 #endif
  96         ctrldata.needSetTakeOff = TRUE;
  97 }
  98
  99 void emergency(void)
 100 {
 101 #ifdef DEBUG_CONTROL
 102         PRINT("%s\n", __FUNCTION__);
 103 #endif
 104         ctrldata.needSetEmergency = TRUE;
 105 }
 106
 107 void inputYaw(float percent)
 108 {
 109 #ifdef DEBUG_CONTROL
 110         PRINT("%s : %f\n", __FUNCTION__, percent);
 111 #endif
 112         if(-1.0 <= percent && percent <= 1.0)
 113                 ctrldata.yaw = percent;
 114         else if(-1.0 < percent)
 115                 ctrldata.yaw = -1.0;
 116         else
 117                 ctrldata.yaw = 1.0;
 118 }
 119
 120 void inputGaz(float percent)
 121 {
 122 #ifdef DEBUG_CONTROL
 123         PRINT("%s : %f\n", __FUNCTION__, percent);
 124 #endif
 125         if(-1.0 <= percent && percent <= 1.0)
 126                 ctrldata.gaz = percent;
 127         else if(-1.0 < percent)
 128                 ctrldata.gaz = -1.0;
 129         else
 130                 ctrldata.gaz = 1.0;
 131 }
 132
 133 void inputPitch(float percent)
 134 {
 135 #ifdef DEBUG_CONTROL
 136         PRINT("%s : %f, accelero_enable : %d\n", __FUNCTION__, percent, (ctrldata.accelero_flag >> ARDRONE_PROGRESSIVE_CMD_ENABLE) & 0x1 );
 137 #endif
 138         if(-1.0 <= percent && percent <= 1.0)
 139                 ctrldata.accelero_theta = -percent;
 140         else if(-1.0 < percent)
 141                 ctrldata.accelero_theta = 1.0;
 142         else
 143                 ctrldata.accelero_theta = -1.0;
 144 }
 145
 146 void inputRoll(float percent)
 147 {
 148 #ifdef DEBUG_CONTROL
 149         PRINT("%s : %f, accelero_enable : %d\n", __FUNCTION__, percent, (ctrldata.accelero_flag >> ARDRONE_PROGRESSIVE_CMD_ENABLE) & 0x1);
 150 #endif
 151         if(-1.0 <= percent && percent <= 1.0)
 152                 ctrldata.accelero_phi = percent;
 153         else if(-1.0 < percent)
 154                 ctrldata.accelero_phi = -1.0;
 155         else
 156                 ctrldata.accelero_phi = 1.0;
 157 }
 158
 159 void sendControls(void)
 160 {
 161         ardrone_at_set_progress_cmd(ctrldata.accelero_flag, ctrldata.accelero_phi, ctrldata.accelero_theta, ctrldata.gaz, ctrldata.yaw);
 162 }
 163
 164 void checkErrors(void)
 165 {
 166         input_state_t* input_state = ardrone_tool_get_input_state();
 167
 168         strcpy(ctrldata.error_msg, "");
 169
 170         if(!ctrldata.wifiReachabled)
 171         {
 172                 strcpy(ctrldata.error_msg, "WIFI NOT REACHABLE");
 173                 resetControlData();
 174         }
 175         else
 176         {
 177                 if(ctrldata.applicationDefaultConfigState == CONFIG_STATE_NEEDED)
 178                 {
 179                         ctrldata.applicationDefaultConfigState = CONFIG_STATE_IN_PROGRESS;
 180                         setApplicationDefaultConfig();
 181                 }
 182
 183                 if(ctrldata.configurationState == CONFIG_STATE_NEEDED)
 184                 {
 185                         ctrldata.configurationState = CONFIG_STATE_IN_PROGRESS;
 186                         ARDRONE_TOOL_CONFIGURATION_GET(config_callback);
 187                 }
 188
 189                 if(ctrldata.needSetTakeOff)
 190                 {
 191                         if(ctrlnavdata.ardrone_state & ARDRONE_EMERGENCY_MASK)
 192                         {
 193                                 ctrldata.needSetEmergency = TRUE;
 194                         }
 195                         else
 196                         {
 197                                 printf("Take off ...\n");
 198                                 if(!(ctrlnavdata.ardrone_state & ARDRONE_USER_FEEDBACK_START))
 199                                         ardrone_tool_set_ui_pad_start(1);
 200                                 else
 201                                         ardrone_tool_set_ui_pad_start(0);
 202                                 ctrldata.needSetTakeOff = FALSE;
 203                         }
 204                 }
 205
 206                 if(ctrldata.needSetEmergency)
 207                 {
 208                         ctrldata.isInEmergency = (ctrlnavdata.ardrone_state & ARDRONE_EMERGENCY_MASK);
 209                         ardrone_tool_set_ui_pad_select(1);
 210                         ctrldata.needSetEmergency = FALSE;
 211                 }
 212
 213                 if(ctrlnavdata.ardrone_state & ARDRONE_NAVDATA_BOOTSTRAP)
 214                 {
 215                         configuration_get();
 216                 }
 217
 218                 if(ardrone_navdata_client_get_num_retries() >= NAVDATA_MAX_RETRIES)
 219                 {
 220                         strcpy(ctrldata.error_msg, "CONTROL LINK NOT AVAILABLE");
 221                         ctrldata.navdata_connected = FALSE;
 222                         resetControlData();
 223                 }
 224                 else
 225                 {
 226                         ctrldata.navdata_connected = TRUE;
 227                         if(ctrlnavdata.ardrone_state & ARDRONE_EMERGENCY_MASK)
 228                         {
 229                                 if(!ctrldata.isInEmergency && input_state->select)
 230                                         ardrone_tool_set_ui_pad_select(0);
 231
 232                                 if(ctrlnavdata.ardrone_state & ARDRONE_CUTOUT_MASK)
 233                                 {
 234                                         strcpy(ctrldata.error_msg, "CUT OUT EMERGENCY");
 235                                 }
 236                                 else if(ctrlnavdata.ardrone_state & ARDRONE_MOTORS_MASK)
 237                                 {
 238                                         strcpy(ctrldata.error_msg, "MOTORS EMERGENCY");
 239                                 }
 240                                 else if(!(ctrlnavdata.ardrone_state & ARDRONE_VIDEO_THREAD_ON))
 241                                 {
 242                                         strcpy(ctrldata.error_msg, "CAMERA EMERGENCY");
 243                                 }
 244                                 else if(ctrlnavdata.ardrone_state & ARDRONE_ADC_WATCHDOG_MASK)
 245                                 {
 246                                         strcpy(ctrldata.error_msg, "PIC WATCHDOG EMERGENCY");
 247                                 }
 248                                 else if(!(ctrlnavdata.ardrone_state & ARDRONE_PIC_VERSION_MASK))
 249                                 {
 250                                         strcpy(ctrldata.error_msg, "PIC VERSION EMERGENCY");
 251                                 }
 252                                 else if(ctrlnavdata.ardrone_state & ARDRONE_ANGLES_OUT_OF_RANGE)
 253                                 {
 254                                         strcpy(ctrldata.error_msg, "TOO MUCH ANGLE EMERGENCY");
 255                                 }
 256                                 else if(ctrlnavdata.ardrone_state & ARDRONE_VBAT_LOW)
 257                                 {
 258                                         strcpy(ctrldata.error_msg, "BATTERY LOW EMERGENCY");
 259                                 }
 260                                 else if(ctrlnavdata.ardrone_state & ARDRONE_USER_EL)
 261                                 {
 262                                         strcpy(ctrldata.error_msg, "USER EMERGENCY");
 263                                 }
 264                                 else if(ctrlnavdata.ardrone_state & ARDRONE_ULTRASOUND_MASK)
 265                                 {
 266                                         strcpy(ctrldata.error_msg, "ULTRASOUND EMERGENCY");
 267                                 }
 268                                 else
 269                                 {
 270                                         strcpy(ctrldata.error_msg, "UNKNOWN EMERGENCY");
 271                                 }
 272
 273                                 strcpy(ctrldata.emergency_msg, "");
 274                                 strcpy(ctrldata.takeoff_msg, "take_off");
 275
 276                                 resetControlData();
 277                                 ardrone_tool_start_reset();
 278                         }
 279                         else // Not emergency landing
 280                         {
 281                                 if(ctrldata.isInEmergency && input_state->select)
 282                                 {
 283                                         ardrone_tool_set_ui_pad_select(0);
 284                                 }
 285
 286                                 if(video_stage_get_num_retries() > VIDEO_MAX_RETRIES)
 287                                 {
 288                                         strcpy(ctrldata.error_msg, "VIDEO CONNECTION ALERT");
 289                                 }
 290                                 else if(ctrlnavdata.ardrone_state & ARDRONE_VBAT_LOW)
 291                                 {
 292                                         strcpy(ctrldata.error_msg, "BATTERY LOW ALERT");
 293                                 }
 294                                 else if(ctrlnavdata.ardrone_state & ARDRONE_ULTRASOUND_MASK)
 295                                 {
 296                                         strcpy(ctrldata.error_msg, "ULTRASOUND ALERT");
 297                                 }
 298                                 else if(!(ctrlnavdata.ardrone_state & ARDRONE_VISION_MASK))
 299                                 {
 300                                         ARDRONE_FLYING_STATE tmp_state = ardrone_navdata_get_flying_state(ctrlnavdata);
 301                                         if(tmp_state == ARDRONE_FLYING_STATE_FLYING)
 302                                         {
 303                                                 strcpy(ctrldata.error_msg, "VISION ALERT");
 304                                         }
 305                                 }
 306
 307                                 if(!(ctrlnavdata.ardrone_state & ARDRONE_TIMER_ELAPSED))
 308                                         strcpy(ctrldata.emergency_msg, "emergency");
 309
 310                                 if(input_state->start)
 311                                 {
 312                                         if(ctrlnavdata.ardrone_state & ARDRONE_USER_FEEDBACK_START)
 313                                         {
 314                                                 strcpy(ctrldata.takeoff_msg, "take_land");
 315                                         }
 316                                         else
 317                                         {
 318                                                 strcpy(ctrldata.takeoff_msg, "take_off");
 319                                                 strcpy(ctrldata.error_msg, "START NOT RECEIVED");
 320
 321                                         }
 322                                 }
 323                                 else
 324                                 {
 325                                         strcpy(ctrldata.takeoff_msg, "take_off");
 326                                 }
 327                         }
 328                 }
 329         }
 330 }
 331