C_RESULT
vp_stages_frame_pipe_receiver_transform(vp_stages_frame_pipe_config_t *cfg, vp_api_io_data_t *in, vp_api_io_data_t *out)
{
vp_os_mutex_lock(&out->lock);
if(out->status == VP_API_STATUS_INIT)
{
out->numBuffers = 1;
out->indexBuffer = 0;
out->status = VP_API_STATUS_PROCESSING;
}
if( out->status == VP_API_STATUS_PROCESSING )
{
vp_os_mutex_lock(&(cfg->pipe_mut));
{
if (cfg->pipe_state != SENDER_ERROR)
{
if (cfg->pipe_state == FETCH_PAUSE)
{
// a frame has alerady been fetched by a fetch stage
cfg->pipe_state = PAUSE;
}
else
{
// no fetch stage was executed, ask for a frame transfer
cfg->pipe_state = WAIT_RECEPTION;
vp_os_cond_wait (&(cfg->buffer_sent));
}
if (cfg->mode == FAST && cfg->pipe_state != SENDER_ERROR)
{
// at this point a new frame is available in the copy buffer
// swap the two output buffer to make new picture available in outPicture
vp_api_picture_t temp = cfg->outPicture;
cfg->outPicture = cfg->copyPicture;
cfg->copyPicture = temp;
// trigger a new frame transfert
cfg->pipe_state = FETCH;
}
// at this point, we are sure that cfg->outPicture was initialized by the sender
out->buffers = &(cfg->outPicture.raw);
out->size = cfg->frame_size;
}
}
vp_os_mutex_unlock(&(cfg->pipe_mut));
if (cfg->callback != NULL)
cfg->callback();
}
vp_os_mutex_unlock(&out->lock);
return C_OK;
}
DEFINE_THREAD_ROUTINE(score_logic, data){
//NOTE: the drone and the enemy have a certain amount of "life".
//Every time the enemy hit the drone, the drone life decrease by one
//Every time the drone hit the enemy, the enemy life decrease by one
//If the drone find tot hills, the drone wins
//If the enemy "kill" the drone, the enemy wins
//If time runs out and the drone find at least one hill, the drone wins
while(game_active){
//This happen if the enemy hits the drone
vp_os_mutex_lock(&drone_score_mutex);
if(drone_lose_score){
if(drone_score > 0){
drone_score--;
vp_os_mutex_lock(&drone_wound_mutex);
drone_wounded = 1;
vp_os_mutex_unlock(&drone_wound_mutex);
} else{
//TODO: else the drone is dead, game over!
}
drone_lose_score = 0;
}
vp_os_mutex_unlock(&drone_score_mutex);
//This happen if the drone hits the enemy
vp_os_mutex_lock(&enemy_score_mutex);
if(enemy_lose_score){
if(enemy_score > 0){
enemy_score--;
//TODO make the enemy aware that he's being hit
} else {
//TODO: enemy is dead!!
}
enemy_lose_score = 0;
}
vp_os_mutex_unlock(&enemy_score_mutex);
//This happen if the drone find a hill
//when the hill points reach a certain amount, the drone win!!
vp_os_mutex_lock(&drone_score_mutex);
if(drone_add_score){
drone_hill_score++;
drone_add_score = 0;
}
if(drone_hill_score > 5){
//TODO game over, the drone wins!!
}
vp_os_mutex_unlock(&drone_score_mutex);
}
return C_OK;
}
C_RESULT overlay_stage_transform(vlib_stage_decoding_config_t *cfg, vp_api_io_data_t *in, vp_api_io_data_t *out)
{
vp_os_mutex_lock( &out->lock );
if(out->status == VP_API_STATUS_INIT)
{
out->status = VP_API_STATUS_PROCESSING;
out->numBuffers = 1;
out->indexBuffer = 0;
out->lineSize = NULL;
}
if( in->status == VP_API_STATUS_ENDED )
{
out->status = in->status;
}
if(in->status == VP_API_STATUS_STILL_RUNNING)
{
out->status = VP_API_STATUS_PROCESSING;
}
if(out->status == VP_API_STATUS_PROCESSING )
{
vp_os_mutex_lock( &overlay_video_config.mutex );
pixbuf_data = (uint8_t*)in->buffers[0];
if (overlay_video_config.showFPS) drawFPS(pixbuf_data);
drawBattery(pixbuf_data);
drawFlyMode(pixbuf_data);
drawRecording(pixbuf_data);
// if (overlay_video_config.showInternalTrackingPoints) drawInternaltrackingPoints(pixbuf_data);
if (overlay_video_config.showExternalTrackingPoints) drawExternaltrackingPoints(pixbuf_data);
out->numBuffers = in->numBuffers;
out->indexBuffer = in->indexBuffer;
out->size = in->size;
out->status = in->status;
out->buffers = in->buffers;
vp_os_mutex_unlock( &overlay_video_config.mutex );
}
vp_os_mutex_unlock( &out->lock );
return C_OK;
}
void videoServer_endBroadcaster() {
/* Signal broadcaster thread */
vp_os_mutex_lock(&frameBufferMutex);
availVideoBuffer = NULL;
vp_os_cond_signal(&frameBufferCond);
vp_os_mutex_unlock(&frameBufferMutex);
}
// 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;
}
C_RESULT output_rendering_device_stage_transform( void *cfg, vp_api_io_data_t *in, vp_api_io_data_t *out)
{
vlib_stage_decoding_config_t* vec = (vlib_stage_decoding_config_t*)cfg;
vp_os_mutex_lock(&video_update_lock);
/* Get a reference to the last decoded picture */
pixbuf_data = (uint8_t*)in->buffers[0];
/** ======= INSERT USER CODE HERE ========== **/
// Send the decoded video frame to the DirectX renderer.
// This is an example; do here whatever you want to do
// with the decoded frame.
/* Send the actual video resolution to the rendering module */
//D3DChangeTextureSize(vec->controller.width,vec->controller.height);
/* Send video picture to the rendering module */
//D3DChangeTexture(pixbuf_data);
/** ======= INSERT USER CODE HERE ========== **/
bot_ardrone_ardronelib_process_frame(pixbuf_data, vec->controller.width, vec->controller.height);
vp_os_mutex_unlock(&video_update_lock);
return (VP_SUCCESS);
}
C_RESULT academy_connect(const char *username, const char *password, academy_callback callback)
{
C_RESULT result = C_FAIL;
if(academy_upload_started && (academy_upload.state.state == ACADEMY_STATE_NONE))
{
if(!academy_upload.connected)
{
_ftp_t *academy_ftp = NULL;
_ftp_status status;
strcpy(academy_upload.user.username, username ? username : "");
strcpy(academy_upload.user.password, password ? password : "");
if(callback != NULL)
academy_upload.callback = callback;
academy_ftp = ftpConnectFromName(ACADEMY_SERVERNAME, ACADEMY_PORT, academy_upload.user.username, academy_upload.user.password, &status);
if(academy_ftp != NULL)
{
ftpClose(&academy_ftp);
vp_os_mutex_lock(&academy_upload.mutex);
academy_upload.connected = TRUE;
vp_os_cond_signal(&academy_upload.cond);
vp_os_mutex_unlock(&academy_upload.mutex);
result = C_OK;
}
}
else
result = C_OK;
}
return result;
}
static inline C_RESULT ardrone_navdata_process( const navdata_unpacked_t* const navdata )
{
if( writeToFile )
{
if( navdata_file == NULL )
{
ardrone_navdata_file_init(root_dir);
PRINT("Saving in %s file\n", root_dir);
}
ardrone_navdata_file_process( navdata );
}
else
{
if(navdata_file != NULL)
ardrone_navdata_file_release();
}
vp_os_mutex_lock( &inst_nav_mutex);
/* inst_nav.ardrone_state = navdata->ardrone_state;
inst_nav.vision_defined = navdata->vision_defined;
vp_os_memcpy(&inst_nav.navdata_demo, &navdata->navdata_demo, sizeof(navdata_demo_t));
vp_os_memcpy(&inst_nav.navdata_vision_detect, &navdata->navdata_vision_detect, sizeof(navdata_vision_detect_t));
*/
vp_os_memcpy(&inst_nav, navdata, sizeof(navdata_unpacked_t));
vp_os_mutex_unlock( &inst_nav_mutex );
return C_OK;
}
//add Alex
bool MagnitoCalibCallback(std_srvs::Empty::Request &request, std_srvs::Empty::Response &response)
{
vp_os_mutex_lock(&twist_lock);
ardrone_at_set_calibration(0);
vp_os_mutex_unlock(&twist_lock);
fprintf(stderr, "\nMagnito calibration.\n");
}
C_RESULT output_gtk_stage_transform( void *cfg, vp_api_io_data_t *in, vp_api_io_data_t *out)
{
vp_os_mutex_lock(&video_update_lock);
/* Get a reference to the last decoded picture */
pixbuf_data = (uint8_t*)in->buffers[0];
n = write(newsockfd, pixbuf_data , IMG_BUFSIZE);
//vp_stages_YUV420P_to_RGB565_QCIF_to_QVGA(vp_stages_yuv2rgb_config_t *cfg, vp_api_picture_t *picture, uint8_t *dst, uint32_t dst_rbytes)
//printf("%s\n", pixbuf_data);
//printf("%d sizeof *\n", sizeof(*pixbuf_data));
//printf("%d sizeof\n", sizeof(pixbuf_data));
//printf("%d sizeofuint\n", sizeof(uint8_t*));
//printf("2 sockfd, %d\n", sockfd);
//n = write(newsockfd, pixbuf_data , sizeof(pixbuf_data));
//printf("%d --- %d\n", n, errno);
vp_os_mutex_unlock(&video_update_lock);
return (SUCCESS);
}
DEFINE_THREAD_ROUTINE( comm_control, data )
{
int seq_no = 0,prev_start = 0;
control_data_t l_control;
PRINT( "Initilizing Thread 1\n" );
while( end_all_threads == 0 )
{
vp_os_delay(10);
//PRINT("E:%d,S:%d\n\n",end_all_threads,control_data.start );
vp_os_mutex_lock( &control_data_lock ); //Taking Control Mutex
l_control = control_data;
vp_os_mutex_unlock( &control_data_lock );
if( prev_start == 0 && l_control.start == 1 ) //To Start Drone
{
ardrone_tool_set_ui_pad_start( 1 );
prev_start = 1;
}
else if( prev_start == 1 && l_control.start == 0 ) //To Stop the Drone
{
ardrone_tool_set_ui_pad_start( 0 );
prev_start = 0;
}
//PRINT("YAW : %f\n\n",l_control.yaw);
ardrone_at_set_progress_cmd( ++seq_no,l_control.roll,l_control.pitch,l_control.gaz,l_control.yaw); //command to make drone move.
//ardrone_at_set_progress_cmd( ++seq_no,0,0,0,-1);
}
PRINT( "Communication Thread Ending\n" );
return C_OK;
}
//extract navdata unpacked from navdata slot
inline void extract_navdata_slot(volatile navdata_slot_t *navdata_slot, navdata_unpacked_t *navdata) {
if (CANREAD(navdata_slot)) {
vp_os_mutex_lock(&navdata_slot_mutex);
NAVDATA_READ(*navdata, navdata_slot);
vp_os_mutex_unlock(&navdata_slot_mutex);
}
}
//fill navdata slot with navdata unpacked
inline void fill_navdata_slot(const navdata_unpacked_t * const navdata, volatile navdata_slot_t* navdata_slot) {
if (CANWRITE(navdata_slot)) {
vp_os_mutex_lock(&navdata_slot_mutex);
NAVDATA_WRITE(*navdata, navdata_slot);
vp_os_mutex_unlock(&navdata_slot_mutex);
}
}
void video_render(long tick, int width, int height)
{
opengl_video_stage_config_t *config;
static int num = 0;
screen_width = width;
screen_height = height;
//glViewport(0, 0, width, height);
glBindTexture(GL_TEXTURE_2D, texture);
config = opengl_video_stage_get();
if (config->num_picture_decoded != num) {
/* a new frame is available, update texture */
if ((config != NULL) && (config->data != NULL) && (config->num_picture_decoded > 0))
{
vp_os_mutex_lock( &config->mutex );
glTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, VIDEO_WIDTH, VIDEO_HEIGHT,
GL_RGB, GL_UNSIGNED_SHORT_5_6_5, config->data );
num = config->num_picture_decoded;
vp_os_mutex_unlock( &config->mutex );
}
}
/* and draw it on the screen */
__android_log_print( ANDROID_LOG_INFO, "ARDrone", "screen_width=%d, screen_height=%d\n", screen_width, screen_height );
glDrawTexiOES(0, 0, 0, screen_width, screen_height);
}
static C_RESULT vp_com_serial_write_sync(vp_com_serial_config_t* config, vp_com_socket_t* socket)
{
uint32_t i;
uint8_t c;
vp_os_mutex_lock(&write_sync_mutex);
if(!config->sync_done)
{
for(i = 0 ; i < sizeof(VP_COM_SYNC_STRING) ; i++)
{
c = (VP_COM_SYNC_STRING)[i];
if(-1 == write((int)socket->priv, &c, sizeof(int8_t)))
{
return (FAIL);
}
}
}
config->sync_done = 1;
vp_os_mutex_unlock(&write_sync_mutex);
return (SUCCESS);
}
static C_RESULT vp_com_serial_wait_sync(vp_com_serial_config_t* config, vp_com_socket_t* socket)
{
uint32_t nb;
uint8_t c;
vp_os_mutex_lock(&wait_sync_mutex);
if(!config->sync_done)
{
nb = 0;
do
{
if(-1 == read((int)socket->priv, &c, sizeof(int8_t)))
return (FAIL);
if(c == (VP_COM_SYNC_STRING)[nb])
{
nb++;
}
else
{
nb = 0;
}
}
while(nb != sizeof(VP_COM_SYNC_STRING));
config->sync_done = 1;
}
vp_os_mutex_unlock(&wait_sync_mutex);
return (SUCCESS);
}
C_RESULT
vp_stages_input_com_stage_transform(vp_stages_input_com_config_t *cfg, vp_api_io_data_t *in, vp_api_io_data_t *out)
{
vp_os_mutex_lock(&out->lock);
if(out->status == VP_API_STATUS_INIT)
{
out->numBuffers = 1;
out->size = cfg->buffer_size;
out->buffers = (int8_t **) vp_os_malloc (sizeof(int8_t *)+out->size*sizeof(int8_t));
out->buffers[0] = (int8_t *)(out->buffers+1);
out->indexBuffer = 0;
// out->lineSize not used
out->status = VP_API_STATUS_PROCESSING;
}
if(out->status == VP_API_STATUS_PROCESSING && cfg->read != NULL)
{
out->size = cfg->buffer_size;
if(VP_FAILED(cfg->read(&cfg->socket_client, out->buffers[0], &out->size)))
out->status = VP_API_STATUS_ERROR;
}
vp_os_mutex_unlock(&out->lock);
return (VP_SUCCESS);
}
C_RESULT
vp_stages_output_com_stage_transform(vp_stages_output_com_config_t *cfg, vp_api_io_data_t *in, vp_api_io_data_t *out)
{
vp_os_mutex_lock(&out->lock);
if(out->status == VP_API_STATUS_INIT)
{
out->status = VP_API_STATUS_PROCESSING;
}
// \todo test
if(out->status != VP_API_STATUS_ERROR)
{
out->size = in->size;
if(in->size > 0 && cfg->write != NULL)
{
cfg->write(&cfg->socket_client, &in->buffers[in->indexBuffer][0], &out->size);
}
out->status = in->status;
}
// \todo test
vp_os_mutex_unlock(&out->lock);
return (VP_SUCCESS);
}
/*---------------------------------------------------------------------------------------------------------------------
Navdata handling function, which is called every time navdata are received
---------------------------------------------------------------------------------------------------------------------*/
inline C_RESULT demo_navdata_client_process( const navdata_unpacked_t* const navdata )
{
static int cpt=0;
int i;
const navdata_demo_t* const nd = &navdata->navdata_demo;
/** ======= INSERT USER CODE HERE ========== **/
ARWin32Demo_AcquireConsole();
if ((cpt++)==90) { system("cls"); cpt=0; }
ARWin32Demo_SetConsoleCursor(0,0); // Print at the top of the console
printf("=================================\n");
printf("Navdata for flight demonstrations\n");
printf("=================================\n");
printf("Control state : %s \n",ctrl_state_str(nd->ctrl_state));
printf("Battery level : %i/100 \n",nd->vbat_flying_percentage);
printf("Orientation : [Theta] %4.3f [Phi] %4.3f [Psi] %4.3f \n",nd->theta,nd->phi,nd->psi);
printf("Altitude : %i \n",nd->altitude);
printf("Speed : [vX] %4.3f [vY] %4.3f [vZ] %4.3f \n",nd->vx,nd->vy,nd->vz);
vp_os_mutex_lock(&qr_update_lock);
for(i = 0; i < qr_count; i++) {
printf("QR #%d: %s\n",i,string_buffers[i]);
}
vp_os_mutex_unlock(&qr_update_lock);
ARWin32Demo_ReleaseConsole();
/** ======= INSERT USER CODE HERE ========== **/
return C_OK;
}
C_RESULT mjpeg_stage_decoding_transform(mjpeg_stage_decoding_config_t *cfg, vp_api_io_data_t *in, vp_api_io_data_t *out)
{
bool_t got_image;
vp_os_mutex_lock( &out->lock );
if(out->status == VP_API_STATUS_INIT)
{
out->numBuffers = 1;
out->buffers = (int8_t**)&cfg->picture;
out->indexBuffer = 0;
out->lineSize = 0;
out->status = VP_API_STATUS_PROCESSING;
}
if( in->status == VP_API_STATUS_ENDED )
out->status = in->status;
// Several cases must be handled in this stage
// 1st: Input buffer is too small to decode a complete picture
// 2nd: Input buffer is big enough to decode 1 frame
// 3rd: Input buffer is so big we can decode more than 1 frame
if( out->status == VP_API_STATUS_PROCESSING )
{
// Reinit stream with new data
stream_config( &cfg->stream, in->size, in->buffers[in->indexBuffer] );
}
if(out->status == VP_API_STATUS_PROCESSING || out->status == VP_API_STATUS_STILL_RUNNING)
{
// If out->size == 1 it means picture is ready
out->size = 0;
out->status = VP_API_STATUS_PROCESSING;
mjpeg_decode( &cfg->mjpeg, cfg->picture, &cfg->stream, &got_image );
if( got_image )
{
// we got one picture (handle case 1)
out->size = 1;
PRINT( "%d picture decoded\n", cfg->mjpeg.num_frames );
// handle case 2 & 3
if( FAILED(stream_is_empty( &cfg->stream )) )
{
// Some data are still in stream
// Next time we run this stage we don't want this data to be lost
// So flag it!
out->status = VP_API_STATUS_STILL_RUNNING;
}
}
}
vp_os_mutex_unlock( &out->lock );
return C_OK;
}
C_RESULT
buffer_to_picture_transform(buffer_to_picture_config_t *cfg, vp_api_io_data_t *in, vp_api_io_data_t *out)
{
vp_os_mutex_lock(&out->lock);
if(out->status == VP_API_STATUS_INIT)
{
out->numBuffers = 1;
out->size = (ACQ_WIDTH*ACQ_HEIGHT*3)/2;
out->buffers = (int8_t **) cfg->picture;
out->indexBuffer = 0;
out->status = VP_API_STATUS_PROCESSING;
}
if(out->status == VP_API_STATUS_ENDED)
{
}
if(out->status == VP_API_STATUS_PROCESSING)
{
vp_os_memcpy( cfg->picture->y_buf, in->buffers[0], ACQ_WIDTH*ACQ_HEIGHT );
vp_os_memcpy( cfg->picture->cb_buf, in->buffers[0] + ACQ_WIDTH*ACQ_HEIGHT, ACQ_WIDTH*ACQ_HEIGHT/4 );
vp_os_memcpy( cfg->picture->cr_buf, in->buffers[0] + ACQ_WIDTH*ACQ_HEIGHT + ACQ_WIDTH*ACQ_HEIGHT/4, ACQ_WIDTH*ACQ_HEIGHT/4 );
}
out->status = in->status;
vp_os_mutex_unlock(&out->lock);
return (SUCCESS);
}
void ardrone_at_set_vicon_data(struct timeval time, int32_t frame_number, float32_t latency, vector31_t global_translation, vector31_t global_rotation_euler)
{
float_or_int_t _latency, _global_translation_x, _global_translation_y, _global_translation_z, _global_rotation_euler_x, _global_rotation_euler_y,
_global_rotation_euler_z;
if (!at_init)
return;
_latency.f = latency;
_global_translation_x.f = global_translation.x;
_global_translation_y.f = global_translation.y;
_global_translation_z.f = global_translation.z;
_global_rotation_euler_x.f = global_rotation_euler.x;
_global_rotation_euler_y.f = global_rotation_euler.y;
_global_rotation_euler_z.f = global_rotation_euler.z;
vp_os_mutex_lock(&at_mutex);
ATcodec_Queue_Message_valist( ids.AT_MSG_ATCMD_VICON_EXE,
++nb_sequence,
(int)time.tv_sec,
(int)time.tv_usec,
(int)frame_number,
_latency.i,
_global_translation_x.i,
_global_translation_y.i,
_global_translation_z.i,
_global_rotation_euler_x.i,
_global_rotation_euler_y.i,
_global_rotation_euler_z.i);
vp_os_mutex_unlock(&at_mutex);
}
C_RESULT
picture_to_buffer_transform(buffer_to_picture_config_t *cfg, vp_api_io_data_t *in, vp_api_io_data_t *out)
{
vp_os_mutex_lock(&out->lock);
if(out->status == VP_API_STATUS_INIT)
{
out->numBuffers = 1;
out->size = (ACQ_WIDTH*ACQ_HEIGHT*3)/2;
out->buffers = (int8_t **) vp_os_malloc(out->size*sizeof(int8_t) + sizeof(int8_t*));
out->indexBuffer = 0;
out->status = VP_API_STATUS_PROCESSING;
out->buffers[0] = (int8_t *)(out->buffers+1);
}
if(out->status == VP_API_STATUS_PROCESSING)
{
if( in->size == 1 )
{
// got a picture
vp_os_memcpy( out->buffers[0], cfg->picture->y_buf, ACQ_WIDTH*ACQ_HEIGHT );
vp_os_memcpy( out->buffers[0] + ACQ_WIDTH*ACQ_HEIGHT, cfg->picture->cb_buf, ACQ_WIDTH*ACQ_HEIGHT/4);
vp_os_memcpy( out->buffers[0] + ACQ_WIDTH*ACQ_HEIGHT + ACQ_WIDTH*ACQ_HEIGHT/4, cfg->picture->cr_buf, ACQ_WIDTH*ACQ_HEIGHT/4);
}
}
// out->status = in->status;
vp_os_mutex_unlock(&out->lock);
return (SUCCESS);
}
static inline C_RESULT navdata_process( const navdata_unpacked_t* const navdata )
{
if( writeToFile )
{
if( navdata_file == NULL )
{
ardrone_navdata_file_data data;
data.filename = NULL;
data.print_header = NULL;
data.print = NULL;
ardrone_navdata_file_init(NULL);
}
ardrone_navdata_file_process( navdata );
}
else
{
if(navdata_file != NULL)
ardrone_navdata_file_release();
}
vp_os_mutex_lock( &inst_nav_mutex);
vp_os_memcpy(&inst_nav, navdata, sizeof(navdata_unpacked_t));
vp_os_mutex_unlock( &inst_nav_mutex );
return C_OK;
}
/**
* @param enable 1,with pitch,roll and 0,without pitch,roll.
* @param pitch Using floating value between -1 to +1.
* @param roll Using floating value between -1 to +1.
* @param gaz Using floating value between -1 to +1.
* @param yaw Using floating value between -1 to +1.
* @param magneto_psi floating value between -1 to +1.
* @param magneto_psi_accuracy floating value between -1 to +1
*
* @brief
*
* @DESCRIPTION
*
*******************************************************************/
void ardrone_at_set_progress_cmd_with_magneto(int32_t flag, float32_t phi, float32_t theta,float32_t gaz, float32_t yaw, float32_t magneto_psi,float32_t magneto_psi_accuracy) {
float_or_int_t _phi, _theta, _gaz, _yaw, _magneto_psi, _magneto_psi_accuracy;
if (!at_init)
return;
_phi.f = phi;
_theta.f = theta;
_gaz.f = gaz;
_yaw.f = yaw;
_magneto_psi.f = magneto_psi;
_magneto_psi_accuracy.f = magneto_psi_accuracy;
// Saving values to set them in navdata_file
nd_iphone_flag = flag;
nd_iphone_phi = phi;
nd_iphone_theta = theta;
nd_iphone_gaz = gaz;
nd_iphone_yaw = yaw;
nd_iphone_magneto_psi = magneto_psi;
nd_iphone_magneto_psi_accuracy = magneto_psi_accuracy;
//printf("Sent : psi_iphone = %.4f acc = %.4f\n",magneto_psi,magneto_psi_accuracy);
vp_os_mutex_lock(&at_mutex);
ATcodec_Queue_Message_valist(ids.AT_MSG_ATCMD_PCMD_MAG_EXE, ++nb_sequence,
flag, _phi.i, _theta.i, _gaz.i, _yaw.i, _magneto_psi.i, _magneto_psi_accuracy.i);
vp_os_mutex_unlock(&at_mutex);
}
void video_stage_resume_thread(void)
{
vp_os_mutex_lock(&video_stage_mutex);
vp_os_cond_signal(&video_stage_condition);
video_stage_in_pause = FALSE;
vp_os_mutex_unlock(&video_stage_mutex);
}
void
vp_os_thread_priority(THREAD_HANDLE handle, int32_t priority)
{
vp_os_mutex_lock(&thread_mutex);
pthread_data_t* freeSlot = findThread( handle );
vp_os_mutex_unlock(&thread_mutex);
if( freeSlot != NULL )
{
int rc, policy = SCHED_OTHER;
struct sched_param param;
vp_os_memset(¶m, 0, sizeof(param));
rc = pthread_getschedparam(handle, &policy, ¶m);
if( policy == SCHED_OTHER)
{
policy = SCHED_FIFO;
}
param.__sched_priority = 99-priority;
rc = pthread_setschedparam(handle, policy, ¶m);
}
}
bool flatTrimCallback(std_srvs::Empty::Request &request, std_srvs::Empty::Response &response)
{
vp_os_mutex_lock(&twist_lock);
ardrone_at_set_flat_trim();
vp_os_mutex_unlock(&twist_lock);
fprintf(stderr, "\nFlat Trim Set.\n");
}
C_RESULT navdata_custom_process(const navdata_unpacked_t * const pnd) {
vp_os_mutex_lock(&navdata_lock);
shared_raw_navdata = (navdata_unpacked_t*)pnd;
vp_os_mutex_unlock(&navdata_lock);
return C_OK;
}
C_RESULT ardrone_navdata_client_suspend(void)
{
vp_os_mutex_lock(&navdata_client_mutex);
navdata_thread_in_pause = TRUE;
vp_os_mutex_unlock(&navdata_client_mutex);
return C_OK;
}
