/**
* \brief Receive the navdata from the drone and write it in NAVDATA_CHANNEL.
* \return the result of recvfrom
*/
int recv_cmd()
{
pthread_mutex_lock(&mutex_navdata);
socklen_t len = sizeof(addr_drone_navdata);
int ret = recvfrom(sock_navdata, &data, sizeof(data), 0, (struct sockaddr*)&addr_drone_navdata, &len);
size_t offset = 0;
switch (data.demo.tag) {
case TAG_DEMO:
jakopter_com_write_int(nav_channel, offset, data.demo.vbat_flying_percentage);
offset += sizeof(data.demo.vbat_flying_percentage);
jakopter_com_write_int(nav_channel, offset, data.demo.altitude);
offset += sizeof(data.demo.altitude);
jakopter_com_write_float(nav_channel, offset, data.demo.theta);
offset += sizeof(data.demo.theta);
jakopter_com_write_float(nav_channel, offset, data.demo.phi);
offset += sizeof(data.demo.phi);
jakopter_com_write_float(nav_channel, offset, data.demo.psi);
offset += sizeof(data.demo.psi);
jakopter_com_write_float(nav_channel, offset, data.demo.vx);
offset += sizeof(data.demo.vx);
jakopter_com_write_float(nav_channel, offset, data.demo.vy);
offset += sizeof(data.demo.vy);
jakopter_com_write_float(nav_channel, offset, data.demo.vz);
break;
default:
break;
}
pthread_mutex_unlock(&mutex_navdata);
return ret;
}
int video_display_init()
{
com_in = jakopter_com_add_channel(CHANNEL_DISPLAY, DISPLAY_COM_IN_SIZE);
if (com_in == NULL) {
fprintf(stderr, "[~][Display] Couldn't create com channel.\n");
return -1;
}
com_out = jakopter_com_add_channel(CHANNEL_CALLBACK, DISPLAY_COM_OUT_SIZE);
if (com_out == NULL) {
fprintf(stderr, "[~][Display] Couldn't create com channel out.\n");
return -1;
}
jakopter_com_write_float(com_out, 0, 0.0);
jakopter_com_write_int(com_in, 28, -1);
jakopter_com_write_int(com_in, 32, -1);
jakopter_com_write_int(com_in, 36, -1);
jakopter_com_write_int(com_in, 40, -1);
prev_update = 0;
return 0;
}
/**
* \brief Receive the navdata from the drone and write it in NAVDATA_CHANNEL.
* \return the result of recvfrom
*/
static int recv_cmd()
{
pthread_mutex_lock(&mutex_navdata);
socklen_t len = sizeof(addr_drone_navdata);
int ret = recvfrom(sock_navdata, &data, sizeof(data), 0, (struct sockaddr*)&addr_drone_navdata, &len);
size_t offset = 0;
//fprintf(stderr,"FBO: Receiving navdata (%d)\n", data.demo.altitude); // use altitude
switch (data.demo.tag) {
case TAG_DEMO:
jakopter_com_write_int(nav_channel, offset, data.demo.vbat_flying_percentage);
offset += sizeof(data.demo.vbat_flying_percentage);
jakopter_com_write_int(nav_channel, offset, data.demo.altitude);
offset += sizeof(data.demo.altitude);
jakopter_com_write_float(nav_channel, offset, data.demo.theta);
offset += sizeof(data.demo.theta);
jakopter_com_write_float(nav_channel, offset, data.demo.phi);
offset += sizeof(data.demo.phi);
jakopter_com_write_float(nav_channel, offset, data.demo.psi);
offset += sizeof(data.demo.psi);
jakopter_com_write_float(nav_channel, offset, data.demo.vx);
offset += sizeof(data.demo.vx);
jakopter_com_write_float(nav_channel, offset, data.demo.vy);
offset += sizeof(data.demo.vy);
jakopter_com_write_float(nav_channel, offset, data.demo.vz);
break;
default:
break;
}
pthread_mutex_unlock(&mutex_navdata);
// ADDED: Display navdatas for debug purpose
debug_navdata_demo();
navdata_timestamp();
return ret;
}
int jakopter_com_write_int_lua(lua_State* L)
{
lua_Integer id = luaL_checkinteger(L, 1);
lua_Integer offset = luaL_checkinteger(L, 2);
lua_Integer value = luaL_checkinteger(L, 3);
jakopter_com_channel_t* cc = jakopter_com_get_channel(id);
if (cc == NULL)
return luaL_error(L, "[~][lua] com_channel of id %d doesn't exist", id);
jakopter_com_write_int(cc, offset, value);
return 0;
}
int jakopter_com_write_string_lua(lua_State* L)
{
lua_Integer id = luaL_checkinteger(L, 1);
lua_Integer offset = luaL_checkinteger(L, 2);
const char *string = luaL_checkstring(L, 3);
jakopter_com_channel_t* cc = jakopter_com_get_channel(id);
if (cc == NULL)
return luaL_error(L, "[~][lua] com_channel of id %d doesn't exist", id);
jakopter_com_write_int(cc, offset, strlen(string)+1);
jakopter_com_write_buf(cc, offset+4, (void*)string, strlen(string)+1);
return 0;
}
static int qrcode_process(uint8_t* frame, int width, int height, int size)
{
init_display(width, height);
if (!initialized || process_changed) {
initialized = true;
process_changed = false;
}
prepare_display(frame, width, height, size);
if (qr_detector.detect(*grey_img)) {
std::vector<vpImagePoint> pol = qr_detector.getPolygon(0);
vpRect bbox = qr_detector.getBBox(0);
vpDisplay::displayRectangle(*display_img, bbox, vpColor::green);
vpDisplay::displayText(*display_img, (int)(bbox.getTop()-10), (int)bbox.getLeft(),
"Message: \"" + qr_detector.getMessage(0) + "\"",
vpColor::red);
for (size_t j = 0; j < pol.size(); j++) {
vpDisplay::displayCross(*display_img, pol[j], 14, vpColor::red, 3);
std::ostringstream number;
number << j;
vpDisplay::displayText(*display_img, pol[j]+vpImagePoint(10,0),
number.str(), vpColor::blue);
}
double x = 0;
double y = 0;
bbox.getCenter(x, y);
std::string msg = qr_detector.getMessage(0);
char cstr[SIZE_MESSAGE];
strncpy(cstr, msg.c_str(),SIZE_MESSAGE);
jakopter_com_write_float(com_out, 0, (float)bbox.getSize());
jakopter_com_write_float(com_out, 4, (float)x);
jakopter_com_write_float(com_out, 8, (float)y);
jakopter_com_write_int(com_out, 12, strlen(cstr)+1);
jakopter_com_write_buf(com_out, 16, cstr, SIZE_MESSAGE);
}
int bat = jakopter_com_read_int(com_in, 0);
std::ostringstream bat_str;
bat_str << bat;
vpDisplay::displayText(*display_img, (int)display_img->getHeight()-25, 10,
"Battery : " + bat_str.str() + " %", vpColor::red);
vpDisplay::flush(*display_img);
return 0;
}
//FBO Sample the video to do some stuff
int sample_video(uint8_t* frame, int width, int height, int size){
static int call_count;
static int init_callback_done=0;
//FBO DO THE JOB HERE
//
if (init_callback_done){
call_count++;
} else {
call_count = 0;
init_callback_done = 1;
}
fprintf(stderr,"Calling sample_video (call count = %d)\n",call_count);
//Return result and status here
jakopter_com_write_int(com_out, 0, call_count);
return 0;
}
//FBO analyse frame on the server once a while // port is 5000
// WARNING this routine is not thread safe
int analyse_video(uint8_t* frame, int width, int height, int size){
static int call_count = 0;
static int init_callback_done=0;
static int sockfd = 0;
//FBO DO THE JOB HERE
//fprintf(stderr,"Calling sample_video (call count = %d)\n",call_count);
jakopter_com_write_int(com_out, 0, call_count); // in all cases need to return the call back instance
if (init_callback_done){
call_count++;
int doit = jakopter_com_read_int(com_in, 28);
if (doit) {
copyDataInBuffer(imageBuffer,frame,IMAGE_TO_ANALYZE,width,height,width);
sendLargeData(sockfd,imageBuffer,IMAGE_BUFFER_SIZE);
receiveLargeData(sockfd, resultBuffer, ANALYSIS_BUFFER_SIZE);
int coord_i_min = *((int *) &(resultBuffer[0]));
int coord_j_min = *((int *) &(resultBuffer[4]));
double valx = *((double *) &(resultBuffer[8]));
int samplenum = *((int *) &(resultBuffer[16]));
// show it on the video also
//printf("server answer sample %d result %d %d %f\n",samplenum, coord_i_min, coord_j_min, valx);
//Return result and status here
jakopter_com_write_int(com_out, 4, samplenum);
jakopter_com_write_int(com_out, 8, coord_i_min);
jakopter_com_write_int(com_out, 12, coord_j_min);
jakopter_com_write_float(com_out, 16, valx);
}
} else {
call_count = 0;
init_callback_done = 1;
// Open the socket
sockfd = openSocket((char *) "localhost",5000);
jakopter_com_write_int(com_out, 4, -1);
jakopter_com_write_int(com_out, 8, 0);
jakopter_com_write_int(com_out, 12, 0);
jakopter_com_write_float(com_out, 16, 0.0);
}
return 0;
}
int video_display_process(uint8_t* frame, int width, int height, int size) {
//if we get a NULL frame, stop displaying.
if(frame == NULL) {
video_display_destroy();
return 0;
}
//first time called ? Initialize things.
if(!initialized) {
if(video_display_init_size(width, height) < 0) {
fprintf(stderr, "[~][Display] Failed initialization.\n");
return -1;
}
if(video_display_set_size(width, height) < 0)
return -1;
initialized = 1;
}
//check whether the size of the video has changed
if (width != current_width || height != current_height)
if (video_display_set_size(width, height) < 0)
return -1;
//check whether there's new stuff in the input com buffer
double new_update = jakopter_com_get_timestamp(com_in);
if (new_update > prev_update) {
update_infos();
if (want_screenshot) {
take_screenshot(frame, size);
want_screenshot = 0;
jakopter_com_write_int(com_in, 24, 0);
}
prev_update = new_update;
}
pthread_mutex_lock(&mutex_process);
if (current_process != NULL) {
current_process(frame, width, height, size);
}
pthread_mutex_unlock(&mutex_process);
unload_element();
if (load_element() < 0)
fprintf(stderr, "Couldn't load graphic element\n");
// update the texture with our new frame
if (SDL_UpdateTexture(frameTex, NULL, frame, width) < 0) {
fprintf(stderr, "[~][Display] Failed to update frame texture : %s\n", SDL_GetError());
return -1;
}
//clear the renderer, then update it so that we get the new frame displayed.
SDL_RenderClear(renderer);
SDL_RenderCopy(renderer, frameTex, NULL, NULL);
//SDL_RenderFillRect(renderer, &rectangle);
//draw all overlay elements, when they exist
int i = 0;
for (i = 0 ; i < VIDEO_NB_NAV_INFOS ; i++)
if (graphs[i].tex != NULL)
SDL_RenderCopy(renderer, graphs[i].tex, NULL, &graphs[i].pos);
pthread_mutex_lock(&mutex_graphics_list);
struct graphics_list *current = icon_registry;
while (current != NULL) {
if (current->graphic != NULL && current->graphic->tex != NULL){
int ret = SDL_RenderCopy(renderer, current->graphic->tex, NULL, ¤t->graphic->pos);
if (ret < 0)
fprintf(stderr, "[~][display] RenderCopy() failed: %s\n", SDL_GetError());
}
current = current->next;
}
pthread_mutex_unlock(&mutex_graphics_list);
draw_attitude_indic();
draw_compass();
SDL_RenderPresent(renderer);
return 0;
}
