ChibiOS: support FMUv5 boards

Tools/scripts/decode_devid.py

@@ -58,7 +58,10 @@
     0x21 : "DEVTYPE_GYR_MPU6000",
     0x22 : "DEVTYPE_GYR_L3GD20",
     0x24 : "DEVTYPE_GYR_MPU9250",
-    0x25 : "DEVTYPE_GYR_I3G4250D"
+    0x25 : "DEVTYPE_GYR_I3G4250D",
+    0x26 : "DEVTYPE_GYR_LSM9DS1",
+    0x27 : "DEVTYPE_INS_ICM20789",
+    0x28 : "DEVTYPE_INS_ICM20689",
 }
 
 decoded_devname = ""

libraries/AP_BLHeli/AP_BLHeli.h

@@ -229,10 +229,4 @@ class AP_BLHeli {
     bool protocol_handler(uint8_t , AP_HAL::UARTDriver *);
 };
 
-
-// start of 12 byte CPU ID
-#ifndef UDID_START
-#define UDID_START	0x1FFF7A10
-#endif
-
 #endif // HAL_SUPPORT_RCOUT_SERIAL

libraries/AP_Baro/AP_Baro.cpp

@@ -494,6 +494,11 @@ void AP_Baro::init(void)
                                             std::move(hal.i2c_mgr->get_device(1, 0x63)),
                                             std::move(hal.spi->get_device(HAL_INS_MPU60x0_NAME))));
         break;
+
+    case AP_BoardConfig::PX4_BOARD_FMUV5:
+        ADD_BACKEND(AP_Baro_MS56XX::probe(*this,
+                                          std::move(hal.spi->get_device(HAL_BARO_MS5611_NAME))));
+        break;
 
     default:
         break;

libraries/AP_BoardConfig/AP_BoardConfig.h

@@ -8,13 +8,13 @@
 #include <AP_Param_Helper/AP_Param_Helper.h>
 #endif
 
-#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN || defined(HAL_CHIBIOS_ARCH_FMUV3) || defined(HAL_CHIBIOS_ARCH_FMUV4) || defined(HAL_CHIBIOS_ARCH_MINDPXV2)
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN || defined(HAL_CHIBIOS_ARCH_FMUV3) || defined(HAL_CHIBIOS_ARCH_FMUV4) || defined(HAL_CHIBIOS_ARCH_FMUV5) || defined(HAL_CHIBIOS_ARCH_MINDPXV2)
 #define AP_FEATURE_BOARD_DETECT 1
 #else
 #define AP_FEATURE_BOARD_DETECT 0
 #endif
 
-#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN || defined(HAL_CHIBIOS_ARCH_FMUV3) || defined(HAL_CHIBIOS_ARCH_FMUV4) || defined(HAL_CHIBIOS_ARCH_MINDPXV2) || defined(HAL_GPIO_PIN_SAFETY_IN)
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN || defined(HAL_CHIBIOS_ARCH_FMUV3) || defined(HAL_CHIBIOS_ARCH_FMUV4) || defined(HAL_CHIBIOS_ARCH_FMUV5) || defined(HAL_CHIBIOS_ARCH_MINDPXV2) || defined(HAL_GPIO_PIN_SAFETY_IN)
 #define AP_FEATURE_SAFETY_BUTTON 1
 #else
 #define AP_FEATURE_SAFETY_BUTTON 0
