zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_OpticalFlow: Add support for Linux 

Add a Linux userspace driver for the PX4FLOW sensor.
mission-4.1.18
Víctor Mayoral Vilches 10 years ago committed by Randy Mackay
parent
42c7b4a116
commit
ed3366aaae
4 changed files with 298 additions and 0 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 199
      libraries/AP_OpticalFlow/AP_OpticalFlow_Linux.cpp
  2. 96
      libraries/AP_OpticalFlow/AP_OpticalFlow_Linux.h
  3. 2
      libraries/AP_OpticalFlow/OpticalFlow.cpp
  4. 1
      libraries/AP_OpticalFlow/OpticalFlow.h

199
libraries/AP_OpticalFlow/AP_OpticalFlow_Linux.cpp
Unescape View File

@ -0,0 +1,199 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
This program 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 3 of the License, or
(at your option) any later version.
This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
*/
/*
* AP_OpticalFlow_Linux.cpp - ardupilot library for the PX4Flow sensor.
* inspired by the PX4Firmware code.
*
* @author: Víctor Mayoral Vilches
*
*/
#include <AP_HAL.h>
#include "OpticalFlow.h"
#define DEBUG 1
#define RAW_READ 0
#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
extern const AP_HAL::HAL& hal;
AP_OpticalFlow_Linux::AP_OpticalFlow_Linux(OpticalFlow &_frontend) :
OpticalFlow_backend(_frontend)
{}
void AP_OpticalFlow_Linux::init(void)
{
uint8_t buff[22];
// get pointer to i2c bus semaphore
AP_HAL::Semaphore *i2c_sem = hal.i2c->get_semaphore();
// take i2c bus sempahore
if (!i2c_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
hal.scheduler->panic(PSTR("PX4FLOW: unable to get semaphore"));
}
// to be sure this is not a ll40ls Lidar (which can also be on
// 0x42) we check if a I2C_FRAME_SIZE byte transfer works from address
// 0. The ll40ls gives an error for that, whereas the flow
// happily returns some data
uint8_t val[I2C_FRAME_SIZE];
if (hal.i2c->readRegisters(I2C_FLOW_ADDRESS, 0, I2C_FRAME_SIZE, val))
hal.scheduler->panic(PSTR("ll40ls Lidar"));
i2c_sem->give();
}
int AP_OpticalFlow_Linux::read(optical_flow_s* report)
{
// get pointer to i2c bus semaphore
AP_HAL::Semaphore *i2c_sem = hal.i2c->get_semaphore();
// take i2c bus sempahore
if (!i2c_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
hal.scheduler->panic(PSTR("PX4FLOW: unable to get semaphore"));
}
uint8_t val[I2C_FRAME_SIZE + I2C_INTEGRAL_FRAME_SIZE] = { 0 };
#if RAW_READ
hal.console->printf_P(PSTR("PX4FLOW: RAW_READ"));
// Send the command to begin a measurement
uint8_t cmd = PX4FLOW_REG;
if (hal.i2c->write(I2C_FLOW_ADDRESS, 1, &cmd)){
hal.console->printf_P(PSTR("PX4FLOW: Error while beginning a measurement"));
i2c_sem->give();
return 0;
}
// Perform the reading
if (PX4FLOW_REG == 0x00) {
if (hal.i2c->read(I2C_FLOW_ADDRESS, I2C_FRAME_SIZE + I2C_INTEGRAL_FRAME_SIZE, val)){
hal.console->printf_P(PSTR("PX4FLOW: Error while reading"));
i2c_sem->give();
return 0;
}
}
if (PX4FLOW_REG == 0x16) {
if (hal.i2c->read(I2C_FLOW_ADDRESS, I2C_INTEGRAL_FRAME_SIZE, val)){
hal.console->printf_P(PSTR("PX4FLOW: Error while reading"));
i2c_sem->give();
return 0;
}
}
#else
// Perform the writing and reading in a single command
if (PX4FLOW_REG == 0x00) {
if (hal.i2c->readRegisters(I2C_FLOW_ADDRESS, PX4FLOW_REG, I2C_FRAME_SIZE + I2C_INTEGRAL_FRAME_SIZE, val)){
hal.console->printf_P(PSTR("PX4FLOW: Error while reading"));
i2c_sem->give();
return 0;
}
}
if (PX4FLOW_REG == 0x16) {
if (hal.i2c->readRegisters(I2C_FLOW_ADDRESS, PX4FLOW_REG, I2C_INTEGRAL_FRAME_SIZE, val)){
hal.console->printf_P(PSTR("PX4FLOW: Error while reading"));
i2c_sem->give();
return 0;
}
}
#endif
if (PX4FLOW_REG == 0) {
memcpy(&f, val, I2C_FRAME_SIZE);
memcpy(&f_integral, &(val[I2C_FRAME_SIZE]), I2C_INTEGRAL_FRAME_SIZE);
}
if (PX4FLOW_REG == 0x16) {
memcpy(&f_integral, val, I2C_INTEGRAL_FRAME_SIZE);
}
// report->timestamp = hrt_absolute_time();
report->pixel_flow_x_integral = static_cast<float>(f_integral.pixel_flow_x_integral) / 10000.0f;//convert to radians
report->pixel_flow_y_integral = static_cast<float>(f_integral.pixel_flow_y_integral) / 10000.0f;//convert to radians
report->frame_count_since_last_readout = f_integral.frame_count_since_last_readout;
report->ground_distance_m = static_cast<float>(f_integral.ground_distance) / 1000.0f;//convert to meters
report->quality = f_integral.qual; //0:bad ; 255 max quality
report->gyro_x_rate_integral = static_cast<float>(f_integral.gyro_x_rate_integral) / 10000.0f; //convert to radians
report->gyro_y_rate_integral = static_cast<float>(f_integral.gyro_y_rate_integral) / 10000.0f; //convert to radians
report->gyro_z_rate_integral = static_cast<float>(f_integral.gyro_z_rate_integral) / 10000.0f; //convert to radians
report->integration_timespan = f_integral.integration_timespan; //microseconds
report->time_since_last_sonar_update = f_integral.sonar_timestamp;//microseconds
report->gyro_temperature = f_integral.gyro_temperature;//Temperature * 100 in centi-degrees Celsius
report->sensor_id = 0;
hal.console->printf_P(PSTR("PX4FLOW measurement: ground_distance_m: %f\n"), report->ground_distance_m);
/*
// rotate measurements according to parameter
float zeroval = 0.0f;
rotate_3f(_sensor_rotation, report.pixel_flow_x_integral, report.pixel_flow_y_integral, zeroval);
*/
i2c_sem->give();
return 1;
}
// update - read latest values from sensor and fill in x,y and totals.
void AP_OpticalFlow_Linux::update(void)
{
struct optical_flow_s report;
// read the report from the sensor
read(&report);
// process
struct OpticalFlow::OpticalFlow_state state;
state.device_id = report.sensor_id;
state.surface_quality = report.quality;
if (report.integration_timespan > 0) {
const Vector2f flowScaler = _flowScaler();
float flowScaleFactorX = 1.0f + 0.001f * flowScaler.x;
float flowScaleFactorY = 1.0f + 0.001f * flowScaler.y;
float integralToRate = 1e6f / float(report.integration_timespan);
state.flowRate.x = flowScaleFactorX * integralToRate * float(report.pixel_flow_x_integral); // rad/sec measured optically about the X sensor axis
state.flowRate.y = flowScaleFactorY * integralToRate * float(report.pixel_flow_y_integral); // rad/sec measured optically about the Y sensor axis
state.bodyRate.x = integralToRate * float(report.gyro_x_rate_integral); // rad/sec measured inertially about the X sensor axis
state.bodyRate.y = integralToRate * float(report.gyro_y_rate_integral); // rad/sec measured inertially about the Y sensor axis
} else {
state.flowRate.zero();
state.bodyRate.zero();
}
#if DEBUG
hal.console->printf_P(PSTR("PX4FLOW print: sensor_id: %d\n"), state.device_id);
hal.console->printf_P(PSTR("PX4FLOW print: surface_quality: %d\n"), state.surface_quality);
hal.console->printf_P(PSTR("PX4FLOW print: flowRate.x: %d\n"), state.flowRate.x);
hal.console->printf_P(PSTR("PX4FLOW print: flowRate.y: %d\n"), state.flowRate.y);
hal.console->printf_P(PSTR("PX4FLOW print: bodyRate.x: %d\n"), state.bodyRate.x);
hal.console->printf_P(PSTR("PX4FLOW print: bodyRate.y: %d\n"), state.bodyRate.y);
#endif
_update_frontend(state);
}
#endif // CONFIG_HAL_BOARD == HAL_BOARD_LINUX

