OpticalFlow_Linux: reworked driver

remove PANICs from init
return semaphore if init fails
add successful initialisation check before attempting to read from sensor
structure made private where possible
formatting fixes
check I2C reads succeed
add request_measurement to request sensor to produce measurement
quit after 20 of previous 40 reads fail
throttle reads to 10hz max

libraries/AP_OpticalFlow/AP_OpticalFlow_Linux.cpp

@@ -25,145 +25,146 @@
 #include <AP_HAL.h>
 #include "OpticalFlow.h"
 
-#define DEBUG 1
+#define PX4FLOW_DEBUG 1
-#define RAW_READ 0
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
+
+#define PX4FLOW_I2C_ADDRESS     0x42    // 7-bit address. 8-bit address is 0x84, range 0x42 - 0x49
+#define PX4FLOW_REG             0x16    // Measure Register 22
+#define I2C_FRAME_SIZE          (sizeof(i2c_frame))
+#define I2C_INTEGRAL_FRAME_SIZE (sizeof(i2c_integral_frame))
+
 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];
+    // only initialise once
+    if (initialised) {
+        return;
+    }
 
     // get pointer to i2c bus semaphore
     AP_HAL::Semaphore *i2c_sem = hal.i2c->get_semaphore();
+    if (i2c_sem == NULL) {
+        return;
+    }
 
     // take i2c bus sempahore
-  if (!i2c_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
+    if (!i2c_sem->take(200)) {
-      hal.scheduler->panic(PSTR("PX4FLOW: unable to get semaphore"));
+        return;
     }
 
-  // to be sure this is not a ll40ls Lidar (which can also be on
+    // read from flow sensor to ensure it is not a ll40ls Lidar (which can be on 0x42)
-  // 0x42) we check if a I2C_FRAME_SIZE byte transfer works from address
+    // read I2C_FRAME_SIZE bytes, the ll40ls will error whereas the flow happily returns data
-  // 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))
+    if (hal.i2c->readRegisters(PX4FLOW_I2C_ADDRESS, 0, I2C_FRAME_SIZE, val) != 0) {
-    hal.scheduler->panic(PSTR("ll40ls Lidar"));
-
         i2c_sem->give();
-
+        return;
     }
 
-int AP_OpticalFlow_Linux::read(optical_flow_s* report)
+    // success
+    initialised = true;
+    i2c_sem->give();
+}
+
+bool AP_OpticalFlow_Linux::request_measurement()
+{
+    // send measure request to sensor
+    uint8_t cmd = PX4FLOW_REG;
+    if (hal.i2c->writeRegisters(PX4FLOW_I2C_ADDRESS, cmd, 0, nullptr) != 0) {
+        return false;
+    }
+    return true;
+}
+
+bool AP_OpticalFlow_Linux::read(optical_flow_s* report)
 {
     // get pointer to i2c bus semaphore
     AP_HAL::Semaphore *i2c_sem = hal.i2c->get_semaphore();
-
+    if (i2c_sem == NULL) {
-  // take i2c bus sempahore
+        num_errors++;
-  if (!i2c_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
+        return false;
-      hal.scheduler->panic(PSTR("PX4FLOW: unable to get semaphore"));
     }
 
-
+    // take i2c bus sempahore (non blocking)
-  uint8_t val[I2C_FRAME_SIZE + I2C_INTEGRAL_FRAME_SIZE] = { 0 };
+    if (!i2c_sem->take_nonblocking()) {
-
+        num_errors++;
-#if RAW_READ
+        return false;
-  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
+    // request measurement
-  if (PX4FLOW_REG == 0x00) {
+    request_measurement();
-    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) {
+    uint8_t val[I2C_FRAME_SIZE + I2C_INTEGRAL_FRAME_SIZE] = {};
-    if (hal.i2c->read(I2C_FLOW_ADDRESS, I2C_INTEGRAL_FRAME_SIZE, val)){
+    i2c_integral_frame f_integral;
-      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)){
+        if (hal.i2c->readRegisters(PX4FLOW_I2C_ADDRESS, 0, I2C_FRAME_SIZE + I2C_INTEGRAL_FRAME_SIZE, val) != 0) {
-      hal.console->printf_P(PSTR("PX4FLOW: Error while reading"));
+            num_errors++;
             i2c_sem->give();
-      return 0;
+            return false;
         }
-  }
-
-  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) {
+        if (hal.i2c->readRegisters(PX4FLOW_I2C_ADDRESS, 0, I2C_INTEGRAL_FRAME_SIZE, val) != 0) {
+            num_errors++;
+            i2c_sem->give();
+            return false;
+        }
         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->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;
 
+    // reduce error count
+    if (num_errors > 0) {
+        num_errors--;
+    }
+
+    return true;
 }
 
 // update - read latest values from sensor and fill in x,y and totals.
 void AP_OpticalFlow_Linux::update(void)
 {
-    struct optical_flow_s report;
+    optical_flow_s report;
+
+    // return immediately if not initialised or more than half of last 40 reads have failed
+    if (!initialised || num_errors >= 20) {
+        return;
+    }
+
+    // throttle reads to no more than 10hz
+    uint32_t now = hal.scheduler->millis();
+    if (now - last_read_ms < 100) {
+        return;
+    }
+    last_read_ms = now;
 
     // read the report from the sensor
-    read(&report);
+    if (!read(&report)) {
+        return;
+    }
 
     // process
     struct OpticalFlow::OpticalFlow_state state;
@@ -183,17 +184,18 @@ void AP_OpticalFlow_Linux::update(void)
         state.bodyRate.zero();
     }
 
-#if DEBUG
+    // copy results to front end
-    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);
+
+#if PX4FLOW_DEBUG
+    hal.console->printf_P(PSTR("PX4FLOW id:%u qual:%u FlowRateX:%4.2f Y:%4.2f BodyRateX:%4.2f y:%4.2f\n"),
+            (unsigned)state.device_id,
+            (unsigned)state.surface_quality,
+            (double)state.flowRate.x,
+            (double)state.flowRate.y,
+            (double)state.bodyRate.x,
+            (double)state.bodyRate.y);
+#endif
 }
 
