ardupilot/libraries/AP_OpticalFlow/AP_OpticalFlow.h

109 lines
4.0 KiB
C++

#ifndef __AP_OPTICALFLOW_H__
#define __AP_OPTICALFLOW_H__
/*
* AP_OpticalFlow.cpp - OpticalFlow Base Class for Ardupilot Mega
* Code by Randy Mackay. DIYDrones.com
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* Methods:
* init() : initializate sensor and library.
* read : reads latest value from OpticalFlow and
* stores values in x,y, surface_quality parameter
* read_register() : reads a value from the sensor (will be
* sensor specific)
* write_register() : writes a value to one of the sensor's
* register (will be sensor specific)
*/
#include <AP_Math.h>
// timer process runs at 1khz. 100 iterations = 10hz
#define AP_OPTICALFLOW_NUM_CALLS_FOR_10HZ 100
// timer process runs at 1khz. 50 iterations = 20hz
#define AP_OPTICALFLOW_NUM_CALLS_FOR_20HZ 50
// timer process runs at 1khz. 20 iterations = 50hz
#define AP_OPTICALFLOW_NUM_CALLS_FOR_50HZ 20
class AP_OpticalFlow
{
public:
// raw sensor change in x and y position (i.e. unrotated)
int raw_dx, raw_dy;
// image quality (below 15 you really can't trust the x,y values returned)
int surface_quality;
// total x,y position
int x,y;
// rotated change in x and y position
int dx,dy;
// position as offsets from original position
float vlon, vlat;
// millis() time of last update
uint32_t last_update;
// field of view in Radians
float field_of_view;
// number returned from sensor when moved one pixel
float scaler;
// number of pixels of resolution in the sensor
int num_pixels;
// temp - delete me!
float exp_change_x, exp_change_y;
float change_x, change_y;
float x_cm, y_cm;
AP_OpticalFlow() {
_sensor = this;
};
~AP_OpticalFlow() {
_sensor = NULL;
};
virtual bool init();
// parameter controls whether I2C/SPI interface is initialised
// (set to false if other devices are on the I2C/SPI bus and have already
// initialised the interface)
virtual uint8_t read_register(uint8_t address);
virtual void write_register(uint8_t address, uint8_t value);
// Rotation vector to transform sensor readings to the body frame.
virtual void set_orientation(enum Rotation rotation);
// sets field of view of sensor
virtual void set_field_of_view(const float fov) {
field_of_view = fov; update_conversion_factors();
};
// called by timer process to read sensor data from all attached sensors
static void read(uint32_t now);
// read latest values from sensor and fill in x,y and totals.
virtual void update(uint32_t now);
// updates internal lon and lat with estimation based on optical flow
virtual void update_position(float roll,
float pitch, float cos_yaw_x, float sin_yaw_y, float altitude);
protected:
// pointer to the last instantiated optical flow sensor. Will be turned
// into a table if we ever add support for more than one sensor
static AP_OpticalFlow * _sensor;
enum Rotation _orientation;
// multiply this number by altitude and pixel change to get horizontal
// move (in same units as altitude)
float conv_factor;
float radians_to_pixels;
float _last_roll;
float _last_pitch;
float _last_altitude;
// rotate raw values to arrive at final x,y,dx and dy values
virtual void apply_orientation_matrix();
virtual void update_conversion_factors();
private:
// number of times we have been called by 1khz timer process.
// We use this to throttle read down to 20hz
static uint8_t _num_calls;
};
#include "AP_OpticalFlow_ADNS3080.h"
#endif