Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 02:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_OpticalFlow: use AP_PeriodicProcess to read at 20hz

Browse Source
This commit is contained in:
rmackay9 2012-09-14 20:33:08 +09:00
parent 1cf0b2334a
commit 17ccc7e62e
2 changed files with 33 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
libraries/AP_OpticalFlow/AP_OpticalFlow.cpp
View File

@ -13,10 +13,11 @@
#define FORTYFIVE_DEGREES 0.78539816
AP_OpticalFlow* AP_OpticalFlow::_sensor = NULL; // pointer to the last instantiated optical flow sensor. Will be turned into a table if we ever add support for more than one sensor
uint8_t AP_OpticalFlow::_num_calls; // number of times we have been called by 1khz timer process. We use this to throttle read down to 20hz
// init - initCommAPI 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)
bool
AP_OpticalFlow::init(bool initCommAPI)
AP_OpticalFlow::init(bool initCommAPI, AP_PeriodicProcess *scheduler)
{
_orientation = ROTATION_NONE;
update_conversion_factors();
@ -30,11 +31,26 @@ AP_OpticalFlow::set_orientation(enum Rotation rotation)
_orientation = rotation;
}
// read value from the sensor. Should be overridden by derived class
bool
AP_OpticalFlow::update()
// parent method called at 1khz by periodic process
// this is slowed down to 20hz and each instance's update function is called (only one instance is supported at the moment)
void
AP_OpticalFlow::read(uint32_t now)
{
_num_calls++;
if( _num_calls >= AP_OPTICALFLOW_NUM_CALLS_FOR_20HZ ) {
_num_calls = 0;
// call to update all attached sensors
if( _sensor != NULL ) {
_sensor->update(now);
}
}
};
// read value from the sensor. Should be overridden by derived class
void
AP_OpticalFlow::update(uint32_t now)
{
return true;
}
// reads a value from the sensor (will be sensor specific)

21
libraries/AP_OpticalFlow/AP_OpticalFlow.h
View File

@ -20,6 +20,11 @@
#include <FastSerial.h>
#include <AP_Math.h>
#include <AP_Common.h>
#include <AP_PeriodicProcess.h>
#define AP_OPTICALFLOW_NUM_CALLS_FOR_10HZ 100 // timer process runs at 1khz. 100 iterations = 10hz
#define AP_OPTICALFLOW_NUM_CALLS_FOR_20HZ 50 // timer process runs at 1khz. 50 iterations = 20hz
#define AP_OPTICALFLOW_NUM_CALLS_FOR_50HZ 20 // timer process runs at 1khz. 20 iterations = 50hz
class AP_OpticalFlow
{
@ -44,17 +49,13 @@ public:
~AP_OpticalFlow() {
_sensor = NULL;
};
virtual bool init(bool initCommAPI = true); // 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 bool init(bool initCommAPI, AP_PeriodicProcess *scheduler); // 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 byte read_register(byte address);
virtual void write_register(byte address, byte value);
virtual void set_orientation(enum Rotation rotation); // Rotation vector to transform sensor readings to the body frame.
virtual void set_field_of_view(const float fov) {
field_of_view = fov; update_conversion_factors();
}; // sets field of view of sensor
static void read(uint32_t ) {
if( _sensor != NULL ) _sensor->update();
}; // call to update all attached sensors
virtual bool update(); // read latest values from sensor and fill in x,y and totals. returns true on success
virtual void set_field_of_view(const float fov) { field_of_view = fov; update_conversion_factors(); }; // sets field of view of sensor
static void read(uint32_t now); // called by timer process to read sensor data from all attached sensors
virtual void update(uint32_t now); // read latest values from sensor and fill in x,y and totals.
virtual void update_position(float roll, float pitch, float cos_yaw_x, float sin_yaw_y, float altitude); // updates internal lon and lat with estimation based on optical flow
protected:
@ -65,9 +66,11 @@ protected:
float _last_roll, _last_pitch, _last_altitude;
virtual void apply_orientation_matrix(); // rotate raw values to arrive at final x,y,dx and dy values
virtual void update_conversion_factors();
private:
static uint8_t _num_calls; // number of times we have been called by 1khz timer process. We use this to throttle read down to 20hz
};
#include "AP_OpticalFlow_ADNS3080.h"
#include "AP_OpticalFlow_ADNS3080_APM2.h"
#endif