/**
* 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
pthread_mutex_lock(&opticflow_mutex);
struct opticflow_state_t temp_state;
memcpy(&temp_state, &opticflow_state, sizeof(struct opticflow_state_t));
pthread_mutex_unlock(&opticflow_mutex);
// Do the optical flow calculation
struct opticflow_result_t temp_result;
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);
/* 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;
}
/**
* 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;
}
/**
* 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;
}
