From 17ccc7e62ef2228b7dde934af0535ed17912ec79 Mon Sep 17 00:00:00 2001 From: rmackay9 Date: Fri, 14 Sep 2012 20:33:08 +0900 Subject: [PATCH] AP_OpticalFlow: use AP_PeriodicProcess to read at 20hz --- libraries/AP_OpticalFlow/AP_OpticalFlow.cpp | 26 +++++++++++++++++---- libraries/AP_OpticalFlow/AP_OpticalFlow.h | 21 ++++++++++------- 2 files changed, 33 insertions(+), 14 deletions(-) diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp index bd8519480b..99b59cefc0 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp @@ -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) diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow.h b/libraries/AP_OpticalFlow/AP_OpticalFlow.h index 18caabe502..7107d73c06 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow.h +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow.h @@ -20,6 +20,11 @@ #include #include #include +#include + +#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