AP_BoardConfig: support ChibiOS as well as NuttX

allow for board detection with ChibiOS

libraries/AP_BoardConfig/AP_BoardConfig.cpp

@@ -22,15 +22,6 @@
 #include "AP_BoardConfig.h"
 #include <stdio.h>
 
-#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
-#include <sys/types.h>
-#include <sys/stat.h>
-#include <fcntl.h>
-#include <unistd.h>
-#include <drivers/drv_pwm_output.h>
-#include <drivers/drv_sbus.h>
-#endif
-
 #if CONFIG_HAL_BOARD == HAL_BOARD_PX4
 # define BOARD_SAFETY_ENABLE_DEFAULT 1
 #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
@@ -49,6 +40,7 @@
 #define BOARD_SER1_RTSCTS_DEFAULT 2
 #endif
 #define BOARD_TYPE_DEFAULT PX4_BOARD_AUTO
+
 #elif CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
 # define BOARD_SAFETY_ENABLE_DEFAULT 0
 # define BOARD_PWM_COUNT_DEFAULT 8
@@ -65,58 +57,68 @@
 # elif defined(CONFIG_ARCH_BOARD_VRBRAIN_V54)
 #  define BOARD_TYPE_DEFAULT VRX_BOARD_BRAIN54
 # endif