96
libraries/AP_OpticalFlow/AP_OpticalFlow_Linux.h
Unescape View File

@ -0,0 +1,96 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#ifndef AP_OpticalFlow_Linux_H
#define AP_OpticalFlow_Linux_H
#include "OpticalFlow.h"
#include <AP_Math.h>
/* Configuration Constants */
#define I2C_FLOW_ADDRESS 0x42 ///< 7-bit address. 8-bit address is 0x84, range 0x42 - 0x49
/* PX4FLOW Registers addresses */
#define PX4FLOW_REG 0x16 ///< Measure Register 22
#define PX4FLOW_CONVERSION_INTERVAL 100000 ///< in microseconds! 20000 = 50 Hz 100000 = 10Hz
#define PX4FLOW_I2C_MAX_BUS_SPEED 400000 ///< 400 KHz maximum speed
typedef struct i2c_frame
{
uint16_t frame_count;
int16_t pixel_flow_x_sum;
int16_t pixel_flow_y_sum;
int16_t flow_comp_m_x;
int16_t flow_comp_m_y;
int16_t qual;
int16_t gyro_x_rate;
int16_t gyro_y_rate;
int16_t gyro_z_rate;
uint8_t gyro_range;
uint8_t sonar_timestamp;
int16_t ground_distance;
} i2c_frame;
#define I2C_FRAME_SIZE (sizeof(i2c_frame))
typedef struct i2c_integral_frame
{
uint16_t frame_count_since_last_readout;
int16_t pixel_flow_x_integral;
int16_t pixel_flow_y_integral;
int16_t gyro_x_rate_integral;
int16_t gyro_y_rate_integral;
int16_t gyro_z_rate_integral;
uint32_t integration_timespan;
uint32_t sonar_timestamp;
uint16_t ground_distance;
int16_t gyro_temperature;
uint8_t qual;
} i2c_integral_frame;
#define I2C_INTEGRAL_FRAME_SIZE (sizeof(i2c_integral_frame))
/**
* Optical flow in NED body frame in SI units.
*
* @see http://en.wikipedia.org/wiki/International_System_of_Units
*/
struct optical_flow_s {
uint64_t timestamp; /**< in microseconds since system start */
uint8_t sensor_id; /**< id of the sensor emitting the flow value */
float pixel_flow_x_integral; /**< accumulated optical flow in radians around x axis */
float pixel_flow_y_integral; /**< accumulated optical flow in radians around y axis */
float gyro_x_rate_integral; /**< accumulated gyro value in radians around x axis */
float gyro_y_rate_integral; /**< accumulated gyro value in radians around y axis */
float gyro_z_rate_integral; /**< accumulated gyro value in radians around z axis */
float ground_distance_m; /**< Altitude / distance to ground in meters */
uint32_t integration_timespan; /**<accumulation timespan in microseconds */
uint32_t time_since_last_sonar_update;/**< time since last sonar update in microseconds */
uint16_t frame_count_since_last_readout;/**< number of accumulated frames in timespan */
int16_t gyro_temperature;/**< Temperature * 100 in centi-degrees Celsius */
uint8_t quality; /**< Average of quality of accumulated frames, 0: bad quality, 255: maximum quality */
};
class AP_OpticalFlow_Linux : public OpticalFlow_backend
{
public:
/// constructor
AP_OpticalFlow_Linux(OpticalFlow &_frontend);
// init - initialise the sensor
void init();
// update - read latest values from sensor and fill in x,y and totals.
void update(void);
private:
int read(optical_flow_s* report);
void print(optical_flow_s* report);
struct i2c_frame f;
struct i2c_integral_frame f_integral;
};
#endif

2
libraries/AP_OpticalFlow/OpticalFlow.cpp
Unescape View File

@ -46,6 +46,8 @@ OpticalFlow::OpticalFlow(void) :
backend(new AP_OpticalFlow_PX4(*this)), backend(new AP_OpticalFlow_PX4(*this)),
#elif CONFIG_HAL_BOARD == HAL_BOARD_SITL #elif CONFIG_HAL_BOARD == HAL_BOARD_SITL
backend(new AP_OpticalFlow_HIL(*this)), backend(new AP_OpticalFlow_HIL(*this)),
#elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX
backend(new AP_OpticalFlow_Linux(*this)),
#else #else
backend(NULL), backend(NULL),
#endif #endif

1
libraries/AP_OpticalFlow/OpticalFlow.h
Unescape View File

@ -96,5 +96,6 @@ private:
#include "OpticalFlow_backend.h" #include "OpticalFlow_backend.h"
#include "AP_OpticalFlow_HIL.h" #include "AP_OpticalFlow_HIL.h"
#include "AP_OpticalFlow_PX4.h" #include "AP_OpticalFlow_PX4.h"
#include "AP_OpticalFlow_Linux.h"
#endif #endif

Write Preview
Loading…
Cancel
Save