bool setLedAnimationCallback(ardrone_autonomy::LedAnim::Request& request, ardrone_autonomy::LedAnim::Response& response)
{
LED_ANIMATION_IDS anim_id = ledAnimMap[request.type % 14]; // Don't trick me
vp_os_mutex_lock(&twist_lock);
ardrone_at_set_led_animation(anim_id, (float) fabs(request.freq), (uint32_t) abs(request.duration));
vp_os_mutex_unlock(&twist_lock);
response.result = true;
return true;
}
//added by miguelb:
int _stdcall SetLedAnimation ( int iAnimation , float32_t fFrequency , uint32_t iDuration )
{
LED_ANIMATION_IDS iAnimationID;
//miguelb: There are more elegant ways to do this, but for now this should be enough
switch ( iAnimation )
{
case 1:
iAnimationID = BLINK_GREEN_RED ;
break;
case 2:
iAnimationID = BLINK_GREEN ;
break;
case 3:
iAnimationID = BLINK_RED ;
break;
case 4:
iAnimationID = BLINK_ORANGE;
break;
case 5:
iAnimationID = SNAKE_GREEN_RED;
break;
case 6:
iAnimationID = FIRE;
break;
case 7:
iAnimationID = STANDARD;
break;
case 8:
iAnimationID = RED;
break;
case 9:
iAnimationID = GREEN;
break;
case 10:
iAnimationID = RED_SNAKE;
break;
case 11:
iAnimationID = BLANK;
break;
case 12:
iAnimationID = RIGHT_MISSILE;
break;
case 13:
iAnimationID = LEFT_MISSILE;
break;
case 14:
iAnimationID = DOUBLE_MISSILE;
break;
default:
return 0;
}
ardrone_at_set_led_animation ( iAnimationID , fFrequency , iDuration);
return 1;
}
C_RESULT update_fpad(void){
/*FuzzyControl( &X , 0.0 , h1 );
FuzzyControl( &Y , 0.0 , h2 );
FuzzyControl( &Yaw , 0.0 , h3 );
FuzzyControl( &Z , 0.0 , h4 );//*/
//printf("%d\n",fcx);
//dispControl(&Z);
/*ardrone_at_set_progress_cmd( 1,
//roll/(float)(stick1LR-center_x1)/32767.0f,
//pitch/(float)(stick1UD-center_y1)/32767.0f,
//gaz/-(float)(stick2UD-center_x2)/32767.0f,
//yaw/(float)(stick2LR-center_y2)/32767.0f );*/
//printf("++%0.2f++\n",Z.vout);
if(heightTestMode==0){
if(theControl.phi!=0||theControl.theta!=0||theControl.gaz!=0||theControl.yaw!=0){
ardrone_at_set_progress_cmd( 1,theControl.phi,theControl.theta,theControl.gaz,theControl.yaw);
ardrone_at_set_led_animation(FIRE,10,1);
}
else{
bringItOn();
}
}
else{
static int timer=0;
static int theFlag=0;
if(timer<heightTestModeTop){
if(theFlag%2==0){
Z.ref=heightTestModeH1;
zg.ref=Z.ref;
}
else{
Z.ref=heightTestModeH2;
zg.ref=Z.ref;
}
timer++;
//printf("%d!\n",timer);
}
else {
printf("CHANGE to %0.2f !!!\n\n",Z.ref);
theFlag++;
timer=0;
}
bringItOn();
}
return C_OK;
}
void configManager_applyValue(uint32_t id) {
switch(id) {
case CONFIGPARAM_MAX_EULER_ANGLES:
ardrone_control_config.euler_angle_max = (configManager_getParamFloat(id, NULL) * M_PI) / 180.0;
ARDRONE_TOOL_CONFIGURATION_ADDEVENT(euler_angle_max, &ardrone_control_config.euler_angle_max, NULL);
break;
case CONFIGPARAM_MAX_VERTICAL_SPEED:
ardrone_control_config.control_vz_max = configManager_getParamFloat(id, NULL) * 1000.0;
ARDRONE_TOOL_CONFIGURATION_ADDEVENT(control_vz_max, &ardrone_control_config.control_vz_max, NULL);
break;
case CONFIGPARAM_MAX_YAW_SPEED:
ardrone_control_config.control_yaw = (configManager_getParamFloat(id, NULL) * M_PI) / 180.0;
ARDRONE_TOOL_CONFIGURATION_ADDEVENT(control_yaw, &ardrone_control_config.control_yaw, NULL);
break;
case CONFIGPARAM_MAX_ALTITUDE:
ardrone_control_config.altitude_max = configManager_getParamFloat(id, NULL) * 1000.0;
ARDRONE_TOOL_CONFIGURATION_ADDEVENT(altitude_max, &ardrone_control_config.altitude_max, NULL);
break;
case CONFIGPARAM_CAMERA_FRAMERATE:
ardrone_control_config.camif_fps = configManager_getParamFloat(id, NULL);
ARDRONE_TOOL_CONFIGURATION_ADDEVENT(camif_fps, &ardrone_control_config.camif_fps, NULL);
break;
case CONFIGPARAM_LEDS_ANIMATION:
{
LedsAnimation anim;
uint32_t actualLength;
configManager_getParam(CONFIGPARAM_LEDS_ANIMATION, (uint8_t *)&anim, sizeof(LedsAnimation), &actualLength);
ardrone_at_set_led_animation(anim.typeId, anim.frequencyHz, anim.durationSec);
}
break;
case CONFIGPARAM_VIDEO_ENCODING_TYPE:
videoServer_setOutputEncoding(configManager_getParamInt32(id, NULL));
break;
case CONFIGPARAM_VIDEO_ENCODING_QUALITY:
videoTranscoder_setQuality(configManager_getParamFloat(id, NULL));
break;
}
}
//NEW THREADS ADDED BY ME
DEFINE_THREAD_ROUTINE(drone_logic, data){
//the game is active from start, but the logic will start only when a match is active
struct timespec flying_sleep_time;
flying_sleep_time.tv_sec = 0;
flying_sleep_time.tv_nsec = 0000000;
int emptiness_counter = 0;
int shooting_counter = 0;
int hovering = 0; //0 hover, 1 move
float phi = 0.0; //left/right angle. Between [-1.0,+1.0], with negatives being leftward movement
float theta = 0.0; //front/back angle. Between [-1.0, +1.0], with negatives being frontward movement
float gaz = 0.0; //vertical speed. Between [-1.0, +1.0]
float yaw = 0.0; //Angular speed. Between [-1.0, +1.0]
while(game_active){
if(match_active){
if(takeoff){
//TODO:Uncomment this when you are ready to make the drone move autonomously
//ardrone_tool_set_ui_pad_start(1);
//ardrone_at_set_progress_cmd(0,0.0,0.0,0.0,0.0);
takeoff = 0;
}
//--- CHASING ---//
//NOTE: Hill have higher priority than enemy, this imply that if there is the enemy and an hill
//the drone will choose the hill.
//NOTE: doing so, the drone won't care if the enemy is standing between it and the hill
if(hill_in_sight){
emptiness_counter = 0;
shooting_counter = 0;
//move toward the hill
if((hill_distance > HILL_MIN_DISTANCE) && (hill_distance < HILL_MAX_DISTANCE)){
printf("MOVING TOWARD THE HILL\n");
hovering = 1;
phi = 0.0;
gaz = 0.0;
//--- SET THE YAW ---//
//This is to correct the direction of the drone
if(abs(hill_offset_from_center) > ERROR_FROM_CENTER_FOR_HILL){
//TODO: find the right multiplier for yaw and discover which way the drone turn
yaw = (hill_offset_from_center) * YAW_COEFF; //YAW_COEFF = 0.007
//yaw has to be between -1.0 and +1.0
if(yaw > 1.0) {
yaw = 1.0;
} else if(yaw < -1.0){
yaw = -1.0;
}
}
//--- SET THE APPROACHING SPEED ---//
//The closer the drone is to the hill, the slower it goes
//Need to be negative to move forward
theta = -1*((hill_distance) / THETA_COEFF); //TODO: find the right theta_coeff
if(theta > 0.0) {
theta = -0.1;
} else if(theta < -1.0){ //TODO: I may want to rethink the maximum speed that the drone can reach!
theta = -1.0;
}
flying_sleep_time.tv_sec = 1;
flying_sleep_time.tv_nsec = 0000000;
//hover over the hill
} else if(hill_distance < HILL_MIN_DISTANCE) {
//ardrone_at_set_progress_cmd(0,0,0,0,0); //to hover
printf("HOVERING ON TOP OF THE HILL\n");
hovering = 0;
phi = 0.0;
theta = 0.0;
gaz = 0.0;
yaw = 0.0;
//TODO: if you are close enough, you have to switch the cam and then inizialize
//the recognition procedure. (wait tot secs)
//TODO: I have problem switching cam
vp_os_mutex_lock(&drone_score_mutex);
drone_add_score = 1;
vp_os_mutex_unlock(&drone_score_mutex);
//TODO: here, you switch the camera back
flying_sleep_time.tv_sec = 5;
flying_sleep_time.tv_nsec = 0000000;
}
} else if(enemy_in_sight){
emptiness_counter = 0;
//back up! Too close to the enemy (we don't want to phisically hit the human player!)
if(enemy_distance < ENEMY_MIN_DISTANCE){
shooting_counter = 0;
hovering = 1;
theta = 1.0; //Move backward at maximum speed
phi = 0.0;
gaz = 0.0;
yaw = 0.0;
//TODO: test this!
flying_sleep_time.tv_sec = 2;
flying_sleep_time.tv_nsec = 0000000;
printf("BACKING UP FROM THE ENEMY\n");
} else if((enemy_distance > ENEMY_MIN_DISTANCE) && (enemy_distance < ENEMY_SHOOTING_DISTANCE)){
printf("INSIDE SHOOTING!!!!\n");
shooting_counter++;
if(shooting_counter > 5){
//TODO: make the drone move
//TODO:set some sleep time
printf("ENOUGH WITH THE SHOOTING\n");
} else {
//TODO: shoot!!
//make animation.
if(enemy_offset_from_center < ERROR_FROM_CENTER_FOR_ENEMY){
vp_os_mutex_lock(&enemy_score_mutex);
enemy_lose_score = 1;
vp_os_mutex_lock(&enemy_score_mutex);
}
printf("SHOOTING!!!!!!!!\n");
hovering = 0;
phi = 0.0;
theta = 0.0;
gaz = 0.0;
//--- SET THE YAW ---//
//This is to correct the direction of the drone
if(abs(enemy_offset_from_center) > ERROR_FROM_CENTER_FOR_ENEMY){
//TODO: find the right multiplier for yaw and discover wich way the drone turn
yaw = (enemy_offset_from_center) * YAW_COEFF; //YAW_COEFF = 0.007
//yaw has to be between -1.0 and +1.0
if(yaw > 1.0) {
yaw = 1.0;
} else if(yaw < -1.0){
yaw = -1.0;
}
}
flying_sleep_time.tv_sec = 1;
flying_sleep_time.tv_nsec = 0000000;
}
} else if((enemy_distance > ENEMY_SHOOTING_DISTANCE) && (enemy_distance < ENEMY_MAX_DISTANCE)){
shooting_counter = 0;
//TODO: in this case I may want to just make the drone search for hills
//TODO: it may escape, turning itself so the led won't face the enemy anymore
//TODO: set some sleep time
//--- SET THE YAW ---//
//This is to correct the direction of the drone
if(abs(enemy_offset_from_center) > ERROR_FROM_CENTER_FOR_ENEMY){
//TODO: find the right multiplier for yaw and discover wich way the drone turn
//TODO: in this case, I won't to make the drone move away from the enemy, so the algorithm below
//has to be changed
yaw = (enemy_offset_from_center) * YAW_COEFF; //YAW_COEFF = 0.007
//yaw has to be between -1.0 and +1.0
if(yaw > 1.0) {
yaw = 1.0;
} else if(yaw < -1.0){
yaw = -1.0;
}
}
}
//NOTHING IN SIGHT
} else {
//If nothing is in sight I set a counter that increment every time I pass directly from here
//and make the drone rotate around itself. After tot passages with nothing in sight I take measure to land the drone.
if(emptiness_counter == 0){
//TODO: set up everything for the algorithm to start
emptiness_counter = 1;
//yaw = something != to zero;
//everything else if zero (I don't know about hovering... problably should be 1 here)
} else {
emptiness_counter++;
//TODO: set some sleep time
if(emptiness_counter > 10){
//TODO:land the drone and make the game stop
//so set everything that should to zero
}
}
}
//---TELL THE DRONE TO MOVE---//
//TODO: uncomment the line below when you're ready!
//ardrone_at_set_progress_cmd(hovering,phi,theta,gaz,yaw);
nanosleep(&flying_sleep_time, NULL);
//NOTE: if the sleep remain under 2 secs it's ok.
//Only the hill recognition can have more, because the drone can't be hurt at that time.
//NOTE: I don't know why, but if I add this printf the condition below works
//printf("%d",drone_wounded);
//---BEING HIT---//
if(drone_wounded){
//TODO: make the drone move as if it was being shot
//maybe this should be moved in the flying thread
vp_os_mutex_lock(&drone_wound_mutex);
drone_wounded = 0;
vp_os_mutex_unlock(&drone_wound_mutex);
//TODO: you can choose between this animation, defined in Soft/Common/config.h
/*ARDRONE_ANIM_PHI_M30_DEG= 0,
A RDRONE_ANIM_PHI_30_DE*G,
ARDRONE_ANIM_THETA_M30_DEG,
ARDRONE_ANIM_THETA_30_DEG,
ARDRONE_ANIM_THETA_20DEG_YAW_200DEG,
ARDRONE_ANIM_THETA_20DEG_YAW_M200DEG,
ARDRONE_ANIM_TURNAROUND,
ARDRONE_ANIM_TURNAROUND_GODOWN,
ARDRONE_ANIM_YAW_SHAKE,
ARDRONE_ANIM_YAW_DANCE,
ARDRONE_ANIM_PHI_DANCE,
ARDRONE_ANIM_THETA_DANCE,
ARDRONE_ANIM_VZ_DANCE,
ARDRONE_ANIM_WAVE,
ARDRONE_ANIM_PHI_THETA_MIXED,
ARDRONE_ANIM_DOUBLE_PHI_THETA_MIXED,
ARDRONE_ANIM_FLIP_AHEAD,
ARDRONE_ANIM_FLIP_BEHIND,
ARDRONE_ANIM_FLIP_LEFT,
ARDRONE_ANIM_FLIP_RIGHT,
ARDRONE_NB_ANIM_MAYDAY*/
//anim_mayday_t param;
//ARDRONE_TOOL_CONFIGURATION_ADDEVENT (flight_anim, param, myCallback);
ardrone_at_set_led_animation(BLINK_GREEN_RED, 0.25, 4);
//TODO: freeze the drone for some time, also?
//nanosleep(&shot_rumble_time, NULL);
}
//MATCH OVER
} else {
//land the drone
ardrone_at_set_progress_cmd(0,0.0,0.0,0.0,0.0);
ardrone_tool_set_ui_pad_start(0);
//TODO: I may need some sleep time to make the drone land before closing everyting
}
}
return C_OK;
}
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");
}
}
}
}
