AP_InertialSensor: implement up to two sensors on PX4

libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp

@@ -17,6 +17,25 @@ const extern AP_HAL::HAL& hal;
 
 uint16_t AP_InertialSensor_PX4::_init_sensor( Sample_rate sample_rate ) 
 {
+    // assumes max 2 instances
+    _accel_fd[0] = open(ACCEL_DEVICE_PATH, O_RDONLY);
+    _accel_fd[1] = open(ACCEL_DEVICE_PATH "1", O_RDONLY);
+    _gyro_fd[0] = open(GYRO_DEVICE_PATH, O_RDONLY);
+    _gyro_fd[1] = open(GYRO_DEVICE_PATH "1", O_RDONLY);
+
+	if (_accel_fd[0] < 0) {
+        hal.scheduler->panic("Unable to open accel device " ACCEL_DEVICE_PATH);
+    }
+	if (_gyro_fd[0] < 0) {
+        hal.scheduler->panic("Unable to open gyro device " GYRO_DEVICE_PATH);
+    }
+    if (_accel_fd[1] >= 0) {
+        _num_accel_instances = 2;
+    }
+    if (_gyro_fd[1] >= 0) {
+        _num_gyro_instances = 2;
+    }
+
     switch (sample_rate) {
     case RATE_50HZ:
         _default_filter_hz = 15;
@@ -33,33 +52,6 @@ uint16_t AP_InertialSensor_PX4::_init_sensor( Sample_rate sample_rate )
         break;
     }
 
-    _delta_time = _sample_time_usec * 1.0e-6f;
-
-	// init accelerometers
-	_accel_fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
-	if (_accel_fd < 0) {
-        hal.scheduler->panic("Unable to open accel device " ACCEL_DEVICE_PATH);
-    }
-
-	_gyro_fd = open(GYRO_DEVICE_PATH, O_RDONLY);
-	if (_gyro_fd < 0) {
-        hal.scheduler->panic("Unable to open gyro device " GYRO_DEVICE_PATH);
-    }
-
-#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
-    uint32_t driver_rate = 1000;
-#else
-    uint32_t driver_rate = 800;
-#endif
-
-    /* 
-     * set the accel and gyro sampling rate. 
-     */
-    ioctl(_accel_fd, ACCELIOCSSAMPLERATE, driver_rate);
-    ioctl(_accel_fd, SENSORIOCSPOLLRATE,  driver_rate);
-    ioctl(_gyro_fd,  GYROIOCSSAMPLERATE,  driver_rate);
-    ioctl(_gyro_fd,  SENSORIOCSPOLLRATE,  driver_rate);
-
     _set_filter_frequency(_mpu6000_filter);
 
 #if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
@@ -77,33 +69,103 @@ void AP_InertialSensor_PX4::_set_filter_frequency(uint8_t filter_hz)
     if (filter_hz == 0) {
         filter_hz = _default_filter_hz;
     }
-    ioctl(_gyro_fd,  GYROIOCSLOWPASS,  filter_hz);
+    for (uint8_t i=0; i<PX4_MAX_INS_INSTANCES; i++) {
-    ioctl(_accel_fd, ACCELIOCSLOWPASS, filter_hz);
+        ioctl(_gyro_fd[i],  GYROIOCSLOWPASS,  filter_hz);
+        ioctl(_accel_fd[i], ACCELIOCSLOWPASS, filter_hz);
+    }
 }
 
 /*================ AP_INERTIALSENSOR PUBLIC INTERFACE ==================== */
 
