inline C_RESULT navdata_process( const navdata_unpacked_t* const navdata )
{
if (bIsInitialized == FALSE) {
LOGW(TAG, "Navdata is not initialized yet");
return C_OK;
}
vp_os_mutex_lock( &instance_navdata_mutex);
vp_os_memcpy(&inst_nav, navdata, sizeof(navdata_unpacked_t));
vp_os_mutex_unlock( &instance_navdata_mutex );
return C_OK;
}
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 teleopcmdVelCallback(const geometry_msgs::TwistStampedConstPtr &msg)
{
vp_os_mutex_lock(&twist_lock);
//printf("cmdVel\n");
const float maxHorizontalSpeed = 1; // use 0.1f for testing and 1 for the real thing
teleopcmd_vel.twist.linear.x = max(min(-msg->twist.linear.x, maxHorizontalSpeed), -maxHorizontalSpeed);
teleopcmd_vel.twist.linear.y = max(min(-msg->twist.linear.y, maxHorizontalSpeed), -maxHorizontalSpeed);
teleopcmd_vel.twist.linear.z = max(min(msg->twist.linear.z, 1), -1);
teleopcmd_vel.twist.angular.x = 0;
teleopcmd_vel.twist.angular.y = 0;
teleopcmd_vel.twist.angular.z = max(min(-msg->twist.angular.z, 1), -1);
vp_os_mutex_unlock(&twist_lock);
}
C_RESULT vp_com_shutdown(vp_com_t* vp_com)
{
VP_OS_ASSERT( vp_com != NULL );
vp_os_mutex_lock( &vp_com->mutex );
if(vp_com->ref_count > 0)
{
vp_com->ref_count--;
if(vp_com->ref_count == 0)
{
vp_os_mutex_unlock( &vp_com->mutex );
vp_os_mutex_destroy( &vp_com->mutex );
return VP_COM_SHUTDOWN();
}
}
vp_os_mutex_unlock(&vp_com->mutex);
return VP_COM_OK;
}
C_RESULT vp_com_timed_wait_for_server_up(uint32_t ms)
{
C_RESULT res = C_OK;
if( server_init_not_finished )
{
vp_os_mutex_lock(&server_initialisation_mutex);
res = vp_os_cond_timed_wait(&server_initialisation_wait, ms);
vp_os_mutex_unlock(&server_initialisation_mutex);
}
return res;
}
static ATCODEC_RET
valist_ATcodec_Queue_Message_valist_Tree(ATcodec_Tree_t *tree, AT_CODEC_MSG_ID id, va_list *va)
{
int32_t len;
ATCODEC_RET res;
ATcodec_Tree_Node_t *node = ATcodec_Tree_Node_get(tree, (int)id);
ATcodec_Message_Data_t *data = (ATcodec_Message_Data_t *)ATcodec_Buffer_getElement(&tree->leaves, node->data);
char *total_str = (char *)ATcodec_Buffer_getElement(&tree->strs, data->total_str);
ATcodec_Memory_t msg, fmt;
char buffer[INTERNAL_BUFFER_SIZE];
if(!atcodec_lib_init_ok)
return ATCODEC_FALSE;
vp_os_mutex_lock(&ATcodec_cond_mutex);
ATcodec_Memory_Init(&msg, (char*)&buffer[0], INTERNAL_BUFFER_SIZE, 1, NULL, NULL);
ATcodec_Memory_Init(&fmt, total_str, 0, 1, NULL, NULL);
if((res = vp_atcodec_sprintf_valist(&msg, &len, &fmt, va)) != ATCODEC_TRUE)
{
vp_os_mutex_unlock(&ATcodec_cond_mutex);
va_end(*va);
return res;
}
if(ATcodec_Message_len + len < INTERNAL_BUFFER_SIZE)
{
memcpy(&ATcodec_Message_Buffer[ATcodec_Message_len], &buffer[0], len);
ATcodec_Message_len += len;
}
//vp_os_cond_signal(&ATcodec_wait_cond);
vp_os_mutex_unlock(&ATcodec_cond_mutex);
va_end(*va);
return ATCODEC_TRUE;
}
PROTO_THREAD_ROUTINE(dct, params)
{
uint32_t i;
PRINT("DCT thread start\n");
while(1)
{
if( current_io_buffer == NULL )
{
vp_os_mutex_lock(&dct_start_mutex);
vp_os_cond_wait(&dct_start_cond);
vp_os_mutex_unlock(&dct_start_mutex);
}
if( current_io_buffer->dct_mode == DCT_MODE_FDCT )
{
for( i = 0; i < current_io_buffer->num_total_blocks; i++ )
{
fdct(current_io_buffer->input[i], current_io_buffer->output[i]);
}
}
else if( current_io_buffer->dct_mode == DCT_MODE_IDCT )
{
for( i = 0; i < current_io_buffer->num_total_blocks; i++ )
{
idct(current_io_buffer->input[i], current_io_buffer->output[i]);
}
}
vp_os_mutex_lock(&critical_section);
result_io_buffer = current_io_buffer;
current_io_buffer = NULL;
vp_os_mutex_unlock(&critical_section);
}
return 0;
}
void ardrone_at_set_anim( anim_mayday_t type, int32_t duration )
{
int32_t animtype = type;
if (!at_init)
return;
vp_os_mutex_lock(&at_mutex);
ATcodec_Queue_Message_valist( ids.AT_MSG_ATCMD_ANIM_EXE,
++nb_sequence,
animtype,
duration );
vp_os_mutex_unlock(&at_mutex);
}
void ardrone_at_set_ui_misc(int32_t m1, int32_t m2, int32_t m3, int32_t m4)
{
if (!at_init)
return;
vp_os_mutex_lock(&at_mutex);
ATcodec_Queue_Message_valist( ids.AT_MSG_ATCMD_MISC_EXE,
++nb_sequence,
m1,
m2,
m3,
m4 );
vp_os_mutex_unlock(&at_mutex);
}
C_RESULT navdata_get(navdata_unpacked_t *data)
{
C_RESULT result = C_FAIL;
if(data)
{
vp_os_mutex_lock( &inst_nav_mutex );
vp_os_memcpy(data, &inst_nav, sizeof(navdata_unpacked_t));
vp_os_mutex_unlock( &inst_nav_mutex );
result = C_OK;
}
return result;
}
C_RESULT draw_trackers_stage_open(vp_stages_draw_trackers_config_t *cfg, vp_api_io_data_t *in, vp_api_io_data_t *out) {
vp_os_mutex_lock(&draw_trackers_update);
int32_t i;
for (i = 0; i < NUM_MAX_SCREEN_POINTS; i++) {
cfg->locked[i] = C_OK;
}
PRINT("Draw trackers inited with %d trackers\n", cfg->num_points);
vp_os_mutex_unlock(&draw_trackers_update);
return (SUCCESS);
}
void cmdVelCallback(const geometry_msgs::TwistConstPtr &msg)
{
vp_os_mutex_lock(&twist_lock);
// Main 4DOF
cmd_vel.linear.x = max(min(-msg->linear.x, 1.0), -1.0);
cmd_vel.linear.y = max(min(-msg->linear.y, 1.0), -1.0);
cmd_vel.linear.z = max(min(msg->linear.z, 1.0), -1.0);
cmd_vel.angular.z = max(min(-msg->angular.z, 1.0), -1.0);
// These 2DOF just change the auto hover behaviour
// No bound() required
cmd_vel.angular.x = msg->angular.x;
cmd_vel.angular.y = msg->angular.y;
vp_os_mutex_unlock(&twist_lock);
}
void ardrone_at_set_control_gains( api_control_gains_t* gains )
{
if (!at_init)
return;
vp_os_mutex_lock(&at_mutex);
ATcodec_Queue_Message_valist( ids.AT_MSG_ATCMD_GAIN_EXE,
++nb_sequence,
gains->pq_kp, gains->r_kp, gains->r_ki, gains->ea_kp, gains->ea_ki,
gains->alt_kp, gains->alt_ki, gains->vz_kp, gains->vz_ki,
gains->hovering_kp, gains->hovering_ki,
gains->hovering_b_kp, gains->hovering_b_ki);
vp_os_mutex_unlock(&at_mutex);
}
/**
* @param p1
*
* @param p2
*
* @param p3
*
* @param p4
*
* @brief .
*
* @DESCRIPTION
*
*******************************************************************/
void ardrone_at_set_pwm(int32_t p1, int32_t p2, int32_t p3, int32_t p4)
{
if (!at_init)
return;
vp_os_mutex_lock(&at_mutex);
ATcodec_Queue_Message_valist( ids.AT_MSG_ATCMD_PWM_EXE,
++nb_sequence,
p1,
p2,
p3,
p4 );
vp_os_mutex_unlock(&at_mutex);
}
extern "C" C_RESULT export_stage_transform(void *cfg, vp_api_io_data_t *in, vp_api_io_data_t *out)
{
// PRINT("In Transform before copy\n");
// printf("The size of buffer is %d\n", in->size);
vp_os_mutex_lock(&video_lock);
memcpy(buffer, in->buffers[0], in->size);
current_frame_id++;
if (realtime_video)
{
ros_driver->PublishVideo();
}
vp_os_mutex_unlock(&video_lock);
// vp_os_mutex_unlock(&video_update_lock);
return (SUCCESS);
}
void ardrone_at_cad( CAD_TYPE type, float32_t tag_size )
{
float_or_int_t size;
size.f = tag_size;
if (!at_init)
return;
vp_os_mutex_lock(&at_mutex);
ATcodec_Queue_Message_valist( ids.AT_MSG_ATCMD_CAD_EXE,
++nb_sequence,
type,
size.i);
vp_os_mutex_unlock(&at_mutex);
}
C_RESULT
vp_stages_output_sdl_stage_close(vp_stages_output_sdl_config_t *cfg)
{
vp_os_mutex_lock(&xlib_mutex);
SDL_ShowCursor(SDL_ENABLE);
SDL_FreeYUVOverlay(cfg->overlay);
SDL_FreeSurface(cfg->surface);
SDL_Quit();
vp_os_mutex_unlock(&xlib_mutex);
return (VP_SUCCESS);
}
DEFINE_THREAD_ROUTINE_STACK(ATcodec_Commands_Client,data,ATCODEC_STACK_SIZE)
{
AT_CODEC_ERROR_CODE res;
int32_t v_loop;
v_continue = 1;
PRINT("Thread AT Commands Client Start\n");
while(!atcodec_lib_init_ok)
{
vp_os_thread_yield();
}
while(v_continue)
{
// open and init
if((res = func_ptrs.open()) != AT_CODEC_OPEN_OK)
v_continue = 0;
for ( v_loop = 1 ; v_loop ; )
{
// vp_os_delay(ATCODEC_SERVER_YIELD_DELAY);
vp_os_thread_yield();
// wait a successful ATcodec_Queue_... has been called
vp_os_mutex_lock(&ATcodec_cond_mutex);
//vp_os_cond_wait(&ATcodec_wait_cond);
//ATCODEC_PRINT("Must send \"%s\"\n", &ATcodec_Message_Buffer[0]);
if(ATcodec_Message_len && func_ptrs.write((int8_t*)&ATcodec_Message_Buffer[0], (int32_t*)&ATcodec_Message_len) != AT_CODEC_WRITE_OK)
v_loop = 0;
ATcodec_Message_len = 0;
vp_os_mutex_unlock(&ATcodec_cond_mutex);
}
// close and un-init : user-defined
if((res = func_ptrs.close()) != AT_CODEC_CLOSE_OK)
v_continue = 0;
}
if((res = func_ptrs.shutdown()) != AT_CODEC_SHUTDOWN_OK)
{
ATCODEC_PRINT("ATcodec Shutdown error\n");
}
return((THREAD_RET)0);
}
void ardrone_at_set_led_animation ( LED_ANIMATION_IDS anim_id, float32_t freq, uint32_t duration_sec )
{
float_or_int_t _freq;
if (!at_init)
return;
_freq.f = freq;
vp_os_mutex_lock(&at_mutex);
ATcodec_Queue_Message_valist(ids.AT_MSG_ATCMD_LED_EXE,
++nb_sequence,
anim_id,
_freq.i,
duration_sec);
vp_os_mutex_unlock(&at_mutex);
}
/**
* @param param A key as read from an ini file is given as "section:key".
*
* @param value
*
* @brief identified ardrone_at_set_toy_configuration
*
* @DESCRIPTION
*
*******************************************************************/
void ardrone_at_set_toy_configuration_ids(const char* param,char* ses_id, char* usr_id, char* app_id, const char* value)
{
if (!at_init)
return;
vp_os_mutex_lock(&at_mutex);
ATcodec_Queue_Message_valist( ids.AT_MSG_ATCMD_CONFIG_IDS,
++nb_sequence,
ses_id,
usr_id,
app_id );
ATcodec_Queue_Message_valist( ids.AT_MSG_ATCMD_CONFIG_EXE,
++nb_sequence,
param,
value );
vp_os_mutex_unlock(&at_mutex);
}
C_RESULT
vp_stages_output_file_stage_transform(vp_stages_output_file_config_t *cfg, vp_api_io_data_t *in, vp_api_io_data_t *out)
{
static int total_size = 0;
vp_os_mutex_lock(&out->lock);
if(in->status == VP_API_STATUS_PROCESSING && in->size > 0)
fwrite(in->buffers[in->indexBuffer], sizeof(int8_t), in->size*sizeof(int8_t), cfg->f);
fflush(cfg->f);
total_size += in->size;
out->status = in->status;
vp_os_mutex_unlock(&out->lock);
return (VP_SUCCESS);
}
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]; // we casten het naar een pointer van type uint8_t!!
// returns: A newly-created GdkPixbuf structure with a reference count of 1.
pixbuf = gdk_pixbuf_new_from_data(pixbuf_data, // image data in 8-bit/sample packed format
GDK_COLORSPACE_RGB,
FALSE, // has alpha
8, // bits per sample
imageWidth,
imageHeight,
imageWidth * 3, // distance in bites between rowstride
NULL, // GdkPixbufDestroyNotify detroy_fn
NULL); // gpoint destroy_fn_data);
sprintf(imageName, "images/frame%05d.jpg", ++imageCounter);
gdk_pixbuf_save(pixbuf, imageName, "jpeg", NULL, "quality", "100", NULL);
vp_os_mutex_unlock(&video_update_lock);
// Check if we need to switch the video feed to bottom or horizontal camera
if(videoSwitch != oldVideoSwitch)
{
oldVideoSwitch = videoSwitch;
if(videoSwitch > 0)
{
ardrone_at_zap(ZAP_CHANNEL_HORI);
imageWidth = 320;
imageHeight = 240;
}
else
{
ardrone_at_zap(ZAP_CHANNEL_VERT);
imageWidth = 320;
imageHeight = 144;
}
}
return (SUCCESS);
}
void
vp_os_thread_resume(THREAD_HANDLE handle)
{
vp_os_mutex_lock(&thread_mutex);
pthread_data_t* freeSlot = findThread( handle );
vp_os_mutex_unlock(&thread_mutex);
if( freeSlot != NULL )
{
if(freeSlot->isSleeping)
{
pthread_kill(handle,SIGRESUME);
freeSlot->isSleeping = 0;
}
}
}
int8_t *videoServer_waitForNewFrame() {
/* Wait for a new available frame */
vp_os_mutex_lock(&frameBufferMutex);
//C_RESULT res = vp_os_cond_timed_wait(&frameBufferCond, MAX_FRAME_WAIT_SECONDS * 1000);
//if (SUCCEED(res) && availVideoBuffer != NULL)
vp_os_cond_wait(&frameBufferCond);
if (availVideoBuffer != NULL) {
((VideoFrameHeader *)frameBuffer)->videoData.timeCodeH = (unsigned int)(videoServer_lastFrameTimeCode >> 32);
((VideoFrameHeader *)frameBuffer)->videoData.timeCodeL = (unsigned int)(videoServer_lastFrameTimeCode & 0xFFFFFFFF);
((VideoFrameHeader *)frameBuffer)->videoData.encoding = videoServer_frameSourceEncoding;
((VideoFrameHeader *)frameBuffer)->videoData.width = videoServer_frameWidth;
((VideoFrameHeader *)frameBuffer)->videoData.height = videoServer_frameHeight;
((VideoFrameHeader *)frameBuffer)->videoData.dataLength = videoServer_framePacketLength - sizeof(VideoFrameHeader);
vp_os_memcpy(&frameBuffer[sizeof(VideoFrameHeader)], availVideoBuffer, videoServer_framePacketLength - sizeof(VideoFrameHeader));
vp_os_mutex_unlock(&frameBufferMutex);
availVideoBuffer = NULL;
return frameBuffer;
}
/* Receving navdata during the event loop */
inline C_RESULT demo_navdata_client_process( const navdata_unpacked_t* const navdata )
{
//printf("\033[%dA", show_state(NULL, navdata));
const navdata_demo_t *nd = &navdata->navdata_demo;
if (nd->altitude == 0)
tag_on_the_ground = true;
else
tag_on_the_ground = false;
fill_navdata_slot(navdata, &navdata_slot);
vp_os_mutex_lock(&NOW_mutex);
NOW.hori_angle = nd->psi;
//printf("now%lf\n psi%lf", NOW.hori_angle, nd->psi);
vp_os_mutex_unlock(&NOW_mutex);
return C_OK;
}
void
vp_os_thread_suspend(THREAD_HANDLE handle)
{
vp_os_mutex_lock(&thread_mutex);
pthread_data_t* freeSlot = findThread( handle );
vp_os_mutex_unlock(&thread_mutex);
if( freeSlot != NULL )
{
if(!freeSlot->isSleeping)
{
freeSlot->isSleeping = 1;
pthread_kill(handle,SIGSUSPEND);
}
}
}
void ardrone_at_set_manual_trims(float32_t trim_pitch, float32_t trim_roll, float32_t trim_yaw)
{
float_or_int_t _trim_pitch, _trim_roll, _trim_yaw;
if (!at_init)
return;
_trim_pitch.f = trim_pitch;
_trim_roll.f = trim_roll;
_trim_yaw.f = trim_yaw;
vp_os_mutex_lock(&at_mutex);
ATcodec_Queue_Message_valist( ids.AT_MSG_ATCMD_MTRIM_EXE,
++nb_sequence,
_trim_pitch.i,
_trim_roll.i,
_trim_yaw.i);
vp_os_mutex_unlock(&at_mutex);
}
void
vp_os_thread_create(THREAD_ROUTINE f, THREAD_PARAMS parameters, THREAD_HANDLE *handle, ...)
{
int32_t priority;
char* name;
void* stack_base;
unsigned int stack_size;
va_list va;
pthread_data_t* freeSlot = NULL;
pthread_once(&once,&init_thread);
vp_os_mutex_lock(&thread_mutex);
freeSlot = findThread( NULL_THREAD_HANDLE );
while(freeSlot == NULL)
{
int old_size = threadTabSize;
threadTabSize += 128;
threadTab = ( pthread_data_t* )vp_os_realloc( threadTab, threadTabSize * sizeof( pthread_data_t ) );
vp_os_memset( threadTab + old_size, 0, threadTabSize * sizeof( pthread_data_t ) );
freeSlot = findThread( NULL_THREAD_HANDLE );
}
vp_os_mutex_unlock(&thread_mutex);
pthread_attr_init( &freeSlot->attr );
va_start(va, handle);
priority = va_arg(va, int32_t);
name = va_arg(va, char *);
stack_base = va_arg(va, void *);
stack_size = va_arg(va, unsigned int);
/* thread = va_arg(va, cyg_thread *);*/
va_end(va);
pthread_create( &freeSlot->handle, &freeSlot->attr, f, parameters);
*handle = freeSlot->handle;
vp_os_thread_priority(freeSlot->handle, priority);
}
void
vp_os_thread_join(THREAD_HANDLE handle)
{
PRINT("vp_os_thread_join\n %d\n", (int)handle);
vp_os_mutex_lock(&thread_mutex);
pthread_data_t* freeSlot = findThread( handle );
vp_os_mutex_unlock(&thread_mutex);
if( freeSlot != NULL )
{
void *res;
vp_os_memset(freeSlot, 0, sizeof(pthread_data_t));
pthread_join( handle, &res);
}
}
void ardrone_at_set_vision_track_params( api_vision_tracker_params_t* params )
{
if (!at_init)
return;
vp_os_mutex_lock(&at_mutex);
ATcodec_Queue_Message_valist( ids.AT_MSG_ATCMD_VISP_EXE,
++nb_sequence,
params->coarse_scale,
params->nb_pair,
params->loss_per,
params->nb_tracker_width,
params->nb_tracker_height,
params->scale,
params->trans_max,
params->max_pair_dist,
params->noise );
vp_os_mutex_unlock(&at_mutex);
}
