void initControlData(void)
{
ctrldata.framecounter = 0;
ctrldata.needAnimation = FALSE;
vp_os_memset(ctrldata.needAnimationParam, 0, sizeof(ctrldata.needAnimationParam));
ctrldata.needVideoSwitch = -1;
ctrldata.needLedAnimation = FALSE;
vp_os_memset(ctrldata.needLedAnimationParam, 0, sizeof(ctrldata.needLedAnimationParam));
resetControlData();
ardrone_tool_input_start_reset();
navdata_write_to_file(FALSE);
ctrldata.navdata_connected = FALSE;
}
void initControlData(void)
{
ctrldata.framecounter = 0;
ctrldata.needSetFrequency = TRUE;
setNavdataDemo(TRUE);
ctrldata.needGetConfiguration = TRUE;
ctrldata.needSetPairing = TRUE;
ctrldata.needSetEmergency = FALSE;
ctrldata.isInEmergency = FALSE;
ctrldata.is_client = FALSE;
ctrldata.needSetManualTrim = FALSE;
ctrldata.manual_trim = FALSE;
ctrldata.manual_trim_enabled = FALSE;
ctrldata.trim_pitch = 0.0;
ctrldata.trim_roll = 0.0;
ctrldata.trim_yaw = 0.0;
ctrldata.needAnimation = -1;
ctrldata.needVideoSwitch = -1;
ctrldata.needChangeCameraDetection = -1;
ctrldata.tag_size = ARDRONE_TAG_SIZE_DEFAULT;
ctrldata.needLedAnimation = -1;
ctrldata.ledAnimFreq = 0.0f;
ctrldata.ledAnimDuration = 0;
ctrldata.wifiReachabled = FALSE;
ctrldata.showInterface = TRUE;
strcpy(ctrldata.error_msg, "");
strcpy(ctrldata.takeoff_msg, "take_off");
strcpy(ctrldata.emergency_msg, "emergency");
initNavdataControlData();
resetControlData();
ardrone_tool_start_reset();
}
void checkErrors(void)
{
input_state_t* input_state = ardrone_tool_get_input_state();
strcpy(ctrldata.error_msg, "");
if(!ctrldata.wifiReachabled)
{
strcat(ctrldata.error_msg, messages[MESSAGEBOX_WIFI_NOT_REACHABLED]);
strcat(ctrldata.error_msg, "\n");
resetControlData();
}
else
{
if(ctrldata.needAnimation >= 0)
{
ardrone_at_set_anim(ctrldata.needAnimation, MAYDAY_TIMEOUT[ctrldata.needAnimation]);
ctrldata.needAnimation = -1;
}
if(ctrldata.needVideoSwitch >= 0)
{
ardrone_at_zap(ctrldata.needVideoSwitch);
ctrldata.needVideoSwitch = -1;
}
if(ctrldata.needChangeCameraDetection >= 0)
{
ardrone_at_cad(ctrldata.needChangeCameraDetection, ctrldata.tag_size);
ctrldata.needChangeCameraDetection = -1;
}
if(ctrldata.needLedAnimation >= 0)
{
ardrone_at_set_led_animation(ctrldata.needLedAnimation, ctrldata.ledAnimFreq, ctrldata.ledAnimDuration);
ctrldata.needLedAnimation = -1;
}
if(ctrldata.needSetEmergency)
{
ctrldata.isInEmergency = ctrlnavdata.emergencyLanding;
ardrone_tool_set_ui_pad_select(1);
ctrldata.needSetEmergency = FALSE;
}
if( ctrldata.needSetNavdataDemo)
{
if( control_ack_configure_navdata_demo(ctrldata.navdata_demo))
ctrldata.needSetNavdataDemo = FALSE;
}
else if( ctrldata.needSetPairing )
{
if(control_ack_configure_mac_address((const char*)&iphone_mac_address[0]))
ctrldata.needSetPairing = FALSE;
}
else if(ctrldata.needSetFrequency)
{
if(control_ack_configure_ultrasound_frequency(!ctrldata.is_client))
ctrldata.needSetFrequency = FALSE;
}
else if(ctrldata.needSetManualTrim)
{
if(control_ack_configure_manual_trim(ctrldata.manual_trim))
{
ctrldata.needSetManualTrim = FALSE;
ctrldata.manual_trim_enabled = ctrldata.manual_trim;
}
}
else if(ctrldata.needGetConfiguration)
{
//PRINT("Request configuration file\n");
if(control_get_configuration())
ctrldata.needGetConfiguration = FALSE;
}
if((ctrldata.framecounter % (kFPS / 2)) == 0)
{
if(ctrlnavdata.bootstrap)
{
setNavdataDemo(TRUE);
ctrldata.needSetFrequency = TRUE;
ctrldata.needGetConfiguration = TRUE;
ctrldata.needSetPairing = TRUE;
}
}
if(ardrone_navdata_client_get_num_retries() >= NAVDATA_MAX_RETRIES)
{
strcat(ctrldata.error_msg, messages[MESSAGEBOX_CANT_CONNECT_TO_TOY]);
strcat(ctrldata.error_msg, "\n");
resetControlData();
}
else if(ctrlnavdata.emergencyLanding)
{
if(!ctrldata.isInEmergency && input_state->select)
ardrone_tool_set_ui_pad_select(0);
strcpy(ctrldata.emergency_msg, "reset");
strcat(ctrldata.error_msg, messages[MESSAGEBOX_EMERGENCY_PRESS_RESET]);
strcat(ctrldata.error_msg, "\n");
if(ctrlnavdata.ultrasoundProblem)
{
strcat(ctrldata.error_msg, messages[MESSAGEBOX_EMERGENCY_ULTRASOUND]);
strcat(ctrldata.error_msg, "\n");
}
else if(ctrlnavdata.cutoutProblem)
{
strcat(ctrldata.error_msg, messages[MESSAGEBOX_EMERGENCY_CUT_OUT]);
strcat(ctrldata.error_msg, "\n");
}
else if(ctrlnavdata.motorsProblem)
{
strcat(ctrldata.error_msg, messages[MESSAGEBOX_EMERGENCY_MOTORS]);
strcat(ctrldata.error_msg, "\n");
}
else if(ctrlnavdata.cameraProblem)
{
strcat(ctrldata.error_msg, messages[MESSAGEBOX_EMERGENCY_CAMERA]);
strcat(ctrldata.error_msg, "\n");
}
else if(ctrlnavdata.adcProblem)
{
strcat(ctrldata.error_msg, messages[MESSAGEBOX_EMERGENCY_PIC_WATCHDOG]);
strcat(ctrldata.error_msg, "\n");
}
else if(ctrlnavdata.adcVersionProblem)
{
strcat(ctrldata.error_msg, messages[MESSAGEBOX_EMERGENCY_PIC_VERSION]);
strcat(ctrldata.error_msg, "\n");
}
else if(ctrlnavdata.anglesProblem)
{
strcat(ctrldata.error_msg, messages[MESSAGEBOX_EMERGENCY_TOO_MUCH_ANGLE]);
strcat(ctrldata.error_msg, "\n");
}
else if(ctrlnavdata.vbatLowProblem)
{
strcat(ctrldata.error_msg, messages[MESSAGEBOX_EMERGENCY_BATTERY_LOW]);
strcat(ctrldata.error_msg, "\n");
}
else if(ctrlnavdata.userEmergency)
{
strcat(ctrldata.error_msg, messages[MESSAGEBOX_EMERGENCY_USER]);
strcat(ctrldata.error_msg, "\n");
}
else
{
strcat(ctrldata.error_msg, messages[MESSAGEBOX_EMERGENCY_UNKNOWN]);
strcat(ctrldata.error_msg, "\n");
}
resetControlData();
ardrone_tool_start_reset();
if(ctrlnavdata.startPressed)
switchTakeOff();
}
else if(!ctrlnavdata.emergencyLanding)
{
if(ctrldata.isInEmergency && input_state->select)
ardrone_tool_set_ui_pad_select(0);
if(video_stage_get_num_retries() > VIDEO_MAX_RETRIES)
{
strcat(ctrldata.error_msg, messages[MESSAGEBOX_ALERT_NO_VIDEO_CONNECTION]);
strcat(ctrldata.error_msg, "\n");
}
else if(ctrlnavdata.vbatLowProblem)
{
strcat(ctrldata.error_msg, messages[MESSAGEBOX_ALERT_BATTERY_LOW]);
strcat(ctrldata.error_msg, "\n");
}
else if(ctrlnavdata.ultrasoundProblem)
{
strcat(ctrldata.error_msg, messages[MESSAGEBOX_ALERT_ULTRASOUND]);
strcat(ctrldata.error_msg, "\n");
}
else if(ctrlnavdata.visionProblem && ctrlnavdata.flyingState)
{
strcat(ctrldata.error_msg, messages[MESSAGEBOX_ALERT_VISION]);
strcat(ctrldata.error_msg, "\n");
}
if(!ctrlnavdata.timerElapsed)
strcpy(ctrldata.emergency_msg, "emergency");
if(input_state->start)
{
if(ctrlnavdata.startPressed)
{
strcpy(ctrldata.takeoff_msg, "take_land");
}
else
{
strcpy(ctrldata.takeoff_msg, "take_off");
strcat(ctrldata.error_msg, messages[MESSAGEBOX_START_NOT_RECEIVED]);
strcat(ctrldata.error_msg, "\n");
}
}
else
{
strcpy(ctrldata.takeoff_msg, "take_off");
}
}
}
}
