opticflow_module.c

Go to the documentation of this file.

/*
 * Copyright (C) 2014 Hann Woei Ho
 *
 * This file is part of Paparazzi.
 *
 * Paparazzi is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2, or (at your option)
 * any later version.
 *
 * Paparazzi is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with Paparazzi; see the file COPYING.  If not, see
 * <http://www.gnu.org/licenses/>.
 */

#include "opticflow_module.h"

#include <stdio.h>
#include <pthread.h>
#include "state.h"
#include "subsystems/abi.h"

#include "lib/v4l/v4l2.h"
#include "lib/encoding/jpeg.h"
#include "lib/encoding/rtp.h"
#include "errno.h"

#include "cv.h"

/* Default sonar/agl to use in opticflow visual_estimator */
#ifndef OPTICFLOW_AGL_ID
#define OPTICFLOW_AGL_ID ABI_BROADCAST    
#endif
PRINT_CONFIG_VAR(OPTICFLOW_AGL_ID)

#ifndef OPTICFLOW_SENDER_ID
#define OPTICFLOW_SENDER_ID 1
#endif

/* The main opticflow variables */
struct opticflow_t opticflow;                      
static struct opticflow_result_t opticflow_result; 
static struct opticflow_state_t opticflow_state;   
static abi_event opticflow_agl_ev;                 
static bool opticflow_got_result;                
static pthread_mutex_t opticflow_mutex;            

/* Static functions */
struct image_t *opticflow_module_calc(struct image_t *img);     
static void opticflow_agl_cb(uint8_t sender_id, float distance);    

#if PERIODIC_TELEMETRY
#include "subsystems/datalink/telemetry.h"
static void opticflow_telem_send(struct transport_tx *trans, struct link_device *dev)
{
  pthread_mutex_lock(&opticflow_mutex);
  if (opticflow_result.noise_measurement < 0.8) {
    pprz_msg_send_OPTIC_FLOW_EST(trans, dev, AC_ID,
                                 &opticflow_result.fps, &opticflow_result.corner_cnt,
                                 &opticflow_result.tracked_cnt, &opticflow_result.flow_x,
                                 &opticflow_result.flow_y, &opticflow_result.flow_der_x,
                                 &opticflow_result.flow_der_y, &opticflow_result.vel_x,
                                 &opticflow_result.vel_y, &opticflow_result.div_size,
                                 &opticflow_result.surface_roughness, &opticflow_result.divergence); // TODO: no noise measurement here...
  }
  pthread_mutex_unlock(&opticflow_mutex);
}
#endif

void opticflow_module_init(void)
{
  // Subscribe to the altitude above ground level ABI messages
  AbiBindMsgAGL(OPTICFLOW_AGL_ID, &opticflow_agl_ev, opticflow_agl_cb);

  // Set the opticflow state to 0
  FLOAT_RATES_ZERO(opticflow_state.rates);
  opticflow_state.agl = 0;

  // Initialize the opticflow calculation
  opticflow_got_result = false;

  cv_add_to_device(&OPTICFLOW_CAMERA, opticflow_module_calc);

#if PERIODIC_TELEMETRY
  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_OPTIC_FLOW_EST, opticflow_telem_send);
#endif

}

void opticflow_module_run(void)
{
  pthread_mutex_lock(&opticflow_mutex);
  // Update the stabilization loops on the current calculation
  if (opticflow_got_result) {
    uint32_t now_ts = get_sys_time_usec();
    AbiSendMsgOPTICAL_FLOW(OPTICFLOW_SENDER_ID, now_ts,
                           opticflow_result.flow_x,
                           opticflow_result.flow_y,
                           opticflow_result.flow_der_x,
                           opticflow_result.flow_der_y,
                           opticflow_result.noise_measurement,// FIXME, scale to some quality measure 0-255
                           opticflow_result.div_size,
                           opticflow_state.agl);
    //TODO Find an appropiate quality measure for the noise model in the state filter, for now it is tracked_cnt
    if (opticflow_result.noise_measurement < 0.8) {
      AbiSendMsgVELOCITY_ESTIMATE(OPTICFLOW_SENDER_ID, now_ts,
                                  opticflow_result.vel_body_x,
                                  opticflow_result.vel_body_y,
                                  0.0f,
                                  opticflow_result.noise_measurement
                                 );
    }
    opticflow_got_result = false;
  }
  pthread_mutex_unlock(&opticflow_mutex);
}

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 opticflow_agl_cb(uint8_t sender_id __attribute__((unused)), float distance)
{
  // Update the distance if we got a valid measurement
  if (distance > 0) {
    opticflow_state.agl = distance;
  }
}