@@ -90,6 +90,7 @@ class AP_BoardConfig {
         PX4_BOARD_PCNC1    = 21,
         PX4_BOARD_MINDPXV2 = 22,
         PX4_BOARD_SP01     = 23,
+        PX4_BOARD_FMUV5    = 24,
         VRX_BOARD_BRAIN51  = 30,
         VRX_BOARD_BRAIN52  = 32,
         VRX_BOARD_BRAIN52E = 33,
@@ -111,12 +112,14 @@ class AP_BoardConfig {
     }
 #endif
 
-    // ask if IOMCU is enabled
-    static bool io_enabled(void) {
+    // ask if IOMCU is enabled. This is a uint8_t to allow
+    // developer debugging by setting BRD_IO_ENABLE=100 to avoid the
+    // crc check of IO firmware on startup
+    static uint8_t io_enabled(void) {
 #if AP_FEATURE_BOARD_DETECT
-        return instance?instance->state.io_enable.get():false;
+        return instance?uint8_t(instance->state.io_enable.get()):0;
 #else
-        return false;
+        return 0;
 #endif
     }
 

libraries/AP_BoardConfig/board_drivers.cpp

@@ -88,6 +88,7 @@ void AP_BoardConfig::board_setup_drivers(void)
     case PX4_BOARD_PX4V1:
     case PX4_BOARD_PIXHAWK:
     case PX4_BOARD_PIXHAWK2:
+    case PX4_BOARD_FMUV5:
     case PX4_BOARD_SP01:
     case PX4_BOARD_PIXRACER:
     case PX4_BOARD_PHMINI:
@@ -239,6 +240,9 @@ void AP_BoardConfig::board_autodetect(void)
 #elif defined(CONFIG_ARCH_BOARD_AEROFC_V1)
     state.board_type.set_and_notify(PX4_BOARD_AEROFC);
     hal.console->printf("Detected Aero FC\n");
+#elif defined(HAL_CHIBIOS_ARCH_FMUV5)
+    state.board_type.set_and_notify(PX4_BOARD_FMUV5);
+    hal.console->printf("Detected FMUv5\n");
 #elif defined(CONFIG_ARCH_BOARD_VRBRAIN_V51)
     state.board_type.set_and_notify(VRX_BOARD_BRAIN51);
     hal.console->printf("Detected VR Brain 5.1\n");

libraries/AP_Compass/AP_Compass.cpp

@@ -592,6 +592,7 @@ void Compass::_detect_backends(void)
     case AP_BoardConfig::PX4_BOARD_PIXHAWK2:
     case AP_BoardConfig::PX4_BOARD_PIXRACER: 
     case AP_BoardConfig::PX4_BOARD_MINDPXV2: 
+    case AP_BoardConfig::PX4_BOARD_FMUV5:
     case AP_BoardConfig::PX4_BOARD_PIXHAWK_PRO:{
         bool both_i2c_external = (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PIXHAWK2);
         // external i2c bus
@@ -665,8 +666,10 @@ void Compass::_detect_backends(void)
         ADD_BACKEND(DRIVER_IST8310, AP_Compass_IST8310::probe(*this, hal.i2c_mgr->get_device(1, HAL_COMPASS_IST8310_I2C_ADDR),
                                                               true, ROTATION_PITCH_180), AP_Compass_IST8310::name, true);
 
-        ADD_BACKEND(DRIVER_IST8310, AP_Compass_IST8310::probe(*this, hal.i2c_mgr->get_device(0, HAL_COMPASS_IST8310_I2C_ADDR),
-                                                              both_i2c_external, ROTATION_PITCH_180), AP_Compass_IST8310::name, both_i2c_external);
+        if (AP_BoardConfig::get_board_type() != AP_BoardConfig::PX4_BOARD_FMUV5) {
+            ADD_BACKEND(DRIVER_IST8310, AP_Compass_IST8310::probe(*this, hal.i2c_mgr->get_device(0, HAL_COMPASS_IST8310_I2C_ADDR),
+                                                                  both_i2c_external, ROTATION_PITCH_180), AP_Compass_IST8310::name, both_i2c_external);
+        }
 #endif // HAL_MINIMIZE_FEATURES
         }
         break;
@@ -730,6 +733,11 @@ void Compass::_detect_backends(void)
                     AP_Compass_AK8963::name, false);
         break;
 
+    case AP_BoardConfig::PX4_BOARD_FMUV5:
+        ADD_BACKEND(DRIVER_IST8310, AP_Compass_IST8310::probe(*this, hal.i2c_mgr->get_device(0, HAL_COMPASS_IST8310_I2C_ADDR),
+                                                              false, ROTATION_ROLL_180_YAW_90), AP_Compass_IST8310::name, false);
+        break;
+
     case AP_BoardConfig::PX4_BOARD_SP01:
         ADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(*this, 1, ROTATION_NONE),
                     AP_Compass_AK8963::name, false);

libraries/AP_GPS/AP_GPS_UBLOX.cpp

@@ -1013,6 +1013,7 @@ AP_GPS_UBLOX::_parse_gps(void)
         state.time_week_ms = AP_HAL::millis() + 3*60*60*1000 + 37000;
         state.last_gps_time_ms = AP_HAL::millis();
         state.hdop = 130;
+        state.speed_accuracy = 0;
         next_fix = state.status;
 #endif
         break;

libraries/AP_HAL/AP_HAL_Boards.h

@@ -72,7 +72,8 @@
 #define HAL_BOARD_SUBTYPE_CHIBIOS_GENERIC       5009
 #define HAL_BOARD_SUBTYPE_CHIBIOS_F4BY          5010
 #define HAL_BOARD_SUBTYPE_CHIBIOS_OMNIBUSF4PRO  5011