+
+#elif CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
+# define BOARD_SAFETY_ENABLE_DEFAULT 1
+# define BOARD_PWM_COUNT_DEFAULT 6
+# define BOARD_SER1_RTSCTS_DEFAULT 2
+# define BOARD_TYPE_DEFAULT PX4_BOARD_AUTO
+#endif
+
+#ifndef HAL_IMU_TEMP_DEFAULT
+#define HAL_IMU_TEMP_DEFAULT       -1 // disabled
 #endif
 
 extern const AP_HAL::HAL& hal;
 
 // table of user settable parameters
 const AP_Param::GroupInfo AP_BoardConfig::var_info[] = {
-#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
+#if AP_FEATURE_BOARD_DETECT
     // @Param: PWM_COUNT
     // @DisplayName: Auxiliary pin config
     // @Description: Control assigning of FMU pins to PWM output, timer capture and GPIO. All unassigned pins can be used for GPIO
     // @Values: 0:No PWMs,2:Two PWMs,4:Four PWMs,6:Six PWMs,7:Three PWMs and One Capture
     // @RebootRequired: True
     // @User: Advanced
-    AP_GROUPINFO("PWM_COUNT",    0, AP_BoardConfig, px4.pwm_count, BOARD_PWM_COUNT_DEFAULT),
+    AP_GROUPINFO("PWM_COUNT",    0, AP_BoardConfig, state.pwm_count, BOARD_PWM_COUNT_DEFAULT),
 #endif
 
-#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
+#if AP_FEATURE_RTSCTS
     // @Param: SER1_RTSCTS
     // @DisplayName: Serial 1 flow control
     // @Description: Enable flow control on serial 1 (telemetry 1) on Pixhawk. You must have the RTS and CTS pins connected to your radio. The standard DF13 6 pin connector for a 3DR radio does have those pins connected. If this is set to 2 then flow control will be auto-detected by checking for the output buffer filling on startup. Note that the PX4v1 does not have hardware flow control pins on this port, so you should leave this disabled.
     // @Values: 0:Disabled,1:Enabled,2:Auto
     // @RebootRequired: True
     // @User: Advanced
-    AP_GROUPINFO("SER1_RTSCTS",    1, AP_BoardConfig, px4.ser1_rtscts, BOARD_SER1_RTSCTS_DEFAULT),
+    AP_GROUPINFO("SER1_RTSCTS",    1, AP_BoardConfig, state.ser1_rtscts, BOARD_SER1_RTSCTS_DEFAULT),
 
     // @Param: SER2_RTSCTS
     // @DisplayName: Serial 2 flow control
-    // @Description: Enable flow control on serial 2 (telemetry 2) on Pixhawk and PX4. You must have the RTS and CTS pins connected to your radio. The standard DF13 6 pin connector for a 3DR radio does have those pins connected. If this is set to 2 then flow control will be auto-detected by checking for the output buffer filling on startup.
+    // @Description: Enable flow control on serial 2 (telemetry 2) on Pixhawk and STATE. You must have the RTS and CTS pins connected to your radio. The standard DF13 6 pin connector for a 3DR radio does have those pins connected. If this is set to 2 then flow control will be auto-detected by checking for the output buffer filling on startup.
     // @Values: 0:Disabled,1:Enabled,2:Auto
     // @RebootRequired: True
     // @User: Advanced
-    AP_GROUPINFO("SER2_RTSCTS",    2, AP_BoardConfig, px4.ser2_rtscts, 2),
+    AP_GROUPINFO("SER2_RTSCTS",    2, AP_BoardConfig, state.ser2_rtscts, 2),
 #endif
 
-#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
+#if AP_FEATURE_SAFETY_BUTTON
     // @Param: SAFETYENABLE
     // @DisplayName: Enable use of safety arming switch
     // @Description: This controls the default state of the safety switch at startup. When set to 1 the safety switch will start in the safe state (flashing) at boot. When set to zero the safety switch will start in the unsafe state (solid) at startup. Note that if a safety switch is fitted the user can still control the safety state after startup using the switch. The safety state can also be controlled in software using a MAVLink message.
     // @Values: 0:Disabled,1:Enabled
     // @RebootRequired: True
     // @User: Standard
-    AP_GROUPINFO("SAFETYENABLE",   3, AP_BoardConfig, px4.safety_enable, BOARD_SAFETY_ENABLE_DEFAULT),
+    AP_GROUPINFO("SAFETYENABLE",   3, AP_BoardConfig, state.safety_enable, BOARD_SAFETY_ENABLE_DEFAULT),
 #endif
 
-#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
+#if AP_FEATURE_SBUS_OUT
     // @Param: SBUS_OUT
     // @DisplayName:  SBUS output rate
     // @Description: This sets the SBUS output frame rate in Hz
     // @Values: 0:Disabled,1:50Hz,2:75Hz,3:100Hz,4:150Hz,5:200Hz,6:250Hz,7:300Hz
     // @RebootRequired: True
     // @User: Advanced
-    AP_GROUPINFO("SBUS_OUT",   4, AP_BoardConfig, px4.sbus_out_rate, 0),
+    AP_GROUPINFO("SBUS_OUT",   4, AP_BoardConfig, state.sbus_out_rate, 0),
 #endif
 
     // @Param: SERIAL_NUM
@@ -126,7 +128,7 @@ const AP_Param::GroupInfo AP_BoardConfig::var_info[] = {
     // @User: Standard
     AP_GROUPINFO("SERIAL_NUM", 5, AP_BoardConfig, vehicleSerialNumber, 0),
 
-#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
+#if AP_FEATURE_SAFETY_BUTTON
     // @Param: SAFETY_MASK
     // @DisplayName: Channels to which ignore the safety switch state
     // @Description: A bitmask which controls what channels can move while the safety switch has not been pressed
@@ -134,7 +136,7 @@ const AP_Param::GroupInfo AP_BoardConfig::var_info[] = {
     // @Bitmask: 0:Ch1,1:Ch2,2:Ch3,3:Ch4,4:Ch5,5:Ch6,6:Ch7,7:Ch8,8:Ch9,9:Ch10,10:Ch11,11:Ch12,12:Ch13,13:Ch14
     // @RebootRequired: True
     // @User: Advanced
-    AP_GROUPINFO("SAFETY_MASK", 7, AP_BoardConfig, px4.ignore_safety_channels, 0),
+    AP_GROUPINFO("SAFETY_MASK", 7, AP_BoardConfig, state.ignore_safety_channels, 0),
 #endif
 
 #if HAL_HAVE_IMU_HEATER
@@ -147,17 +149,17 @@ const AP_Param::GroupInfo AP_BoardConfig::var_info[] = {
     AP_GROUPINFO("IMU_TARGTEMP", 8, AP_BoardConfig, _imu_target_temperature, HAL_IMU_TEMP_DEFAULT),
 #endif
 
-#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
+#if AP_FEATURE_BOARD_DETECT
     // @Param: TYPE
     // @DisplayName: Board type
     // @Description: This allows selection of a PX4 or VRBRAIN board type. If set to zero then the board type is auto-detected (PX4)
     // @Values: 0:AUTO,1:PX4V1,2:Pixhawk,3:Cube/Pixhawk2,4:Pixracer,5:PixhawkMini,6:Pixhawk2Slim,7:VRBrain 5.1,8:VRBrain 5.2,9:VR Micro Brain 5.1,10:VR Micro Brain 5.2,11:VRBrain Core 1.0,12:VRBrain 5.4,13:Intel Aero FC,20:AUAV2.1
     // @RebootRequired: True
     // @User: Advanced
-    AP_GROUPINFO("TYPE", 9, AP_BoardConfig, px4.board_type, BOARD_TYPE_DEFAULT),
+    AP_GROUPINFO("TYPE", 9, AP_BoardConfig, state.board_type, BOARD_TYPE_DEFAULT),
 #endif
 
-#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
+#if AP_FEATURE_BOARD_DETECT
 #if HAL_PX4_HAVE_PX4IO
     // @Param: IO_ENABLE
     // @DisplayName: Enable IO co-processor
@@ -165,7 +167,7 @@ const AP_Param::GroupInfo AP_BoardConfig::var_info[] = {
     // @Values: 0:Disabled,1:Enabled
     // @RebootRequired: True
     // @User: Advanced
-    AP_GROUPINFO("IO_ENABLE", 10, AP_BoardConfig, px4.io_enable, 1),
+    AP_GROUPINFO("IO_ENABLE", 10, AP_BoardConfig, state.io_enable, 1),
 #endif
 #endif
 
@@ -176,8 +178,8 @@ const AP_Param::GroupInfo AP_BoardConfig::var_info[] = {
 
 void AP_BoardConfig::init()
 {
-#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
+#if AP_FEATURE_BOARD_DETECT
-    px4_setup();
+    board_setup();
 #endif
 
 #if HAL_HAVE_IMU_HEATER
@@ -191,16 +193,18 @@ void AP_BoardConfig::init()
 // set default value for BRD_SAFETY_MASK
 void AP_BoardConfig::set_default_safety_ignore_mask(uint16_t mask)
 {
-#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
+#if AP_FEATURE_SAFETY_BUTTON
-    px4.ignore_safety_channels.set_default(mask);
+    state.ignore_safety_channels.set_default(mask);
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
     px4_setup_safety_mask();
 #endif
+#endif
 }
 
 void AP_BoardConfig::init_safety()
 {
-#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
+#if AP_FEATURE_SAFETY_BUTTON
-    px4_init_safety();
+    board_init_safety();
 #endif
 }
 

libraries/AP_BoardConfig/AP_BoardConfig.h

@@ -3,7 +3,22 @@
 #include <AP_HAL/AP_HAL.h>
 #include <AP_Common/AP_Common.h>
 #include <AP_Param/AP_Param.h>
-#include <sys/ioctl.h>
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN || CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
+#define AP_FEATURE_BOARD_DETECT 1
+#define AP_FEATURE_SAFETY_BUTTON 1
+#else
+#define AP_FEATURE_BOARD_DETECT 0
+#define AP_FEATURE_SAFETY_BUTTON 0
+#endif
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
+#define AP_FEATURE_RTSCTS 1
+#define AP_FEATURE_SBUS_OUT 1
+#else
+#define AP_FEATURE_RTSCTS 0
+#define AP_FEATURE_SBUS_OUT 0
+#endif
 
 extern "C" typedef int (*main_fn_t)(int argc, char **);
 
@@ -32,11 +47,14 @@ public:
 #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
     // public method to start a driver
     static bool px4_start_driver(main_fn_t main_function, const char *name, const char *arguments);
+#endif
+
+#if AP_FEATURE_BOARD_DETECT
 
     // valid types for BRD_TYPE: these values need to be in sync with the
     // values from the param description
     enum px4_board_type {
-#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
         PX4_BOARD_AUTO     = 0,
         PX4_BOARD_PX4V1    = 1,
         PX4_BOARD_PIXHAWK  = 2,
@@ -47,6 +65,7 @@ public:
         PX4_BOARD_AEROFC   = 13,
         PX4_BOARD_PIXHAWK_PRO = 14,
         PX4_BOARD_AUAV21   = 20,
+        PX4_BOARD_PCNC1    = 21,
         PX4_BOARD_OLDDRIVERS = 100,
 #endif
 #if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
@@ -58,12 +77,12 @@ public:
         VRX_BOARD_BRAIN54  = 12,
 #endif
     };
-#endif
+#endif // AP_FEATURE_BOARD_DETECT
 
     // set default value for BRD_SAFETY_MASK
     void set_default_safety_ignore_mask(uint16_t mask);
 
-#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
+#if AP_FEATURE_BOARD_DETECT
     static enum px4_board_type get_board_type(void) {
         return px4_configured_board;
     }
@@ -72,41 +91,42 @@ public:
 private:
     AP_Int16 vehicleSerialNumber;
 
-#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
+#if AP_FEATURE_BOARD_DETECT
     struct {
         AP_Int8 pwm_count;
         AP_Int8 safety_enable;
         AP_Int32 ignore_safety_channels;
-#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
         AP_Int8 ser1_rtscts;
         AP_Int8 ser2_rtscts;
         AP_Int8 sbus_out_rate;
 #endif
         AP_Int8 board_type;
         AP_Int8 io_enable;
-    } px4;
+    } state;
 
     static enum px4_board_type px4_configured_board;
 
-    void px4_drivers_start(void);
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
-    void px4_setup(void);
     void px4_setup_pwm(void);
-    void px4_init_safety(void);
     void px4_setup_safety_mask(void);
-    void px4_setup_uart(void);
-    void px4_setup_sbus(void);
-    void px4_setup_drivers(void);
-    void px4_setup_peripherals(void);
-    void px4_setup_px4io(void);
     void px4_tone_alarm(const char *tone_string);
+    void px4_setup_px4io(void);
+    void px4_setup_peripherals(void);
+#endif
+    
+    void board_setup(void);
+
+    void board_init_safety(void);
+    void board_setup_safety_mask(void);
+    void board_setup_uart(void);
+    void board_setup_sbus(void);
+    void board_setup_drivers(void);
     bool spi_check_register(const char *devname, uint8_t regnum, uint8_t value, uint8_t read_flag = 0x80);
     void validate_board_type(void);
+    void board_autodetect(void);
 
-#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
+#endif // AP_FEATURE_BOARD_DETECT
-    void px4_autodetect(void);
-#endif
-
-#endif // HAL_BOARD_PX4 || HAL_BOARD_VRBRAIN
 
     static bool _in_sensor_config_error;
 

libraries/AP_BoardConfig/AP_BoardConfig_CAN.h

@@ -4,8 +4,9 @@
 #include "AP_BoardConfig.h"
 #include <AP_Common/AP_Common.h>
 #include <AP_Param/AP_Param.h>
+#if CONFIG_HAL_BOARD != HAL_BOARD_CHIBIOS //we don't have ioctls in ChibiOS
 #include <sys/ioctl.h>
-
+#endif
 #if HAL_WITH_UAVCAN
 #define UAVCAN_PROTOCOL_ENABLE  1
 

libraries/AP_BoardConfig/board_drivers.cpp

@@ -0,0 +1,287 @@
+/*
+   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_BoardConfig - px4 driver loading and setup
+ */
+
+#include <AP_HAL/AP_HAL.h>
+#include "AP_BoardConfig.h"
+#include <stdio.h>
+
+#if AP_FEATURE_BOARD_DETECT
+
+extern const AP_HAL::HAL& hal;
+
+AP_BoardConfig::px4_board_type AP_BoardConfig::px4_configured_board;
+
+/*
+  setup flow control on UARTs
+ */
+void AP_BoardConfig::board_setup_uart()
+{
+#if AP_FEATURE_RTSCTS
+    hal.uartC->set_flow_control((AP_HAL::UARTDriver::flow_control)state.ser1_rtscts.get());
+    if (hal.uartD != nullptr) {
+        hal.uartD->set_flow_control((AP_HAL::UARTDriver::flow_control)state.ser2_rtscts.get());
+    }
+#endif
+}
+
+/*
+  init safety state
+ */
+void AP_BoardConfig::board_init_safety()
+{
+#if AP_FEATURE_SAFETY_BUTTON
+    if (state.safety_enable.get() == 0) {
+        hal.rcout->force_safety_off();
+        hal.rcout->force_safety_no_wait();
+        // wait until safety has been turned off
+        uint8_t count = 20;
+        while (hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_ARMED && count--) {
+            hal.scheduler->delay(20);
+        }
+    }
+#endif
+}
+
+/*
+  setup SBUS
+ */
+void AP_BoardConfig::board_setup_sbus(void)
+{
+#if AP_FEATURE_SBUS_OUT
+    if (state.sbus_out_rate.get() >= 1) {
+        static const struct {
+            uint8_t value;
+            uint16_t rate;
+        } rates[] = {
+            { 1, 50 },
+            { 2, 75 },
+            { 3, 100 },
+            { 4, 150 },
+            { 5, 200 },
+            { 6, 250 },
+            { 7, 300 }
+        };
+        uint16_t rate = 300;
+        for (uint8_t i=0; i<ARRAY_SIZE(rates); i++) {
+            if (rates[i].value == state.sbus_out_rate) {
+                rate = rates[i].rate;
+            }
+        }
+        if (!hal.rcout->enable_px4io_sbus_out(rate)) {
+            hal.console->printf("Failed to enable SBUS out\n");
+        }
+    }
+#endif
+}
+
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V4)
+extern "C" {
+    int fmu_main(int, char **);
+};
+#endif
+
+void AP_BoardConfig::board_setup_drivers(void)
+{
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V4)
+    /*
+      this works around an issue with some FMUv4 hardware (eg. copies
+      of the Pixracer) which have incorrect components leading to
+      sensor brownout on boot
+     */
+    if (px4_start_driver(fmu_main, "fmu", "sensor_reset 20")) {
+        printf("FMUv4 sensor reset complete\n");
+    }
+#endif
+
+    if (state.board_type == PX4_BOARD_OLDDRIVERS) {
+        printf("Old drivers no longer supported\n");
+        state.board_type = PX4_BOARD_AUTO;
+    }
+
+    // run board auto-detection
+    board_autodetect();
+
+    if (state.board_type == PX4_BOARD_PH2SLIM ||
+        state.board_type == PX4_BOARD_PIXHAWK2) {
+        _imu_target_temperature.set_default(45);
+        if (_imu_target_temperature.get() < 0) {
+            // don't allow a value of -1 on the cube, or it could cook
+            // the IMU
+            _imu_target_temperature.set(45);
+        }
+    }
+
+    px4_configured_board = (enum px4_board_type)state.board_type.get();
+
+    switch (px4_configured_board) {
+    case PX4_BOARD_PX4V1:
+    case PX4_BOARD_PIXHAWK:
+    case PX4_BOARD_PIXHAWK2:
+    case PX4_BOARD_PIXRACER:
+    case PX4_BOARD_PHMINI:
+    case PX4_BOARD_AUAV21:
+    case PX4_BOARD_PH2SLIM:
+    case PX4_BOARD_AEROFC:
+    case PX4_BOARD_PIXHAWK_PRO:
+    case PX4_BOARD_PCNC1:
+        break;
+    default:
+        sensor_config_error("Unknown board type");
+        break;
+    }
+}
+
+/*
+  check a SPI device for a register value
+ */
+bool AP_BoardConfig::spi_check_register(const char *devname, uint8_t regnum, uint8_t value, uint8_t read_flag)
+{
+    auto dev = hal.spi->get_device(devname);
+    if (!dev) {
+        hal.console->printf("%s: no device\n", devname);
+        return false;
+    }
+    dev->set_read_flag(read_flag);
+    uint8_t v;
+    if (!dev->read_registers(regnum, &v, 1)) {
+        hal.console->printf("%s: reg %02x read fail\n", devname, (unsigned)regnum);
+        return false;
+    }
+    hal.console->printf("%s: reg %02x expected:%02x got:%02x\n", devname, (unsigned)regnum, (unsigned)value, (unsigned)v);
+    return v == value;
+}
+
+#define MPUREG_WHOAMI 0x75
+#define MPU_WHOAMI_MPU60X0  0x68
+#define MPU_WHOAMI_MPU9250  0x71
+#define MPU_WHOAMI_ICM20608 0xaf
+#define MPU_WHOAMI_ICM20602 0x12
+
+#define LSMREG_WHOAMI 0x0f
+#define LSM_WHOAMI_LSM303D 0x49
+
+/*
+  validation of the board type
+ */
+void AP_BoardConfig::validate_board_type(void)
+{
+    /* some boards can be damaged by the user setting the wrong board
+       type.  The key one is the cube which has a heater which can
+       cook the IMUs if the user uses an old paramater file. We
+       override the board type for that specific case
+     */
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(HAL_CHIBIOS_ARCH_FMUV3)
+    if (state.board_type == PX4_BOARD_PIXHAWK &&
+        (spi_check_register("mpu6000_ext", MPUREG_WHOAMI, MPU_WHOAMI_MPU60X0) ||
+         spi_check_register("mpu9250_ext", MPUREG_WHOAMI, MPU_WHOAMI_MPU9250) ||
+         spi_check_register("icm20608", MPUREG_WHOAMI, MPU_WHOAMI_ICM20608) ||
+         spi_check_register("icm20608_ext", MPUREG_WHOAMI, MPU_WHOAMI_ICM20602)) &&
+        spi_check_register("lsm9ds0_ext_am", LSMREG_WHOAMI, LSM_WHOAMI_LSM303D)) {
+        // Pixhawk2 has LSM303D and MPUxxxx on external bus. If we
+        // detect those, then force PIXHAWK2, even if the user has
+        // configured for PIXHAWK1
+#if !defined(CONFIG_ARCH_BOARD_PX4FMU_V3) && !defined(HAL_CHIBIOS_ARCH_FMUV3)
+        // force user to load the right firmware
+        sensor_config_error("Pixhawk2 requires FMUv3 firmware");        
+#endif
+        state.board_type.set(PX4_BOARD_PIXHAWK2);
+        hal.console->printf("Forced PIXHAWK2\n");
+    }
+#endif
+
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V4PRO)
+	// Nothing to do for the moment
+#endif
+}
+
+/*
+  auto-detect board type
+ */
+void AP_BoardConfig::board_autodetect(void)
+{
+    if (state.board_type != PX4_BOARD_AUTO) {
+        validate_board_type();
+        // user has chosen a board type
+        return;
+    }
+
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
+    // only one choice
+    state.board_type.set(PX4_BOARD_PX4V1);
+    hal.console->printf("Detected PX4v1\n");
+
+#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(HAL_CHIBIOS_ARCH_FMUV3)
+    if ((spi_check_register("mpu6000_ext", MPUREG_WHOAMI, MPU_WHOAMI_MPU60X0) ||
+         spi_check_register("mpu6000_ext", MPUREG_WHOAMI, MPU_WHOAMI_MPU60X0) ||
+         spi_check_register("mpu9250_ext", MPUREG_WHOAMI, MPU_WHOAMI_MPU60X0) ||
+         spi_check_register("mpu9250_ext", MPUREG_WHOAMI, MPU_WHOAMI_MPU9250) ||
+         spi_check_register("icm20608_ext", MPUREG_WHOAMI, MPU_WHOAMI_ICM20608) ||
+         spi_check_register("icm20608_ext", MPUREG_WHOAMI, MPU_WHOAMI_ICM20602)) &&
+        spi_check_register("lsm9ds0_ext_am", LSMREG_WHOAMI, LSM_WHOAMI_LSM303D)) {
+        // Pixhawk2 has LSM303D and MPUxxxx on external bus
+        state.board_type.set(PX4_BOARD_PIXHAWK2);
+        hal.console->printf("Detected PIXHAWK2\n");
+    } else if ((spi_check_register("icm20608-am", MPUREG_WHOAMI, MPU_WHOAMI_ICM20608) ||
+                spi_check_register("icm20608-am", MPUREG_WHOAMI, MPU_WHOAMI_ICM20602)) &&
+               spi_check_register("mpu9250", MPUREG_WHOAMI, MPU_WHOAMI_MPU9250)) {
+        // PHMINI has an ICM20608 and MPU9250 on sensor bus
+        state.board_type.set(PX4_BOARD_PHMINI);
+        hal.console->printf("Detected PixhawkMini\n");
+    } else if (spi_check_register("lsm9ds0_am", LSMREG_WHOAMI, LSM_WHOAMI_LSM303D) &&
+               (spi_check_register("mpu6000", MPUREG_WHOAMI, MPU_WHOAMI_MPU60X0) ||
+                spi_check_register("icm20608", MPUREG_WHOAMI, MPU_WHOAMI_ICM20608) ||
+                spi_check_register("icm20608", MPUREG_WHOAMI, MPU_WHOAMI_ICM20602) ||
+                spi_check_register("mpu9250", MPUREG_WHOAMI, MPU_WHOAMI_MPU9250))) {
+
+        // classic or upgraded Pixhawk1
+        state.board_type.set(PX4_BOARD_PIXHAWK);
+        hal.console->printf("Detected Pixhawk\n");
+    } else {
+        sensor_config_error("Unable to detect board type");
+    }
+#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V4)
+    // only one choice
+    state.board_type.set_and_notify(PX4_BOARD_PIXRACER);
+    hal.console->printf("Detected Pixracer\n");
+#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V4PRO)
+    // only one choice
+    state.board_type.set_and_notify(PX4_BOARD_PIXHAWK_PRO);
+    hal.console->printf("Detected Pixhawk Pro\n");	
+#elif defined(CONFIG_ARCH_BOARD_AEROFC_V1)
+    state.board_type.set_and_notify(PX4_BOARD_AEROFC);
+    hal.console->printf("Detected Aero FC\n");
+#endif
+
+}
+
+/*
+  setup peripherals and drivers
+ */
+void AP_BoardConfig::board_setup()
+{
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
+    px4_setup_peripherals();
+    px4_setup_pwm();
+    px4_setup_safety_mask();
+#endif
+    board_setup_uart();
+    board_setup_sbus();
+    board_setup_drivers();
+}
+
+#endif // HAL_BOARD_PX4

libraries/AP_BoardConfig/px4_drivers.cpp

@@ -31,8 +31,6 @@
 
 extern const AP_HAL::HAL& hal;
 
-AP_BoardConfig::px4_board_type AP_BoardConfig::px4_configured_board;
-
 /* 
    declare driver main entry points
  */
@@ -64,7 +62,7 @@ void AP_BoardConfig::px4_setup_pwm()
         { 8, PWM_SERVO_MODE_12PWM, 0 },
 #endif
     };
-    uint8_t mode_parm = (uint8_t)px4.pwm_count.get();
+    uint8_t mode_parm = (uint8_t)state.pwm_count.get();
     uint8_t i;
     for (i=0; i<ARRAY_SIZE(mode_table); i++) {
         if (mode_table[i].mode_parm == mode_parm) {
@@ -92,19 +90,6 @@ void AP_BoardConfig::px4_setup_pwm()
     }
 }
 
-/*
-  setup flow control on UARTs
- */
-void AP_BoardConfig::px4_setup_uart()
-{
-#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
-    hal.uartC->set_flow_control((AP_HAL::UARTDriver::flow_control)px4.ser1_rtscts.get());
-    if (hal.uartD != nullptr) {
-        hal.uartD->set_flow_control((AP_HAL::UARTDriver::flow_control)px4.ser2_rtscts.get());
-    }
-#endif
-}
-
 /*
   setup safety switch
  */
@@ -114,13 +99,13 @@ void AP_BoardConfig::px4_setup_safety_mask()
     // setup channels to ignore the armed state
     int px4io_fd = open("/dev/px4io", 0);
     if (px4io_fd != -1) {
-        if (ioctl(px4io_fd, PWM_SERVO_IGNORE_SAFETY, (uint16_t)px4.ignore_safety_channels) != 0) {
+        if (ioctl(px4io_fd, PWM_SERVO_IGNORE_SAFETY, (uint16_t)state.ignore_safety_channels) != 0) {
             hal.console->printf("IGNORE_SAFETY failed\n");
         }
     }
     int px4fmu_fd = open("/dev/px4fmu", 0);
     if (px4fmu_fd != -1) {
-        uint16_t mask = px4.ignore_safety_channels;
+        uint16_t mask = state.ignore_safety_channels;
         if (px4io_fd != -1) {
             mask >>= 8;
         }
@@ -135,54 +120,6 @@ void AP_BoardConfig::px4_setup_safety_mask()
 #endif
 }
 
-/*
-  init safety state
- */
-void AP_BoardConfig::px4_init_safety()
-{
-    if (px4.safety_enable.get() == 0) {
-        hal.rcout->force_safety_off();
-        hal.rcout->force_safety_no_wait();
-        // wait until safety has been turned off
-        uint8_t count = 20;
-        while (hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_ARMED && count--) {
-            hal.scheduler->delay(20);
-        }
-    }
-}
-
-/*
-  setup SBUS
- */
-void AP_BoardConfig::px4_setup_sbus(void)
-{
-#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
-    if (px4.sbus_out_rate.get() >= 1) {
-        static const struct {
-            uint8_t value;
-            uint16_t rate;
-        } rates[] = {
-            { 1, 50 },
-            { 2, 75 },
-            { 3, 100 },
-            { 4, 150 },
-            { 5, 200 },
-            { 6, 250 },
-            { 7, 300 }
-        };
-        uint16_t rate = 300;
-        for (uint8_t i=0; i<ARRAY_SIZE(rates); i++) {
-            if (rates[i].value == px4.sbus_out_rate) {
-                rate = rates[i].rate;
-            }
-        }
-        if (!hal.rcout->enable_px4io_sbus_out(rate)) {
-            hal.console->printf("Failed to enable PX4IO SBUS out\n");
-        }
-    }
-#endif
-}
-
 extern "C" int waitpid(pid_t, int *, int);
 
 /*
@@ -225,56 +162,6 @@ bool AP_BoardConfig::px4_start_driver(main_fn_t main_function, const char *name,
     return (status >> 8) == 0;
 }
 
-void AP_BoardConfig::px4_setup_drivers(void)
-{
-#if defined(CONFIG_ARCH_BOARD_PX4FMU_V4)
-    /*
-      this works around an issue with some FMUv4 hardware (eg. copies
-      of the Pixracer) which have incorrect components leading to
-      sensor brownout on boot
-     */
-    if (px4_start_driver(fmu_main, "fmu", "sensor_reset 20")) {
-        printf("FMUv4 sensor reset complete\n");
-    }
-#endif
-
-    if (px4.board_type == PX4_BOARD_OLDDRIVERS) {
-        printf("Old drivers no longer supported\n");
-        px4.board_type = PX4_BOARD_AUTO;
-    }
-
-    // run board auto-detection
-    px4_autodetect();
-
-    if (px4.board_type == PX4_BOARD_PH2SLIM ||
-        px4.board_type == PX4_BOARD_PIXHAWK2) {
-        _imu_target_temperature.set_default(45);
-        if (_imu_target_temperature.get() < 0) {
-            // don't allow a value of -1 on the cube, or it could cook
-            // the IMU
-            _imu_target_temperature.set(45);
-        }
-    }
-
-    px4_configured_board = (enum px4_board_type)px4.board_type.get();
-
-    switch (px4_configured_board) {
-    case PX4_BOARD_PX4V1:
-    case PX4_BOARD_PIXHAWK:
-    case PX4_BOARD_PIXHAWK2:
-    case PX4_BOARD_PIXRACER:
-    case PX4_BOARD_PHMINI:
-    case PX4_BOARD_AUAV21:
-    case PX4_BOARD_PH2SLIM:
-    case PX4_BOARD_AEROFC:
-    case PX4_BOARD_PIXHAWK_PRO:
-        break;
-    default:
-        sensor_config_error("Unknown board type");
-        break;
-    }
-}
-
 /*
   play a tune
  */
@@ -365,7 +252,7 @@ void AP_BoardConfig::px4_setup_peripherals(void)
     }
 
 #if HAL_PX4_HAVE_PX4IO
-    if (px4.io_enable.get() != 0) {
+    if (state.io_enable.get() != 0) {
         px4_setup_px4io();
     }
 #endif
@@ -388,138 +275,4 @@ void AP_BoardConfig::px4_setup_peripherals(void)
     hal.rcout->init();
 }
 
-/*
-  check a SPI device for a register value
- */
-bool AP_BoardConfig::spi_check_register(const char *devname, uint8_t regnum, uint8_t value, uint8_t read_flag)
-{
-    auto dev = hal.spi->get_device(devname);
-    if (!dev) {
-        printf("%s: no device\n", devname);
-        return false;
-    }
-    dev->set_read_flag(read_flag);
-    uint8_t v;
-    if (!dev->read_registers(regnum, &v, 1)) {
-        printf("%s: reg %02x read fail\n", devname, (unsigned)regnum);
-        return false;
-    }
-    printf("%s: reg %02x %02x %02x\n", devname, (unsigned)regnum, (unsigned)value, (unsigned)v);
-    return v == value;
-}
-
-#define MPUREG_WHOAMI 0x75
-#define MPU_WHOAMI_MPU60X0  0x68
-#define MPU_WHOAMI_MPU9250  0x71
-#define MPU_WHOAMI_ICM20608 0xaf
-#define MPU_WHOAMI_ICM20602 0x12
-
-#define LSMREG_WHOAMI 0x0f
-#define LSM_WHOAMI_LSM303D 0x49
-
-/*
-  validation of the board type
- */
-void AP_BoardConfig::validate_board_type(void)
-{
-    /* some boards can be damaged by the user setting the wrong board
-       type.  The key one is the cube which has a heater which can
-       cook the IMUs if the user uses an old paramater file. We
-       override the board type for that specific case
-     */
-#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
-    if (px4.board_type == PX4_BOARD_PIXHAWK &&
-        (spi_check_register(HAL_INS_MPU60x0_EXT_NAME, MPUREG_WHOAMI, MPU_WHOAMI_MPU60X0) ||
-         spi_check_register(HAL_INS_MPU9250_EXT_NAME, MPUREG_WHOAMI, MPU_WHOAMI_MPU9250) ||
-         spi_check_register(HAL_INS_ICM20608_EXT_NAME, MPUREG_WHOAMI, MPU_WHOAMI_ICM20608) ||
-         spi_check_register(HAL_INS_ICM20608_EXT_NAME, MPUREG_WHOAMI, MPU_WHOAMI_ICM20602)) &&
-        spi_check_register(HAL_INS_LSM9DS0_EXT_A_NAME, LSMREG_WHOAMI, LSM_WHOAMI_LSM303D)) {
-        // Pixhawk2 has LSM303D and MPUxxxx on external bus. If we
-        // detect those, then force PIXHAWK2, even if the user has
-        // configured for PIXHAWK1
-#if !defined(CONFIG_ARCH_BOARD_PX4FMU_V3)
-        // force user to load the right firmware
-        sensor_config_error("Pixhawk2 requires FMUv3 firmware");        
-#endif
-        px4.board_type.set(PX4_BOARD_PIXHAWK2);
-        hal.console->printf("Forced PIXHAWK2\n");
-    }
-#endif
-
-#if defined(CONFIG_ARCH_BOARD_PX4FMU_V4PRO)
-	// Nothing to do for the moment
-#endif
-}
-
-/*
-  auto-detect board type
- */
-void AP_BoardConfig::px4_autodetect(void)
-{
-    if (px4.board_type != PX4_BOARD_AUTO) {
-        validate_board_type();
-        // user has chosen a board type
-        return;
-    }
-
-#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
-    // only one choice
-    px4.board_type.set(PX4_BOARD_PX4V1);
-    hal.console->printf("Detected PX4v1\n");
-
-#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
-    if ((spi_check_register(HAL_INS_MPU60x0_EXT_NAME, MPUREG_WHOAMI, MPU_WHOAMI_MPU60X0) ||
-         spi_check_register(HAL_INS_MPU9250_EXT_NAME, MPUREG_WHOAMI, MPU_WHOAMI_MPU9250) ||
-         spi_check_register(HAL_INS_ICM20608_EXT_NAME, MPUREG_WHOAMI, MPU_WHOAMI_ICM20608) ||
-         spi_check_register(HAL_INS_ICM20608_EXT_NAME, MPUREG_WHOAMI, MPU_WHOAMI_ICM20602)) &&
-        spi_check_register(HAL_INS_LSM9DS0_EXT_A_NAME, LSMREG_WHOAMI, LSM_WHOAMI_LSM303D)) {
-        // Pixhawk2 has LSM303D and MPUxxxx on external bus
-        px4.board_type.set(PX4_BOARD_PIXHAWK2);
-        hal.console->printf("Detected PIXHAWK2\n");
-    } else if ((spi_check_register(HAL_INS_ICM20608_AM_NAME, MPUREG_WHOAMI, MPU_WHOAMI_ICM20608) ||
-                spi_check_register(HAL_INS_ICM20608_AM_NAME, MPUREG_WHOAMI, MPU_WHOAMI_ICM20602)) &&
-               spi_check_register(HAL_INS_MPU9250_NAME, MPUREG_WHOAMI, MPU_WHOAMI_MPU9250)) {
-        // PHMINI has an ICM20608 and MPU9250 on sensor bus
-        px4.board_type.set(PX4_BOARD_PHMINI);
-        hal.console->printf("Detected PixhawkMini\n");
-    } else if (spi_check_register(HAL_INS_LSM9DS0_A_NAME, LSMREG_WHOAMI, LSM_WHOAMI_LSM303D) &&
-               (spi_check_register(HAL_INS_MPU60x0_NAME, MPUREG_WHOAMI, MPU_WHOAMI_MPU60X0) ||
-                spi_check_register(HAL_INS_ICM20608_NAME, MPUREG_WHOAMI, MPU_WHOAMI_ICM20608) ||
-                spi_check_register(HAL_INS_ICM20608_NAME, MPUREG_WHOAMI, MPU_WHOAMI_ICM20602) ||
-                spi_check_register(HAL_INS_MPU9250_NAME, MPUREG_WHOAMI, MPU_WHOAMI_MPU9250))) {
-
-        // classic or upgraded Pixhawk1
-        px4.board_type.set(PX4_BOARD_PIXHAWK);
-        hal.console->printf("Detected Pixhawk\n");
-    } else {
-        sensor_config_error("Unable to detect board type");
-    }
-#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V4)
-    // only one choice
-    px4.board_type.set_and_notify(PX4_BOARD_PIXRACER);
-    hal.console->printf("Detected Pixracer\n");
-#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V4PRO)
-    // only one choice
-    px4.board_type.set_and_notify(PX4_BOARD_PIXHAWK_PRO);
-    hal.console->printf("Detected Pixhawk Pro\n");	
-#elif defined(CONFIG_ARCH_BOARD_AEROFC_V1)
-    px4.board_type.set_and_notify(PX4_BOARD_AEROFC);
-    hal.console->printf("Detected Aero FC\n");
-#endif
-
-}
-
-/*
-  setup px4 peripherals and drivers
- */
-void AP_BoardConfig::px4_setup()
-{
-    px4_setup_peripherals();
-    px4_setup_pwm();
-    px4_setup_safety_mask();
-    px4_setup_uart();
-    px4_setup_sbus();
-    px4_setup_drivers();
-}
-
 #endif // HAL_BOARD_PX4