-
-
 #endif // CONFIG_HAL_BOARD == HAL_BOARD_LINUX

libraries/AP_OpticalFlow/AP_OpticalFlow_Linux.h

@@ -1,23 +1,31 @@
 /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
+/*
+ * Portions of this driver were borrowed from the PX4Firmware px4flow driver which can be found here:
+ *     https://github.com/PX4/Firmware/blob/master/src/drivers/px4flow/px4flow.cpp
+ */
+
 #ifndef AP_OpticalFlow_Linux_H
 #define AP_OpticalFlow_Linux_H
 
 #include "OpticalFlow.h"
-
 #include <AP_Math.h>
 
-/* Configuration Constants */
+class AP_OpticalFlow_Linux : public OpticalFlow_backend
-#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
 {
+public:
+    // constructor
+    AP_OpticalFlow_Linux(OpticalFlow &_frontend);
+
+    // initialise the sensor
+    void init();
+
+    // read latest values from sensor and fill in x,y and totals.
+    void update(void);
+
+private:
+
+    typedef struct {
         uint16_t frame_count;
         int16_t pixel_flow_x_sum;
         int16_t pixel_flow_y_sum;
@@ -32,11 +40,7 @@ typedef  struct i2c_frame
         int16_t ground_distance;
     } i2c_frame;
 
-#define I2C_FRAME_SIZE (sizeof(i2c_frame))
+    typedef struct {
-
-
-typedef struct i2c_integral_frame
-{
         uint16_t frame_count_since_last_readout;
         int16_t pixel_flow_x_integral;
         int16_t pixel_flow_y_integral;
@@ -50,47 +54,31 @@ typedef struct i2c_integral_frame
         uint8_t qual;
     } i2c_integral_frame;
 
-#define I2C_INTEGRAL_FRAME_SIZE (sizeof(i2c_integral_frame))
+    typedef struct {
+        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 time span 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 time span
+        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
+    } optical_flow_s;
 
-/**
+    // request the sensor produce a measurement, returns true on success
- * Optical flow in NED body frame in SI units.
+    bool request_measurement();
- *
- * @see http://en.wikipedia.org/wiki/International_System_of_Units
- */
-struct optical_flow_s {
 
-	uint64_t timestamp;		/**< in microseconds since system start  */
+    // read from sensor, returns true if successful
-	uint8_t sensor_id;		/**< id of the sensor emitting the flow value */
+    bool read(optical_flow_s* report);
-	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
+    bool initialised = false;
-{
+    uint16_t num_errors = 0;
-public:
+    uint32_t last_read_ms = 0;
-    /// 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