-#define HAL_BOARD_SUBTYPE_CHIBIOS_AIRBOTF4  5012
+#define HAL_BOARD_SUBTYPE_CHIBIOS_AIRBOTF4      5012
+#define HAL_BOARD_SUBTYPE_CHIBIOS_FMUV5         5013
 
 /* InertialSensor driver types */
 #define HAL_INS_NONE         0

libraries/AP_HAL_ChibiOS/AnalogIn.cpp

@@ -27,6 +27,8 @@
 extern AP_IOMCU iomcu;
 #endif
 
+#include "hwdef/common/stm32_util.h"
+
 #ifndef CHIBIOS_ADC_MAVLINK_DEBUG
 // this allows the first 6 analog channels to be reported by mavlink for debugging purposes
 #define CHIBIOS_ADC_MAVLINK_DEBUG 0
@@ -58,7 +60,7 @@ const AnalogIn::pin_info AnalogIn::pin_config[] = HAL_ANALOG_PINS;
 #define ADC_GRP1_NUM_CHANNELS   ARRAY_SIZE_SIMPLE(AnalogIn::pin_config)
 
 // samples filled in by ADC DMA engine
-adcsample_t AnalogIn::samples[ADC_DMA_BUF_DEPTH*ADC_GRP1_NUM_CHANNELS];
+adcsample_t *AnalogIn::samples;
 uint32_t AnalogIn::sample_sum[ADC_GRP1_NUM_CHANNELS];
 uint32_t AnalogIn::sample_count;
 
@@ -201,6 +203,9 @@ void AnalogIn::init()
     if (ADC_GRP1_NUM_CHANNELS == 0) {
         return;
     }
+
+    samples = (adcsample_t *)hal.util->malloc_type(sizeof(adcsample_t)*ADC_DMA_BUF_DEPTH*ADC_GRP1_NUM_CHANNELS, AP_HAL::Util::MEM_DMA_SAFE);
+
     adcStart(&ADCD1, NULL);
     memset(&adcgrpcfg, 0, sizeof(adcgrpcfg));
     adcgrpcfg.circular = true;
