void update_infos()
{
//base y position of the text
int base_y = 0;
//height of a line of text with the current font.
int line_height = TTF_FontLineSkip(font);
//buffer to hold the current textto be drawn
char buf[TEXT_BUF_SIZE];
//retrieve navdata one by one
int bat = jakopter_com_read_int(com_in, 0);
//and format it for textual display
snprintf(buf, TEXT_BUF_SIZE, "Battery : %d%%", bat);
buf[TEXT_BUF_SIZE-1] = '\0';
//finally, print it onto a texture using SDL_ttf
graphs[0].tex = video_make_text(buf, &graphs[0].pos.w, &graphs[0].pos.h);
//and set its draw position accordingly
graphs[0].pos.x = 0;
graphs[0].pos.y = base_y;
//go to a new line
base_y += line_height;
int alt = jakopter_com_read_int(com_in, 4);
snprintf(buf, TEXT_BUF_SIZE, "Altitude : %d", alt);
buf[TEXT_BUF_SIZE-1] = '\0';
graphs[1].tex = video_make_text(buf, &graphs[1].pos.w, &graphs[1].pos.h);
graphs[1].pos.x = 0;
graphs[1].pos.y = base_y;
//update pitch, roll, yaw and speed
pitch = jakopter_com_read_float(com_in, 8);
roll = jakopter_com_read_float(com_in, 12);
yaw = jakopter_com_read_float(com_in, 16);
speed = jakopter_com_read_float(com_in, 20);
}
static int blob_process(uint8_t* frame, int width, int height, int size)
{
init_display(width, height);
if (!initialized || process_changed) {
blob.setGraphics(true);
blob.setGraphicsThickness(2);
initialized = true;
process_changed = false;
}
prepare_display(frame, width, height, size);
//compute blob
if (!init_track) {
blob.initTracking(*grey_img, germ);
init_track = true;
}
else {
blob.track(*grey_img);
}
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;
}
int jakopter_com_read_int_lua(lua_State* L)
{
lua_Integer id = luaL_checkinteger(L, 1);
lua_Integer offset = luaL_checkinteger(L, 2);
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);
lua_pushinteger(L, jakopter_com_read_int(cc, offset));
return 1;
}
int count_blue_px(uint8_t* frame, int width, int height, int size)
{
//size = width*height*3/2
int y_size = width*height;
int param = jakopter_com_read_int(com_in, 28);
int brightness = (param > -1 && param <= 256) ? param : 70;
param = jakopter_com_read_int(com_in, 32);
int darkness = (param > -1 && param <= 256) ? param : 256;
param = jakopter_com_read_int(com_in, 36);
int cr_u = (param > -1 && param <= 256) ? param : 127;
param = jakopter_com_read_int(com_in, 40);
int cr_v = (param > -1 && param <= 256) ? param : 127;
int h = 0;
float count = 0.0;
while (h < height) {
int w = 0;
while (w < width) {
int y = *(frame + (h * width + w));
int u = *(frame + ((h / 2) * (width / 2) + (w / 2) + y_size));
int v = *(frame + ((h / 2) * (width / 2) + (w / 2) + y_size + (y_size / 4)));
if (y > brightness && y < darkness && u >= cr_u && v <= cr_v)
count += 1.0;
w++;
}
//printf("%.2f\n", count);
h++;
}
count = count / (float)width;
//printf("%.2f\n", count);
jakopter_com_write_float(com_out, 0, count);
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;
}
static int face_process(uint8_t* frame, int width, int height, int size)
{
//alt_tree, lbpcascade
std::string face_cascade_script = "/usr/share/opencv/lbpcascades/lbpcascade_frontalface.xml";
init_display(width, height);
if (!initialized || process_changed) {
face_detector.setCascadeClassifierFile(face_cascade_script);
initialized = true;
process_changed = false;
}
prepare_display(frame, width, height, size);
//compute face detection
bool face_found = face_detector.detect(*grey_img);
if (face_found) {
std::ostringstream text;
text << "Found " << face_detector.getNbObjects() << " face(s)";
vpDisplay::displayText(*display_img, 10, 10, text.str(), vpColor::red);
//we get the first face in the list, which is also the biggest
std::vector<vpImagePoint> p = face_detector.getPolygon(0);
vpRect bbox = face_detector.getBBox(0);
vpDisplay::displayRectangle(*display_img, bbox, vpColor::green, false, 4);
vpDisplay::displayText(*display_img, (int)bbox.getTop()-10, (int)bbox.getLeft(),
"Message: \"" + face_detector.getMessage(0) + "\"",
vpColor::red);
double x = 0;
double y = 0;
bbox.getCenter(x, y);
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);
}
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;
}
int jakopter_com_read_string_lua(lua_State* L)
{
lua_Integer id = luaL_checkinteger(L, 1);
lua_Integer offset = luaL_checkinteger(L, 2);
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);
size_t size = jakopter_com_read_int(cc, offset);
char* string = malloc(size);
jakopter_com_read_buf(cc, offset+4, size, string);
if (size == 0)
lua_pushstring(L, NULL);
else
lua_pushstring(L, string);
free(string);
return 1;
}
//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;
}
