/**
* The main optical flow calculation thread
* This thread passes the images trough the optical flow
* calculator
* @param[in] *img The image_t structure of the captured image
* @return *img The processed image structure
*/
struct image_t *opticflow_module_calc(struct image_t *img)
{
// Copy the state
struct pose_t pose = get_rotation_at_timestamp(img->pprz_ts);
struct opticflow_state_t temp_state;
temp_state.agl = opticflow_state.agl;
temp_state.rates = pose.rates;
// Do the optical flow calculation
struct opticflow_result_t temp_result = {}; // new initialization
opticflow_calc_frame(&opticflow, &temp_state, img, &temp_result);
// Copy the result if finished
pthread_mutex_lock(&opticflow_mutex);
memcpy(&opticflow_result, &temp_result, sizeof(struct opticflow_result_t));
opticflow_got_result = true;
/* Rotate velocities from camera frame coordinates to body coordinates for control
* IMPORTANT!!! This frame to body orientation should be the case for the Parrot
* ARdrone and Bebop, however this can be different for other quadcopters
* ALWAYS double check!
*/
opticflow_result.vel_body_x = opticflow_result.vel_y;
opticflow_result.vel_body_y = - opticflow_result.vel_x;
// release the mutex as we are done with editing the opticflow result
pthread_mutex_unlock(&opticflow_mutex);
return img;
}
static void save_shot_on_disk(struct image_t *img, struct image_t *img_jpeg)
{
// Search for a file where we can write to
char save_name[128];
snprintf(save_name, sizeof(save_name), "%s/img_%05d.jpg", foldername, shotNumber);
shotNumber++;
// Check if file exists or not
if (access(save_name, F_OK) == -1) {
// Create a high quality image (99% JPEG encoded)
jpeg_encode_image(img, img_jpeg, 99, TRUE);
#if VIDEO_USB_LOGGER_JPEG_WITH_EXIF_HEADER
write_exif_jpeg(save_name, img_jpeg->buf, img_jpeg->buf_size, img_jpeg->w, img_jpeg->h);
#else
FILE *fp = fopen(save_name, "w");
if (fp == NULL) {
printf("[video_thread-thread] Could not write shot %s.\n", save_name);
} else {
// Save it to the file and close it
fwrite(img_jpeg->buf, sizeof(uint8_t), img_jpeg->buf_size, fp);
fclose(fp);
printf("Wrote image\n");
}
#endif
/** Log the values to a csv file */
if (video_usb_logger == NULL) {
return;
}
static uint32_t counter = 0;
struct pose_t pose = get_rotation_at_timestamp(img->pprz_ts);
struct NedCoor_i *ned = stateGetPositionNed_i();
struct NedCoor_i *accel = stateGetAccelNed_i();
static uint32_t sonar = 0;
// Save current information to a file
fprintf(video_usb_logger, "%d,%d,%f,%f,%f,%d,%d,%d,%d,%d,%d,%f,%f,%f,%d\n", counter,
shotNumber,
pose.eulers.phi, pose.eulers.theta, pose.eulers.psi,
ned->x, ned->y, ned->z,
accel->x, accel->y, accel->z,
pose.rates.p, pose.rates.q, pose.rates.r,
sonar);
counter++;
}
}
/**
* The main optical flow calculation thread
* This thread passes the images trough the optical flow
* calculator
* @param[in] *img The image_t structure of the captured image
* @return *img The processed image structure
*/
struct image_t *opticflow_module_calc(struct image_t *img)
{
// Copy the state
struct pose_t pose = get_rotation_at_timestamp(img->pprz_ts);
struct opticflow_state_t temp_state;
temp_state.agl = opticflow_state.agl;
temp_state.rates = pose.rates;
// Do the optical flow calculation
struct opticflow_result_t temp_result = {}; // new initialization
opticflow_calc_frame(&opticflow, &temp_state, img, &temp_result);
// Copy the result if finished
pthread_mutex_lock(&opticflow_mutex);
memcpy(&opticflow_result, &temp_result, sizeof(struct opticflow_result_t));
opticflow_got_result = true;
pthread_mutex_unlock(&opticflow_mutex);
// TODO: why is there a mutex above and not below when changing opticflow_result?
/* Rotate velocities from camera frame coordinates to body coordinates for control
* IMPORTANT!!! This frame to body orientation should be the case for the Parrot
* ARdrone and Bebop, however this can be different for other quadcopters
* ALWAYS double check!
*/
#if CAMERA_ROTATED_180 == 0 //Case for ARDrone 2.0
opticflow_result.vel_body_x = opticflow_result.vel_y;
opticflow_result.vel_body_y = - opticflow_result.vel_x;
#else // Case for Bebop 2
opticflow_result.vel_body_x = - opticflow_result.vel_y;
opticflow_result.vel_body_y = opticflow_result.vel_x;
#endif
return img;
}
