// ros service callback function for toggling Cam
bool toggleCamCallback(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response)
{
if (cam_state == 0) // toggle to 1, the vertical camera
{
cam_state = 1;
#ifdef _USING_SDK_1_7_
ARDRONE_TOOL_CONFIGURATION_ADDEVENT (video_channel, &cam_state, NULL);
#else
ardrone_at_set_toy_configuration("video:video_channel","1");
#endif
fprintf(stderr, "\nToggling from frontal camera to vertical camera.\n");
}
else if (cam_state == 1) // toggle to the forward camera
{
cam_state = 0;
#ifdef _USING_SDK_1_7_
ARDRONE_TOOL_CONFIGURATION_ADDEVENT (video_channel, &cam_state, NULL);
#else
ardrone_at_set_toy_configuration("video:video_channel","0");
#endif
fprintf(stderr, "\nToggling from vertical camera to frontal camera.\n");
}
return true;
}
// ros service callback to turn on and off camera recording
bool SetRecordCallback(ardrone_autonomy::RecordEnable::Request &request,
ardrone_autonomy::RecordEnable::Response& response)
{
char record_command[ARDRONE_DATE_MAXSIZE + 64];
int32_t new_codec;
if (request.enable == true)
{
char date[ARDRONE_DATE_MAXSIZE];
time_t t = time(NULL);
// For some reason the linker can't find this, so we'll just do it manually, cutting and pasting
// ardrone_time2date(t, ARDRONE_FILE_DATE_FORMAT, date);
strftime(date, ARDRONE_DATE_MAXSIZE, ARDRONE_FILE_DATE_FORMAT, localtime(&t));
snprintf(record_command, sizeof(record_command), "%d,%s", USERBOX_CMD_START, date);
new_codec = MP4_360P_H264_720P_CODEC;
}
else
{
snprintf(record_command, sizeof(record_command), "%d", USERBOX_CMD_STOP);
new_codec = H264_360P_CODEC;
}
vp_os_mutex_lock(&twist_lock);
ARDRONE_TOOL_CONFIGURATION_ADDEVENT(video_codec, &new_codec, NULL);
ARDRONE_TOOL_CONFIGURATION_ADDEVENT(userbox_cmd, record_command, NULL);
vp_os_mutex_unlock(&twist_lock);
response.result = true;
return true;
}
bool configCb(ardrone2_mudd::Config::Request &req,
ardrone2_mudd::Config::Response &res)
{
std::vector<std::string> command;
std::istringstream iss(req.command);
std::copy(std::istream_iterator<std::string>(iss),
std::istream_iterator<std::string>(),
std::back_inserter<std::vector<std::string> >(command));
printf("Command: %s \n", command[0].c_str());
if (command[0].compare("camera") == 0) {
int channel = atoi(command[1].c_str());
if ((channel >= 0) && (channel <= 4))
{
if (channel == 4)
currentCamera = (currentCamera + 1)%3;
else
currentCamera = channel;
ARDRONE_TOOL_CONFIGURATION_ADDEVENT(video_channel, &channel, NULL);
}
} else if (command[0].compare("anim") == 0) {
char buffer[128];
int anim = atoi(command[1].c_str());
snprintf(buffer,sizeof(buffer),"%i,%i",anim, MAYDAY_TIMEOUT[anim]);
ARDRONE_TOOL_CONFIGURATION_ADDEVENT(flight_anim, buffer, NULL);
} else if (command[0].compare("retrim") == 0) {
ardrone_at_set_flat_trim();
}
return true;
}
void parrot_ardrone_ctrl_switch_camera(ardrone_tool_configuration_callback callback)
{
if(channel++ == ARDRONE_VIDEO_CHANNEL_LAST)
channel = ARDRONE_VIDEO_CHANNEL_FIRST;
ARDRONE_TOOL_CONFIGURATION_ADDEVENT(video_channel, (int32_t*)&channel, callback);
}
static void ihm_send_VideoCodec(GtkComboBox *widget, gpointer data) {
gint pos;
int32_t codec;
pos = gtk_combo_box_get_active( widget );
codec = (pos == 0) ? UVLC_CODEC : P264_CODEC;
ARDRONE_TOOL_CONFIGURATION_ADDEVENT(video_codec, &codec, NULL);
}
// Older rostopic callback function for toggling Cam
void toggleCamCallback(const std_msgs::Empty &msg)
{
if (cam_state == 0) // toggle to 1, the vertical camera
{
cam_state = 1; //rujian aug 30
ARDRONE_TOOL_CONFIGURATION_ADDEVENT (video_channel, &cam_state, NULL);
//ardrone_at_set_toy_configuration("video:video_channel","1");
fprintf(stderr, "\nToggling from frontal camera to vertical camera.\n");
}
else if (cam_state == 1) // toggle to the forward camera
{
cam_state = 0;
ARDRONE_TOOL_CONFIGURATION_ADDEVENT (video_channel, &cam_state, NULL);
//ardrone_at_set_toy_configuration("video:video_channel","0");
fprintf(stderr, "\nToggling from vertical camera to frontal camera.\n");
}
} //rujian aug 8
// ros service callback function for toggling Cam
bool toggleCamCallback(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response)
{
const int _modes = (IS_ARDRONE1) ? 4 : 2;
cam_state = (cam_state + 1) % _modes;
ARDRONE_TOOL_CONFIGURATION_ADDEVENT (video_channel, &cam_state, NULL);
fprintf(stderr, "\nSetting camera channel to : %d.\n", cam_state);
return true;
}
void zap()
{
g_is_bellycam = !g_is_bellycam;
int32_t channel = g_is_bellycam ? ZAP_CHANNEL_VERT : ZAP_CHANNEL_HORI;
ARDRONE_TOOL_CONFIGURATION_ADDEVENT(video_channel, &channel, NULL);
}
//ros service callback to set the camera channel
bool setCamChannelCallback(ardrone_autonomy::CamSelect::Request& request, ardrone_autonomy::CamSelect::Response& response)
{
const int _modes = (IS_ARDRONE1) ? 4 : 2;
cam_state = request.channel % _modes;
ARDRONE_TOOL_CONFIGURATION_ADDEVENT (video_channel, &cam_state, NULL);
fprintf(stderr, "\nSetting camera channel to : %d.\n", cam_state);
response.result = true;
return true;
}
void ardrone_academy_navdata_userbox_switch(void)
{
static char param[ARDRONE_DATE_MAXSIZE + 8];
static char date[ARDRONE_DATE_MAXSIZE];
if(navdata_state.userbox_state == USERBOX_STATE_STOPPED)
{
navdata_state.userbox_time = time(NULL);
ardrone_time2date(navdata_state.userbox_time, ARDRONE_FILE_DATE_FORMAT, date);
sprintf(param, "%d,%s", USERBOX_CMD_START, date);
navdata_state.userbox_state = USERBOX_STATE_STARTING;
ARDRONE_TOOL_CONFIGURATION_ADDEVENT(userbox_cmd, param, ardrone_academy_navdata_userbox_cb);
}
else if(navdata_state.userbox_state == USERBOX_STATE_STARTED)
{
sprintf(param, "%d", USERBOX_CMD_STOP);
navdata_state.userbox_state = USERBOX_STATE_STOPPING;
ARDRONE_TOOL_CONFIGURATION_ADDEVENT(userbox_cmd, param, ardrone_academy_navdata_userbox_cb);
}
}
bool ARDroneRos::toggleCamCallback(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response)
{
if (ardrone_application_default_config.video_channel == 0)
{
ROS_INFO("Toggling from frontal camera to vertical camera.");
ardrone_application_default_config.video_channel = 1;
ARDRONE_TOOL_CONFIGURATION_ADDEVENT(video_channel,
&ardrone_application_default_config.video_channel,
NULL);
}
else
{
ROS_INFO("Toggling from vertical camera to frontal camera.");
ardrone_application_default_config.video_channel = 0;
ARDRONE_TOOL_CONFIGURATION_ADDEVENT(video_channel,
&ardrone_application_default_config.video_channel,
NULL);
}
return true;
}
void send_new_custom_session_request(GtkWidget *widget, gpointer data)
{
const char * id = gtk_entry_get_text(GTK_ENTRY(ihm_config.new_custom_configs[CAT_SESSION].entry));
if (configuration_check_config_id(id)==C_OK)
{
printf("Creating/requesting custom session <%s>\n",id);
ARDRONE_TOOL_CONFIGURATION_ADDEVENT(session_id, (char*)id, send_custom_application_request_callback);
ARDRONE_TOOL_CONFIGURATION_GET(configuration_received_callback);
}
}
bool setFlightAnimationCallback(ardrone_autonomy::FlightAnim::Request &request, ardrone_autonomy::FlightAnim::Response &response)
{
char param[20];
const int anim_type = request.type % ARDRONE_NB_ANIM_MAYDAY;
const int anim_duration = (request.duration > 0) ? request.duration : MAYDAY_TIMEOUT[anim_type];
snprintf(param, sizeof (param), "%d,%d", anim_type, anim_duration);
vp_os_mutex_lock(&twist_lock);
ARDRONE_TOOL_CONFIGURATION_ADDEVENT(flight_anim, param, NULL);
vp_os_mutex_unlock(&twist_lock);
response.result = true;
return true;
}
void send_custom_session_request(GtkWidget *widget, gpointer data)
{
const char * id;
id = gtk_combo_box_get_active_text(GTK_COMBO_BOX(widget));
if (configuration_check_config_id(id)==C_OK)
{
printf("Requesting custom session <%s>\n",id);
ARDRONE_TOOL_CONFIGURATION_ADDEVENT(session_id, (char*)id, send_custom_session_request_callback);
ARDRONE_TOOL_CONFIGURATION_GET(configuration_received_callback);
}
}
C_RESULT ardrone_general_navdata_init( void* data )
{
ARDRONE_TOOL_CONFIGURATION_ADDEVENT (session_id, "-all", NULL);
navdataState = NAVDATA_REQUEST_IDLE;
configState = MULTICONFIG_NEEDED;
appSwitch = 1;
usrSwitch = 1;
sesSwitch = 1;
navdataNeeded = 1;
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;
}
}
void ardrone_demo_config_callback(unsigned int success)
{
ardrone_tool_configuration_callback cb = (ardrone_tool_configuration_callback) ardrone_demo_config_callback;
if (success && ++ardrone_nr_configs_suc <= ardrone_demo_nr_configs)
{
switch (ardrone_nr_configs_suc)
{
/*
case 1:
ARDRONE_TOOL_CONFIGURATION_ADDEVENT(video_codec, &ardrone_control_config.video_codec, cb);
break;
*/
case 1:
ARDRONE_TOOL_CONFIGURATION_ADDEVENT (navdata_demo, &ardrone_control_config.navdata_demo, cb);
break;
case 2:
ARDRONE_TOOL_CONFIGURATION_ADDEVENT (altitude_max, &ardrone_control_config.altitude_max, cb);
break;
case 3:
ARDRONE_TOOL_CONFIGURATION_ADDEVENT (control_vz_max, &ardrone_control_config.control_vz_max, cb);
break;
case 4:
ARDRONE_TOOL_CONFIGURATION_ADDEVENT (outdoor, &ardrone_control_config.outdoor, cb);
break;
case 5:
SetEvent(ardrone_ready);
ardrone_demo_redirect_to_interface = 1;
break;
}
}
}
void ARDroneRos::configure()
{
ROS_INFO("Setting the video properties...");
configure_video(requested_resolution_); // this function is located in ardrone_video.cpp
ROS_INFO("Setting the navdata properties...");
configure_navdata(); // this function is located in ardrone_navdata.cpp
ROS_INFO("Setting the control properties...");
configure_control(); // this function is located in ardrone_control.cpp
ROS_INFO("Setting the indoor mode...");
ardrone_application_default_config.outdoor = FALSE;
ARDRONE_TOOL_CONFIGURATION_ADDEVENT (outdoor,
&ardrone_application_default_config.outdoor,
NULL);
}
void tag_configurate( const char color) {
/*deactivate navdata demo*/
bool_t demo = FALSE;
ARDRONE_TOOL_CONFIGURATION_ADDEVENT(navdata_demo, &demo, ack);
/*activate fullmask navdata*/
int32_t options = NAVDATA_OPTION_FULL_MASK;
ARDRONE_TOOL_CONFIGURATION_ADDEVENT (navdata_options, &options, ack);
/*setting the color*/
int32_t enemyColors = ARDRONE_DETECTION_COLOR_ORANGE_BLUE;
switch (color){
case 'b':
enemyColors = ARDRONE_DETECTION_COLOR_ORANGE_BLUE;
break;
case 'g':
enemyColors = ARDRONE_DETECTION_COLOR_ORANGE_GREEN;
break;
case 'y':
enemyColors = ARDRONE_DETECTION_COLOR_ORANGE_YELLOW;
break;
}
printf("setting tag color %d\n", enemyColors);
ARDRONE_TOOL_CONFIGURATION_ADDEVENT (enemy_colors, &enemyColors, ack);
printf("setting shell\n");
int32_t activated = 0;
ARDRONE_TOOL_CONFIGURATION_ADDEVENT (enemy_without_shell, &activated, ack);
printf("setting detection mode\n");
int32_t detectType = CAD_TYPE_MULTIPLE_DETECTION_MODE;
ARDRONE_TOOL_CONFIGURATION_ADDEVENT (detect_type, &detectType, ack);
/*detection on h_cam at 30fps*/
printf("setting detection h_cam\n");
int32_t detectH = TAG_TYPE_MASK (TAG_TYPE_SHELL_TAG);
ARDRONE_TOOL_CONFIGURATION_ADDEVENT (detections_select_h, &detectH, ack);
/*detection on v_cam at 60fps*/
printf("setting detection v_cam\n");
int32_t detectV = TAG_TYPE_MASK (TAG_TYPE_ROUNDEL);
ARDRONE_TOOL_CONFIGURATION_ADDEVENT (detections_select_v, &detectV, ack);
}
void ardrone_academy_check_take_off_timeout (void)
{
static char cancelCommand[ARDRONE_DATE_MAXSIZE];
time_t elapsedTime = time (NULL) - navdata_state.takeoff_time;
if (TAKEOFF_TIMEOUT_SEC < elapsedTime)
{
switch (navdata_state.takeoff_state)
{
case TAKEOFF_STATE_WAIT_USERBOX:
snprintf (cancelCommand, ARDRONE_DATE_MAXSIZE-1, "%d", USERBOX_CMD_CANCEL);
navdata_state.userbox_state = USERBOX_STATE_STOPPING;
navdata_state.takeoff_state = TAKEOFF_STATE_CANCELLING;
ARDRONE_TOOL_CONFIGURATION_ADDEVENT (userbox_cmd, cancelCommand, ardrone_academy_navdata_userbox_cb);
navdata_state.takeOffWasCancelled = TRUE;
break;
case TAKEOFF_STATE_NEEDED:
navdata_state.takeoff_state = TAKEOFF_STATE_IDLE;
navdata_state.takeOffWasCancelled = TRUE;
break;
default:
break;
}
}
}
void looping() {
char param[20];
snprintf (param, sizeof (param), "%d,%d", ARDRONE_ANIM_FLIP_LEFT, MAYDAY_TIMEOUT[ARDRONE_ANIM_FLIP_LEFT]);
ARDRONE_TOOL_CONFIGURATION_ADDEVENT (flight_anim, param, NULL);
}
void hoola_hoop() {
char param[20];
snprintf (param, sizeof (param), "%d,%d", ARDRONE_ANIM_WAVE, MAYDAY_TIMEOUT[ARDRONE_ANIM_WAVE]);
ARDRONE_TOOL_CONFIGURATION_ADDEVENT (flight_anim, param, NULL);
}
C_RESULT ardrone_tool_init_custom(void)
{
/**
* Set application default configuration
*
* In this example, we use the AR.FreeFlight configuration :
* - Demo navdata rate (15Hz)
* - Useful additionnal navdata packets enabled (detection, games, video record, wifi quality estimation ...)
* - Adaptive video enabled (bitrate_ctrl_mode) -> video bitrate will change according to the available bandwidth
*/
ardrone_application_default_config.navdata_demo = TRUE;
//use this -> NAVDATA_OPTION_FULL_MASK
//or coment the line below if detection doesn't work
ardrone_application_default_config.navdata_options = NAVDATA_OPTION_FULL_MASK;//(NAVDATA_OPTION_MASK(NAVDATA_DEMO_TAG) | NAVDATA_OPTION_MASK(NAVDATA_VISION_DETECT_TAG) | NAVDATA_OPTION_MASK(NAVDATA_GAMES_TAG) | NAVDATA_OPTION_MASK(NAVDATA_MAGNETO_TAG) | NAVDATA_OPTION_MASK(NAVDATA_HDVIDEO_STREAM_TAG) | NAVDATA_OPTION_MASK(NAVDATA_WIFI_TAG));
if (IS_ARDRONE2){
ardrone_application_default_config.video_codec = drone2Codec;
} else {
ardrone_application_default_config.video_codec = drone1Codec;
}
ardrone_application_default_config.video_channel = videoChannel;
ardrone_application_default_config.bitrate_ctrl_mode = 1;
/**
* Define the number of video stages we'll add before/after decoding
*/
#define EXAMPLE_PRE_STAGES 1
#define EXAMPLE_POST_STAGES 1
/**
* Allocate useful structures :
* - index counter
* - thread param structure and its substructures
*/
uint8_t stages_index = 0;
specific_parameters_t *params = (specific_parameters_t *)vp_os_calloc (1, sizeof (specific_parameters_t));
specific_stages_t *example_pre_stages = (specific_stages_t *)vp_os_calloc (1, sizeof (specific_stages_t));
specific_stages_t *example_post_stages = (specific_stages_t *)vp_os_calloc (1, sizeof (specific_stages_t));
vp_api_picture_t *in_picture = (vp_api_picture_t *)vp_os_calloc (1, sizeof (vp_api_picture_t));
vp_api_picture_t *out_picture = (vp_api_picture_t *)vp_os_calloc (1, sizeof (vp_api_picture_t));
/**
* Fill the vp_api_pictures used for video decodig
* --> out_picture->format is mandatory for AR.Drone 1 and 2. Other lines are only necessary for AR.Drone 1 video decoding
*/
in_picture->width = 640; // Drone 1 only : Must be greater than the drone 1 picture size (320)
in_picture->height = 360; // Drone 1 only : Must be greater that the drone 1 picture size (240)
out_picture->framerate = 20; // Drone 1 only, must be equal to drone target FPS
out_picture->format = PIX_FMT_RGB24; // MANDATORY ! Only RGB24, RGB565 are supported
out_picture->width = in_picture->width;
out_picture->height = in_picture->height;
// Alloc Y, CB, CR bufs according to target format
uint32_t bpp = 0;
switch (out_picture->format)
{
case PIX_FMT_RGB24:
// One buffer, three bytes per pixel
bpp = 3;
out_picture->y_buf = vp_os_malloc ( out_picture->width * out_picture->height * bpp );
out_picture->cr_buf = NULL;
out_picture->cb_buf = NULL;
out_picture->y_line_size = out_picture->width * bpp;
out_picture->cb_line_size = 0;
out_picture->cr_line_size = 0;
break;
case PIX_FMT_RGB565:
// One buffer, two bytes per pixel
bpp = 2;
out_picture->y_buf = vp_os_malloc ( out_picture->width * out_picture->height * bpp );
out_picture->cr_buf = NULL;
out_picture->cb_buf = NULL;
out_picture->y_line_size = out_picture->width * bpp;
out_picture->cb_line_size = 0;
out_picture->cr_line_size = 0;
break;
default:
fprintf (stderr, "Wrong video format, must be either PIX_FMT_RGB565 or PIX_FMT_RGB24\n");
exit (-1);
break;
}
/**
* Allocate the stage lists
*
* - "pre" stages are called before video decoding is done
* -> A pre stage get the encoded video frame (including PaVE header for AR.Drone 2 frames) as input
* -> A pre stage MUST NOT modify these data, and MUST pass it to the next stage
* - Typical "pre" stage : Encoded video recording for AR.Drone 1 (recording for AR.Drone 2 is handled differently)
*
* - "post" stages are called after video decoding
* -> The first post stage will get the decoded video frame as its input
* --> Video frame format depend on out_picture->format value (RGB24 / RGB565)
* -> A post stage CAN modify the data, as ardrone_tool won't process it afterwards
* -> All following post stages will use the output of the previous stage as their inputs
* - Typical "post" stage : Display the decoded frame
*/
example_pre_stages->stages_list = (vp_api_io_stage_t *)vp_os_calloc (EXAMPLE_PRE_STAGES, sizeof (vp_api_io_stage_t));
example_post_stages->stages_list = (vp_api_io_stage_t *)vp_os_calloc (EXAMPLE_POST_STAGES, sizeof (vp_api_io_stage_t));
/**
* Fill the PRE stage list
* - name and type are debug infos only
* - cfg is the pointer passed as "cfg" in all the stages calls
* - funcs is the pointer to the stage functions
*/
stages_index = 0;
vp_os_memset (&precfg, 0, sizeof (pre_stage_cfg_t));
strncpy (precfg.outputName, encodedFileName, 255);
example_pre_stages->stages_list[stages_index].name = "Encoded Dumper"; // Debug info
example_pre_stages->stages_list[stages_index].type = VP_API_FILTER_DECODER; // Debug info
example_pre_stages->stages_list[stages_index].cfg = &precfg;
example_pre_stages->stages_list[stages_index++].funcs = pre_stage_funcs;
example_pre_stages->length = stages_index;
/**
* Fill the POST stage list
* - name and type are debug infos only
* - cfg is the pointer passed as "cfg" in all the stages calls
* - funcs is the pointer to the stage functions
*/
stages_index = 0;
vp_os_memset (&dispCfg, 0, sizeof (display_stage_cfg_t));
dispCfg.bpp = bpp;
dispCfg.decoder_info = in_picture;
example_post_stages->stages_list[stages_index].name = "Decoded display"; // Debug info
example_post_stages->stages_list[stages_index].type = VP_API_OUTPUT_SDL; // Debug info
example_post_stages->stages_list[stages_index].cfg = &dispCfg;
example_post_stages->stages_list[stages_index++].funcs = display_stage_funcs;
example_post_stages->length = stages_index;
/**
* Fill thread params for the ardrone_tool video thread
* - in_pic / out_pic are reference to our in_picture / out_picture
* - pre/post stages lists are references to our stages lists
* - needSetPriority and priority are used to control the video thread priority
* -> if needSetPriority is set to 1, the thread will try to set its priority to "priority"
* -> if needSetPriority is set to 0, the thread will keep its default priority (best on PC)
*/
params->in_pic = in_picture;
params->out_pic = out_picture;
params->pre_processing_stages_list = example_pre_stages;
params->post_processing_stages_list = example_post_stages;
params->needSetPriority = 0;
params->priority = 0;
//set the tag detection
ENEMY_COLORS_TYPE enemyColors = ARDRONE_DETECTION_COLOR_ORANGE_BLUE;
ARDRONE_TOOL_CONFIGURATION_ADDEVENT (enemy_colors, &enemyColors, NULL);
CAD_TYPE detectType = CAD_TYPE_MULTIPLE_DETECTION_MODE;
ARDRONE_TOOL_CONFIGURATION_ADDEVENT (detect_type, &detectType, NULL);
uint32_t detectH = TAG_TYPE_MASK (TAG_TYPE_SHELL_TAG_V2);
ARDRONE_TOOL_CONFIGURATION_ADDEVENT (detections_select_h, &detectH, NULL);
/**
* Start the video thread (and the video recorder thread for AR.Drone 2)
*/
START_THREAD(video_stage, params);
START_THREAD (video_recorder, NULL);
video_stage_init();
if (2 <= ARDRONE_VERSION ())
{
START_THREAD (video_recorder, NULL);
video_recorder_init ();
}
video_stage_resume_thread();
//King of the Hill threads
START_THREAD(wiimote_logic, NULL);
START_THREAD(drone_logic, NULL);
START_THREAD(score_logic, NULL);
return C_OK;
}
C_RESULT ardrone_academy_navdata_process( const navdata_unpacked_t* const pnd )
{
static bool_t prevCameraIsReady = FALSE;
static bool_t prevDroneUsbRecording = FALSE;
if (C_OK == TRYLOCK_ND_MUTEX ())
{
input_state_t* input_state = ardrone_tool_input_get_state();
bool_t recordIsReady = ! ardrone_academy_navdata_get_record_ready();
bool_t recordIsInProgress = ardrone_academy_navdata_get_record_state();
// Save ARDrone State
navdata_state.lastState = pnd->ardrone_state;
// Save remaining USB time
navdata_state.usbFreeTime = pnd->navdata_hdvideo_stream.usbkey_remaining_time;
bool_t currentDroneUsbRecording = (NAVDATA_HDVIDEO_USBKEY_IS_RECORDING == (NAVDATA_HDVIDEO_USBKEY_IS_RECORDING & pnd->navdata_hdvideo_stream.hdvideo_state));
// Check for record stop from drone
if (navdata_state.usbRecordInProgress)
{
if (TRUE == prevDroneUsbRecording &&
FALSE == currentDroneUsbRecording)
{
navdata_state.droneStoppedUsbRecording = TRUE;
navdata_state.record_state = RECORD_STATE_NEEDED;
}
}
prevDroneUsbRecording = currentDroneUsbRecording;
ardrone_academy_check_take_off_timeout ();
// Take off and Emergency management
if(navdata_state.takeoff_state == TAKEOFF_STATE_NEEDED)
{
if(pnd->ardrone_state & ARDRONE_EMERGENCY_MASK)
{
navdata_state.needSetEmergency = TRUE;
}
else
{
if(recordIsInProgress)
{
if(!(pnd->ardrone_state & ARDRONE_USER_FEEDBACK_START))
ardrone_tool_set_ui_pad_start(1);
else
ardrone_tool_set_ui_pad_start(0);
navdata_state.takeoff_state = TAKEOFF_STATE_IDLE;
}
else
{
navdata_state.takeoff_state = TAKEOFF_STATE_WAIT_USERBOX;
ardrone_academy_navdata_userbox_switch();
}
}
}
if(navdata_state.needSetEmergency)
{
ardrone_tool_set_ui_pad_select(1);
navdata_state.needSetEmergency = FALSE;
}
if(pnd->ardrone_state & ARDRONE_EMERGENCY_MASK)
{
if(!navdata_state.wasEmergency && (input_state->user_input & (1 << ARDRONE_UI_BIT_SELECT)))
{
ardrone_tool_set_ui_pad_select(0);
}
navdata_state.isEmergency = TRUE;
navdata_state.isTakeOff = FALSE;
if(!navdata_state.internalRecordInProgress && !navdata_state.usbRecordInProgress && (navdata_state.userbox_state == USERBOX_STATE_STARTED))
{
PRINT("Emergency !! Stopping userbox...\n");
ardrone_academy_navdata_userbox_switch();
}
ardrone_tool_input_start_reset();
}
else // Not emergency landing
{
if(navdata_state.wasEmergency && (input_state->user_input & (1 << ARDRONE_UI_BIT_SELECT)))
{
ardrone_tool_set_ui_pad_select(0);
}
if(!(pnd->ardrone_state & ARDRONE_TIMER_ELAPSED))
navdata_state.isEmergency = FALSE;
if(input_state->user_input & (1 << ARDRONE_UI_BIT_START))
{
navdata_state.isTakeOff = (pnd->ardrone_state & ARDRONE_USER_FEEDBACK_START) ? TRUE : FALSE;
}
else
{
navdata_state.isTakeOff = FALSE;
}
}
navdata_state.wasEmergency = (pnd->ardrone_state & ARDRONE_EMERGENCY_MASK) ? TRUE : FALSE;
// Video record management
bool_t usbRecordEnable = FALSE;
if(navdata_state.record_state == RECORD_STATE_NEEDED &&
TAKEOFF_STATE_IDLE == navdata_state.takeoff_state)
{
bool_t continueRecord = TRUE;
bool_t nextInternalRecordState = FALSE;
if (IS_LEAST_ARDRONE2)
{
static codec_type_t oldCodec;
codec_type_t cancelCodec;
if (recordIsReady && ! navdata_state.usbRecordInProgress && !navdata_state.internalRecordInProgress) // We want to enable recording
{
if ((ARDRONE_USB_MASK == (pnd->ardrone_state & ARDRONE_USB_MASK)) &&
(0 < pnd->navdata_hdvideo_stream.usbkey_remaining_time) &&
(TRUE == ardrone_control_config.video_on_usb))
{
usbRecordEnable = TRUE;
}
// Set the "non-record codec" to the codec defined as the application default
oldCodec = ardrone_application_default_config.video_codec;
cancelCodec = oldCodec;
ardrone_control_config.video_codec = (TRUE == usbRecordEnable) ? usbRecordCodec : deviceWifiRecordCodec;
PRINT ("Set codec %d -> %d\n", oldCodec, ardrone_control_config.video_codec);
nextInternalRecordState = TRUE;
}
else // We want to disable recording
{
cancelCodec = ardrone_control_config.video_codec;
ardrone_control_config.video_codec = oldCodec;
PRINT ("Reset codec %d -> %d\n", cancelCodec, oldCodec);
}
bool_t addEventSucceeded = ARDRONE_TOOL_CONFIGURATION_ADDEVENT (video_codec, &ardrone_control_config.video_codec, NULL);
if (FALSE == addEventSucceeded)
{
PRINT ("Unable to send codec switch ... retry later\n");
ardrone_control_config.video_codec = cancelCodec;
continueRecord = FALSE;
}
}
else if (IS_ARDRONE1)
{
nextInternalRecordState = !recordIsInProgress;
}
if (TRUE == continueRecord)
{
navdata_state.internalRecordInProgress = nextInternalRecordState;
switch (navdata_state.userbox_state)
{
case USERBOX_STATE_STOPPED:
navdata_state.record_state = RECORD_STATE_WAIT_USERBOX;
navdata_state.usbRecordInProgress = usbRecordEnable;
ardrone_academy_navdata_userbox_switch();
break;
case USERBOX_STATE_STARTED:
if(navdata_state.isTakeOff)
{
if(! recordIsReady)
{
PRINT("Userbox is started and record is started => Stop record\n");
ardrone_academy_navdata_recorder_enable(FALSE, navdata_state.userbox_time);
navdata_state.usbRecordInProgress = FALSE;
}
else
{
PRINT("Userbox is started and record is stopped => Start record\n");
if (FALSE == usbRecordEnable)
{
// Only activate the local recorder if we don't record on USB
ardrone_academy_navdata_recorder_enable(TRUE, navdata_state.userbox_time);
navdata_state.usbRecordInProgress = FALSE;
}
else
{
navdata_state.usbRecordInProgress = TRUE;
}
}
navdata_state.record_state = RECORD_STATE_IDLE;
}
else
{
navdata_state.record_state = RECORD_STATE_WAIT_USERBOX;
ardrone_academy_navdata_userbox_switch();
}
break;
case USERBOX_STATE_STARTING:
navdata_state.usbRecordInProgress = usbRecordEnable;
navdata_state.record_state = RECORD_STATE_WAIT_USERBOX;
break;
case USERBOX_STATE_STOPPING:
navdata_state.usbRecordInProgress = usbRecordEnable;
// Should never be here
PRINT ("Don't know what to do for USERBOX_STATE_STOPPING\n");
VP_OS_ASSERT (0 == 1); // Debug handler
break;
}
}
}
// Screenshot management
prevCameraIsReady = navdata_state.cameraIsReady;
navdata_state.cameraIsReady = (pnd->ardrone_state & ARDRONE_CAMERA_MASK) ? TRUE : FALSE;
if (TRUE == navdata_state.cameraIsReady && FALSE == prevCameraIsReady)
{
academy_download_resume ();
}
if((navdata_state.screenshot_state == SCREENSHOT_STATE_NEEDED) && navdata_state.cameraIsReady)
{
static char param[ARDRONE_DATE_MAXSIZE + 64];
static char date[ARDRONE_DATE_MAXSIZE];
time_t t = time(NULL);
ardrone_time2date(t, ARDRONE_FILE_DATE_FORMAT, date);
navdata_state.screenshot_state = SCREENSHOT_STATE_INPROGRESS;
sprintf(param, "%d,%d,%d,%s", USERBOX_CMD_SCREENSHOT, 0, 0, date);
ARDRONE_TOOL_CONFIGURATION_ADDEVENT(userbox_cmd, param, ardrone_academy_navdata_screenshot_cb);
}
// USB management
navdata_state.usbIsReady = (pnd->ardrone_state & ARDRONE_USB_MASK) ? TRUE : FALSE;
UNLOCK_ND_MUTEX ();
}
return C_OK;
}
void parrot_ardrone_ctrl_set_yaw_max_angle(float32_t max_angle)
{
ARDRONE_TOOL_CONFIGURATION_ADDEVENT(control_yaw, &max_angle, NULL);
}
static void switchToApplication(void)
{
ARDRONE_TOOL_CONFIGURATION_ADDEVENT (application_id , app_id, NULL);
}
static void switchToUser(void)
{
ARDRONE_TOOL_CONFIGURATION_ADDEVENT (profile_id, usr_id, NULL);
}
static void switchToSession(void)
{
ARDRONE_TOOL_CONFIGURATION_ADDEVENT (session_id, ses_id, NULL);
}
C_RESULT ardrone_general_navdata_process( const navdata_unpacked_t* const pnd )
{
navdata_mode_t current_navdata_state = NAVDATA_BOOTSTRAP;
/* Makes sure the navdata stream will be resumed if the drone is disconnected and reconnected.
* Allows changing the drone battery during debugging sessions. */
if( ardrone_get_mask_from_state(pnd->ardrone_state, ARDRONE_NAVDATA_BOOTSTRAP) )
{
current_navdata_state = NAVDATA_BOOTSTRAP;
}
else
{
current_navdata_state = (ardrone_get_mask_from_state(pnd->ardrone_state, ARDRONE_NAVDATA_DEMO_MASK)) ? NAVDATA_DEMO : NAVDATA_FULL ;
}
if (current_navdata_state == NAVDATA_BOOTSTRAP && configState == MULTICONFIG_IDLE && navdataState == NAVDATA_REQUEST_IDLE)
{
navdataState = NAVDATA_REQUEST_NEEDED;
}
/* Multiconfig settings */
int configIndex, userNeedInit, appNeedInit;
userNeedInit = 0; appNeedInit = 0;
switch (configState)
{
case MULTICONFIG_GOT_DRONE_VERSION:
PRINTDBG ("Checking drone version ...\n");
// Check if drone version is >= 1.6
if (versionSupportsMulticonfiguration (ardrone_control_config.num_version_soft))
{
PRINTDBG ("Drone supports multiconfig\n");
configState = MULTICONFIG_IN_PROGRESS_LIST;
ARDRONE_TOOL_CUSTOM_CONFIGURATION_GET (configurationCallback);
droneSupportsMulticonfig = 1;
}
else
{
PRINTDBG ("Drone does not support multiconfig\n");
// Drone does not support multiconfig ... don't call init functions because we don't want to mess up things in default config
configState = MULTICONFIG_REQUEST_NAVDATA;
}
break;
case MULTICONFIG_GOT_IDS_LIST:
// At this point, we're sure that the AR.Drone supports multiconfiguration, so we'll wheck if our ids exists, and send them.
PRINTDBG ("Got AR.Drone ID list. Switch->%d,%d,%d. ND->%d\n", sesSwitch, usrSwitch, appSwitch, navdataNeeded);
if (1 == sesSwitch)
{
switchToSession(); // Go to session ...
}
if (1 == appSwitch)
{
if (0 != strcmp(ardrone_control_config_default.application_id, app_id)) // Check for application only if we're not asking for the default one
{
appNeedInit = 1;
for (configIndex = 0; configIndex < available_configurations[CAT_APPLI].nb_configurations; configIndex++) // Check all existing app_ids
{
PRINTDBG ("Checking application %s (desc : %s)\n", available_configurations[CAT_APPLI].list[configIndex].id,
available_configurations[CAT_APPLI].list[configIndex].description);
if (0 == strcmp(available_configurations[CAT_APPLI].list[configIndex].id, app_id))
{
PRINTDBG ("Found our application ... should not init\n");
appNeedInit = 0;
break;
}
}
switchToApplication();
}
else
{
PRINTDBG ("We're requesting default application (%s), do nothing.\n", app_id);
}
}
if (1 == usrSwitch)
{
if (0 != strcmp(ardrone_control_config_default.profile_id, usr_id)) // Check for user only if we're not asking for the default one
{
userNeedInit = 1;
for (configIndex = 0; configIndex < available_configurations[CAT_USER].nb_configurations; configIndex++) // Check all existing user_ids
{
PRINTDBG ("Checking user %s (desc : %s)\n", available_configurations[CAT_USER].list[configIndex].id,
available_configurations[CAT_USER].list[configIndex].description);
if (0 == strcmp(available_configurations[CAT_USER].list[configIndex].id, usr_id))
{
PRINTDBG ("Found our user ... should not init\n");
userNeedInit = 0;
break;
}
}
switchToUser();
}
else
{
PRINTDBG ("We're requesting default user (%s), do nothing.\n", usr_id);
}
}
if (1 == appNeedInit)
{
// Send application defined default values
ardrone_tool_send_application_default();
PRINTDBG ("Creating app. profile on AR.Drone\n");
ARDRONE_TOOL_CONFIGURATION_ADDEVENT (application_desc, app_name, NULL);
}
if (1 == userNeedInit)
{
// Send user defined default values
ardrone_tool_send_user_default();
PRINTDBG ("Creating usr. profile on AR.Drone\n");
ARDRONE_TOOL_CONFIGURATION_ADDEVENT (profile_desc, usr_name, NULL);
}
if (1 == sesSwitch)
{
if (0 != strcmp(ardrone_control_config_default.session_id, ses_id)) // Send session info only if session is not the default one
{
ARDRONE_TOOL_CONFIGURATION_ADDEVENT (session_desc, ses_name, NULL);
// Send session specific default values
ardrone_tool_send_session_default();
}
else
{
PRINTDBG ("We're requesting default session (%s), do nothing.\n", ses_id);
}
}
configState = MULTICONFIG_IN_PROGRESS_IDS;
ARDRONE_TOOL_CONFIGURATION_GET (configurationCallback);
case MULTICONFIG_GOT_CURRENT_IDS:
if (0 != strcmp(ardrone_control_config.session_id, ses_id) ||
0 != strcmp(ardrone_control_config.profile_id, usr_id) ||
0 != strcmp(ardrone_control_config.application_id, app_id))
{
configState = MULTICONFIG_GOT_DRONE_VERSION; // We failed at setting up the application ids ... restart (but assume that drone supports multiconfig as we already checked)
}
else if (1 == navdataNeeded)
{
configState = MULTICONFIG_REQUEST_NAVDATA;
}
else
{
configState = MULTICONFIG_IDLE;
}
break;
case MULTICONFIG_NEEDED:
PRINTDBG ("Need to check multiconfig ... request config file\n");
// Get config file for reset
configState = MULTICONFIG_IN_PROGRESS_VERSION;
ARDRONE_TOOL_CONFIGURATION_GET (configurationCallback);
break;
case MULTICONFIG_REQUEST_NAVDATA:
PRINTDBG ("Send application navdata demo/options\n");
// Send application navdata demo/options to start navdatas from AR.Drone
ARDRONE_TOOL_CONFIGURATION_ADDEVENT (navdata_demo, &ardrone_application_default_config.navdata_demo, NULL);
if (TRUE == ardrone_application_default_config.navdata_demo)
{ // Send navdata options only for navdata demo mode
ARDRONE_TOOL_CONFIGURATION_ADDEVENT (navdata_options, &ardrone_application_default_config.navdata_options, NULL);
}
configState = MULTICONFIG_IDLE;
break;
case MULTICONFIG_IDLE:
case MULTICONFIG_IN_PROGRESS_LIST:
case MULTICONFIG_IN_PROGRESS_VERSION:
case MULTICONFIG_IN_PROGRESS_IDS:
default:
break;
}
/* Navdata request settings */
switch (navdataState)
{
case NAVDATA_REQUEST_NEEDED:
PRINTDBG ("Resetting navdatas to %s\n", (0 == ardrone_application_default_config.navdata_demo) ? "full" : "demo");
navdataState = NAVDATA_REQUEST_IN_PROGRESS;
switchToSession(); // Resend session id when reconnecting.
ARDRONE_TOOL_CONFIGURATION_ADDEVENT(navdata_demo, &ardrone_application_default_config.navdata_demo, NULL);
if (TRUE == ardrone_application_default_config.navdata_demo)
{ // Send navdata options only for navdata demo mode
ARDRONE_TOOL_CONFIGURATION_ADDEVENT (navdata_options, &ardrone_application_default_config.navdata_options, navdataCallback);
}
break;
case NAVDATA_REQUEST_IN_PROGRESS:
case NAVDATA_REQUEST_IDLE:
default:
break;
}
return C_OK;
}
void parrot_ardrone_ctrl_set_tilt_max_angle(float32_t max_angle)
{
ARDRONE_TOOL_CONFIGURATION_ADDEVENT(euler_angle_max, &max_angle, NULL);
}