@@ -297,7 +302,11 @@ void AnalogIn::_timer_tick(void)
     if (AP_HAL::millis() > 5000 && count++ == 10) {
         count = 0;
         uint16_t adc[6] {};
-        for (uint8_t i=0; i < ADC_GRP1_NUM_CHANNELS; i++) {
+        uint8_t n = ADC_GRP1_NUM_CHANNELS;
+        if (n > 6) {
+            n = 6;
+        }
+        for (uint8_t i=0; i < n; i++) {
             adc[i] = buf_adc[i];
         }
         mavlink_msg_ap_adc_send(MAVLINK_COMM_0, adc[0], adc[1], adc[2], adc[3], adc[4], adc[5]);

libraries/AP_HAL_ChibiOS/AnalogIn.h

@@ -88,7 +88,7 @@ class ChibiOS::AnalogIn : public AP_HAL::AnalogIn {
     }; 
     static const pin_info pin_config[];
 
-    static adcsample_t samples[];
+    static adcsample_t *samples;
     static uint32_t sample_sum[];
     static uint32_t sample_count;
 };

libraries/AP_HAL_ChibiOS/Device.cpp

@@ -20,13 +20,21 @@
 #include "Scheduler.h"
 #include "Semaphores.h"
 #include "Util.h"
+#include "hwdef/common/stm32_util.h"
 
 #if HAL_USE_I2C == TRUE || HAL_USE_SPI == TRUE
 
 using namespace ChibiOS;
 
 static const AP_HAL::HAL &hal = AP_HAL::get_HAL();
 
+DeviceBus::DeviceBus(uint8_t _thread_priority) :
+        thread_priority(_thread_priority)
+{
+    bouncebuffer_init(&bounce_buffer_tx);
+    bouncebuffer_init(&bounce_buffer_rx);
+}
+
 /*
   per-bus callback thread
 */
@@ -150,53 +158,25 @@ bool DeviceBus::adjust_timer(AP_HAL::Device::PeriodicHandle h, uint32_t period_u
 void DeviceBus::bouncebuffer_setup(const uint8_t *&buf_tx, uint16_t tx_len,
                                    uint8_t *&buf_rx, uint16_t rx_len)
 {
-    bouncebuffer_setup_tx(buf_tx, tx_len);
-    bouncebuffer_setup_rx(buf_rx, rx_len);
-}
-
-void DeviceBus::bouncebuffer_setup_tx(const uint8_t *&buf_tx, uint16_t tx_len)
-{
-    if (buf_tx && !IS_DMA_SAFE(buf_tx)) {
-        if (tx_len > bounce_buffer_tx_size) {
-            if (bounce_buffer_tx_size) {
-                hal.util->free_type(bounce_buffer_tx, bounce_buffer_tx_size, AP_HAL::Util::MEM_DMA_SAFE);
-                bounce_buffer_tx_size = 0;
-            }
-            bounce_buffer_tx = (uint8_t *)hal.util->malloc_type(tx_len, AP_HAL::Util::MEM_DMA_SAFE);
-            if (bounce_buffer_tx == nullptr) {
-                AP_HAL::panic("Out of memory for DMA TX");
-            }
-            bounce_buffer_tx_size = tx_len;
-        }
-        memcpy(bounce_buffer_tx, buf_tx, tx_len);
-        buf_tx = bounce_buffer_tx;
+    if (buf_rx) {
+        bouncebuffer_setup_read(bounce_buffer_rx, &buf_rx, rx_len);
     }
-}
-
-void DeviceBus::bouncebuffer_setup_rx(uint8_t *&buf_rx, uint16_t rx_len)
-{
-    if (buf_rx && !IS_DMA_SAFE(buf_rx)) {
-        if (rx_len > bounce_buffer_rx_size) {
-            if (bounce_buffer_rx_size) {
-                hal.util->free_type(bounce_buffer_rx, bounce_buffer_rx_size, AP_HAL::Util::MEM_DMA_SAFE);
-                bounce_buffer_rx_size = 0;
-            }
-            bounce_buffer_rx = (uint8_t *)hal.util->malloc_type(rx_len, AP_HAL::Util::MEM_DMA_SAFE);
-            if (bounce_buffer_rx == nullptr) {
-                AP_HAL::panic("Out of memory for DMA RX");
-            }
-            bounce_buffer_rx_size = rx_len;
-        }
-        buf_rx = bounce_buffer_rx;
+    if (buf_tx) {
+        bouncebuffer_setup_write(bounce_buffer_tx, &buf_tx, tx_len);
     }
 }
 
 /*
   complete a transfer using DMA bounce buffer
  */
-void DeviceBus::bouncebuffer_rx_copy(uint8_t *buf_rx, uint16_t rx_len)
+void DeviceBus::bouncebuffer_finish(const uint8_t *buf_tx, uint8_t *buf_rx, uint16_t rx_len)
 {
-    memcpy(buf_rx, bounce_buffer_rx, rx_len);
+    if (buf_rx) {
+        bouncebuffer_finish_read(bounce_buffer_rx, buf_rx, rx_len);
+    }
+    if (buf_tx) {
+        bouncebuffer_finish_write(bounce_buffer_tx, buf_tx);
+    }
 }
 
 #endif // HAL_USE_I2C || HAL_USE_SPI

libraries/AP_HAL_ChibiOS/Device.h

@@ -19,13 +19,13 @@
 #include "Semaphores.h"
 #include "Scheduler.h"
 #include "shared_dma.h"
+#include "hwdef/common/bouncebuffer.h"
 
 namespace ChibiOS {
 
 class DeviceBus {
 public:
-    DeviceBus(uint8_t _thread_priority = APM_I2C_PRIORITY) :
-        thread_priority(_thread_priority) {}
+    DeviceBus(uint8_t _thread_priority = APM_I2C_PRIORITY);
 
     struct DeviceBus *next;
     Semaphore semaphore;
@@ -37,12 +37,7 @@ class DeviceBus {
 
     void bouncebuffer_setup(const uint8_t *&buf_tx, uint16_t tx_len,
                             uint8_t *&buf_rx, uint16_t rx_len);
-
-    void bouncebuffer_setup_tx(const uint8_t *&buf_tx, uint16_t tx_len);
-
-    void bouncebuffer_setup_rx(uint8_t *&buf_rx, uint16_t rx_len);
-
-    void bouncebuffer_rx_copy(uint8_t *buf_rx, uint16_t rx_len);
+    void bouncebuffer_finish(const uint8_t *buf_tx, uint8_t *buf_rx, uint16_t rx_len);
 
 private:
     struct callback_info {
@@ -57,10 +52,8 @@ class DeviceBus {
     AP_HAL::Device *hal_device;
 
     // support for bounce buffers for DMA-safe transfers
-    uint8_t *bounce_buffer_tx;
-    uint8_t *bounce_buffer_rx;
-    uint16_t bounce_buffer_tx_size;
-    uint16_t bounce_buffer_rx_size;
+    struct bouncebuffer_t *bounce_buffer_tx;
+    struct bouncebuffer_t *bounce_buffer_rx;
 };
 
 }