// 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); }