+// multi-device interface
+bool AP_InertialSensor_PX4::get_gyro_instance_health(uint8_t instance) const
+{
+    if (instance >= _num_gyro_instances) {
+        return false;
+    }
+    if (_sample_time_usec == 0) {
+        // not initialised yet, show as healthy to prevent scary GCS
+        // warnings
+        return true;
+    }
+    uint64_t tnow = hrt_absolute_time();
+
+    if ((tnow - _last_gyro_timestamp[instance]) > 2*_sample_time_usec) {
+        // gyros have not updated
+        return false;
+    }
+    return true;
+}
+
+uint8_t AP_InertialSensor_PX4::get_gyro_count(void) const
+{
+    return _num_gyro_instances;
+}
+
+bool AP_InertialSensor_PX4::get_gyro_instance(uint8_t instance, Vector3f &gyro) const
+{
+    if (instance >= _num_gyro_instances) {
+        return false;
+    }
+    gyro = _gyro_in[instance];
+    gyro.rotate(_board_orientation);
+    gyro -= _gyro_offset;
+    return true;
+}
+
+bool AP_InertialSensor_PX4::get_accel_instance_health(uint8_t instance) const
+{
+    if (instance >= _num_accel_instances) {
+        return false;
+    }
+    if (_sample_time_usec == 0) {
+        // not initialised yet, show as healthy to prevent scary GCS
+        // warnings
+        return true;
+    }
+    uint64_t tnow = hrt_absolute_time();
+
+    if ((tnow - _last_accel_timestamp[instance]) > 2*_sample_time_usec) {
+        // accels have not updated
+        return false;
+    }
+    if (fabsf(_accel.x) > 30 && fabsf(_accel.y) > 30 && fabsf(_accel.z) > 30 &&
+        (_previous_accels[instance] - _accel_in[instance]).length() < 0.01f) {
+        // unchanging accel, large in all 3 axes. This is a likely
+        // accelerometer failure of the LSM303d
+        return false;
+    }
+    return true;
+    
+}
+uint8_t AP_InertialSensor_PX4::get_accel_count(void) const
+{
+    return _num_accel_instances;
+}
+
+bool AP_InertialSensor_PX4::get_accel_instance(uint8_t instance, Vector3f &accel) const
+{
+    if (instance >= _num_accel_instances) {
+        return false;
+    }
+    accel = _accel_in[instance];
+    accel.rotate(_board_orientation);
+    accel.x *= _accel_scale.get().x;
+    accel.y *= _accel_scale.get().y;
+    accel.z *= _accel_scale.get().z;
+    accel   -= _accel_offset;
+    return true;    
+}
+
 bool AP_InertialSensor_PX4::update(void) 
 {
-    Vector3f accel_scale = _accel_scale.get();
-
     // get the latest sample from the sensor drivers
     _get_sample();
 
     _previous_accel = _accel;
 
-    _accel = _accel_in;
+    get_accel_instance(0, _accel);
-    _gyro  = _gyro_in;
+    get_gyro_instance(0, _gyro);
-
-    // add offsets and rotation
-    _accel.rotate(_board_orientation);
-    _accel.x *= accel_scale.x;
-    _accel.y *= accel_scale.y;
-    _accel.z *= accel_scale.z;
-    _accel   -= _accel_offset;
-
-    _gyro.rotate(_board_orientation);
-    _gyro -= _gyro_offset;
 
     if (_last_filter_hz != _mpu6000_filter) {
         _set_filter_frequency(_mpu6000_filter);
@@ -117,38 +179,39 @@ bool AP_InertialSensor_PX4::update(void)
 
 float AP_InertialSensor_PX4::get_delta_time(void)
 {
-    return _delta_time;
+    return _sample_time_usec * 1.0e-6f;
 }
 
 float AP_InertialSensor_PX4::get_gyro_drift_rate(void) 
 {
-    // 0.5 degrees/second/minute
+    // assume 0.5 degrees/second/minute
     return ToRad(0.5/60);
 }
 
 void AP_InertialSensor_PX4::_get_sample(void)
 {
-    struct accel_report	accel_report;
+    for (uint8_t i=0; i<PX4_MAX_INS_INSTANCES; i++) {
-    struct gyro_report	gyro_report;
+        struct accel_report	accel_report;
-
+        while (_accel_fd[i] != -1 && 
-    if (_accel_fd == -1 || _gyro_fd == -1) {
+               ::read(_accel_fd[i], &accel_report, sizeof(accel_report)) == sizeof(accel_report) &&
-        return;
+               accel_report.timestamp != _last_accel_timestamp[i]) {        
+            _previous_accels[i] = _accel_in[i];
+            _accel_in[i] = Vector3f(accel_report.x, accel_report.y, accel_report.z);
+            _last_accel_timestamp[i] = accel_report.timestamp;
+        }
+    }
+    for (uint8_t i=0; i<PX4_MAX_INS_INSTANCES; i++) {
+        struct gyro_report	gyro_report;
+        while (_gyro_fd[i] != -1 && 
+               ::read(_gyro_fd[i], &gyro_report, sizeof(gyro_report)) == sizeof(gyro_report) &&
+               gyro_report.timestamp != _last_gyro_timestamp[i]) {        
+            _gyro_in[i] = Vector3f(gyro_report.x, gyro_report.y, gyro_report.z);
+            _last_gyro_timestamp[i] = gyro_report.timestamp;
+        }
     }
-
-    while (::read(_accel_fd, &accel_report, sizeof(accel_report)) == sizeof(accel_report) &&
-        accel_report.timestamp != _last_accel_timestamp) {        
-        _accel_in = Vector3f(accel_report.x, accel_report.y, accel_report.z);
-        _last_accel_timestamp = accel_report.timestamp;
-	}
-
-    while (::read(_gyro_fd, &gyro_report, sizeof(gyro_report)) == sizeof(gyro_report) &&
-        gyro_report.timestamp != _last_gyro_timestamp) {        
-        _gyro_in = Vector3f(gyro_report.x, gyro_report.y, gyro_report.z);
-        _last_gyro_timestamp = gyro_report.timestamp;
-	}
 }
 
-bool AP_InertialSensor_PX4::sample_available(void)
+bool AP_InertialSensor_PX4::_sample_available(void)
 {
     uint64_t tnow = hrt_absolute_time();
     while (tnow - _last_sample_timestamp > _sample_time_usec) {
@@ -160,7 +223,7 @@ bool AP_InertialSensor_PX4::sample_available(void)
 
 bool AP_InertialSensor_PX4::wait_for_sample(uint16_t timeout_ms)
 {
-    if (sample_available()) {
+    if (_sample_available()) {
         return true;
     }
     uint32_t start = hal.scheduler->millis();
@@ -172,7 +235,7 @@ bool AP_InertialSensor_PX4::wait_for_sample(uint16_t timeout_ms)
         if (_last_sample_timestamp + _sample_time_usec > tnow+timing_lag) {
             hal.scheduler->delay_microseconds(_last_sample_timestamp + _sample_time_usec - (tnow+timing_lag));
         }
-        if (sample_available()) {
+        if (_sample_available()) {
             return true;
         }
     }
@@ -182,37 +245,9 @@ bool AP_InertialSensor_PX4::wait_for_sample(uint16_t timeout_ms)
 /**
    try to detect bad accel/gyro sensors
  */
-bool AP_InertialSensor_PX4::healthy(void)
+bool AP_InertialSensor_PX4::healthy(void) const
 {
-    if (_sample_time_usec == 0) {
+    return get_gyro_instance_health(0) && get_accel_instance_health(0);
-        // not initialised yet, show as healthy to prevent scary GCS
-        // warnings
-        return true;
-    }
-    uint64_t tnow = hrt_absolute_time();
-
-    if ((tnow - _last_accel_timestamp) > 2*_sample_time_usec ||
-        (tnow - _last_gyro_timestamp) > 2*_sample_time_usec) {
-        // see if new samples are available
-        _get_sample();
-        tnow = hrt_absolute_time();
-    }
-
-    if ((tnow - _last_accel_timestamp) > 2*_sample_time_usec) {
-        // accels have not updated
-        return false;
-    }
-    if ((tnow - _last_gyro_timestamp) > 2*_sample_time_usec) {
-        // gyros have not updated
-        return false;
-    }
-    if (fabsf(_accel.x) > 30 && fabsf(_accel.y) > 30 && fabsf(_accel.z) > 30 &&
-        (_previous_accel - _accel).length() < 0.01f) {
-        // unchanging accel, large in all 3 axes. This is a likely
-        // accelerometer failure of the LSM303d
-        return false;
-    }
-    return true;
 }
 
 #endif // CONFIG_HAL_BOARD

libraries/AP_InertialSensor/AP_InertialSensor_PX4.h

@@ -13,37 +13,45 @@
 #include <uORB/uORB.h>
 #include <uORB/topics/sensor_combined.h>
 
+#define PX4_MAX_INS_INSTANCES 2
+
 class AP_InertialSensor_PX4 : public AP_InertialSensor
 {
 public:
 
     AP_InertialSensor_PX4() : 
         AP_InertialSensor(),
-        _sample_time_usec(0),
+        _sample_time_usec(0)
-        _accel_fd(-1),
+        {
-        _gyro_fd(-1)
+        }
-        {}
 
     /* Concrete implementation of AP_InertialSensor functions: */
     bool            update();
     float        	get_delta_time();
     float           get_gyro_drift_rate();
-    bool            sample_available();
     bool            wait_for_sample(uint16_t timeout_ms);
-    bool            healthy(void);
+    bool            healthy(void) const;
+
+    // multi-device interface
+    bool get_gyro_instance_health(uint8_t instance) const;
+    uint8_t get_gyro_count(void) const;
+    bool get_gyro_instance(uint8_t instance, Vector3f &gyro) const;
+
+    bool get_accel_instance_health(uint8_t instance) const;
+    uint8_t get_accel_count(void) const;
+    bool get_accel_instance(uint8_t instance, Vector3f &accel) const;
 
 private:
     uint16_t _init_sensor( Sample_rate sample_rate );
     void     _get_sample(void);
-    uint64_t _last_update_usec;
+    bool     _sample_available(void);
-    float    _delta_time;
+    Vector3f _accel_in[PX4_MAX_INS_INSTANCES];
-    Vector3f _accel_in;
+    Vector3f _gyro_in[PX4_MAX_INS_INSTANCES];
-    Vector3f _gyro_in;
+    uint64_t _last_accel_timestamp[PX4_MAX_INS_INSTANCES];
-    uint64_t _last_accel_timestamp;
+    uint64_t _last_gyro_timestamp[PX4_MAX_INS_INSTANCES];
-    uint64_t _last_gyro_timestamp;
     uint64_t _last_sample_timestamp;
-    bool     _have_sample_available;
     uint32_t _sample_time_usec;
+    bool     _have_sample_available;
 
     // support for updating filter at runtime
     uint8_t _last_filter_hz;
@@ -51,9 +59,16 @@ private:
 
     void _set_filter_frequency(uint8_t filter_hz);
 
+    Vector3f _accel_data[PX4_MAX_INS_INSTANCES];
+    Vector3f _gyro_data[PX4_MAX_INS_INSTANCES];
+
+    Vector3f _previous_accels[PX4_MAX_INS_INSTANCES];
+
     // accelerometer and gyro driver handles
-    int _accel_fd;
+    uint8_t _num_accel_instances;
-    int _gyro_fd;
+    uint8_t _num_gyro_instances;
+    int _accel_fd[PX4_MAX_INS_INSTANCES];
+    int _gyro_fd[PX4_MAX_INS_INSTANCES];
 };
 #endif
 #endif // __AP_INERTIAL_SENSOR_PX4_H__