diff --git a/libraries/AP_BoardConfig/AP_BoardConfig.cpp b/libraries/AP_BoardConfig/AP_BoardConfig.cpp index 05d027c1de..23857ed3c5 100644 --- a/libraries/AP_BoardConfig/AP_BoardConfig.cpp +++ b/libraries/AP_BoardConfig/AP_BoardConfig.cpp @@ -30,47 +30,11 @@ #endif #endif -#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 -# define BOARD_SAFETY_ENABLE_DEFAULT 1 -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) -#define BOARD_PWM_COUNT_DEFAULT 2 -#define BOARD_SER1_RTSCTS_DEFAULT 0 // no flow control on UART5 on FMUv1 -#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V4) -#define BOARD_PWM_COUNT_DEFAULT 6 -#define BOARD_SER1_RTSCTS_DEFAULT 2 -#elif defined(CONFIG_ARCH_BOARD_AEROFC_V1) -#define BOARD_PWM_COUNT_DEFAULT 0 -#define BOARD_SER1_RTSCTS_DEFAULT 0 -# undef BOARD_SAFETY_ENABLE_DEFAULT -# define BOARD_SAFETY_ENABLE_DEFAULT 0 -#else // V2 -#define BOARD_PWM_COUNT_DEFAULT 4 -#define BOARD_SER1_RTSCTS_DEFAULT 2 -#endif #ifndef BOARD_TYPE_DEFAULT #define BOARD_TYPE_DEFAULT PX4_BOARD_AUTO #endif -#elif CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN -# define BOARD_SAFETY_ENABLE_DEFAULT 0 -# define BOARD_PWM_COUNT_DEFAULT 8 -# if defined(CONFIG_ARCH_BOARD_VRBRAIN_V51) -# define BOARD_TYPE_DEFAULT VRX_BOARD_BRAIN51 -# elif defined(CONFIG_ARCH_BOARD_VRBRAIN_V52) -# define BOARD_TYPE_DEFAULT VRX_BOARD_BRAIN52 -# elif defined(CONFIG_ARCH_BOARD_VRBRAIN_V52E) -# define BOARD_TYPE_DEFAULT VRX_BOARD_BRAIN52E -# elif defined(CONFIG_ARCH_BOARD_VRUBRAIN_V51) -# define BOARD_TYPE_DEFAULT VRX_BOARD_UBRAIN51 -# elif defined(CONFIG_ARCH_BOARD_VRUBRAIN_V52) -# define BOARD_TYPE_DEFAULT VRX_BOARD_UBRAIN52 -# elif defined(CONFIG_ARCH_BOARD_VRCORE_V10) -# define BOARD_TYPE_DEFAULT VRX_BOARD_CORE10 -# elif defined(CONFIG_ARCH_BOARD_VRBRAIN_V54) -# define BOARD_TYPE_DEFAULT VRX_BOARD_BRAIN54 -# endif - -#elif CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS +#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS # define BOARD_SAFETY_ENABLE_DEFAULT 1 #ifndef BOARD_PWM_COUNT_DEFAULT # define BOARD_PWM_COUNT_DEFAULT 6 @@ -296,9 +260,6 @@ void AP_BoardConfig::set_default_safety_ignore_mask(uint16_t mask) { #if HAL_HAVE_SAFETY_SWITCH state.ignore_safety_channels.set_default(mask); -#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 - px4_setup_safety_mask(); -#endif #endif } diff --git a/libraries/AP_BoardConfig/AP_BoardConfig.h b/libraries/AP_BoardConfig/AP_BoardConfig.h index 447f5e3ade..ae0a52278b 100644 --- a/libraries/AP_BoardConfig/AP_BoardConfig.h +++ b/libraries/AP_BoardConfig/AP_BoardConfig.h @@ -6,7 +6,7 @@ #include #ifndef AP_FEATURE_BOARD_DETECT -#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_CHIBIOS_ARCH_FMUV4PRO) || defined(HAL_CHIBIOS_ARCH_BRAINV51) || defined(HAL_CHIBIOS_ARCH_BRAINV52) || defined(HAL_CHIBIOS_ARCH_UBRAINV51) || defined(HAL_CHIBIOS_ARCH_COREV10) || defined(HAL_CHIBIOS_ARCH_BRAINV54) +#if defined(HAL_CHIBIOS_ARCH_FMUV3) || defined(HAL_CHIBIOS_ARCH_FMUV4) || defined(HAL_CHIBIOS_ARCH_FMUV5) || defined(HAL_CHIBIOS_ARCH_MINDPXV2) || defined(HAL_CHIBIOS_ARCH_FMUV4PRO) || defined(HAL_CHIBIOS_ARCH_BRAINV51) || defined(HAL_CHIBIOS_ARCH_BRAINV52) || defined(HAL_CHIBIOS_ARCH_UBRAINV51) || defined(HAL_CHIBIOS_ARCH_COREV10) || defined(HAL_CHIBIOS_ARCH_BRAINV54) #define AP_FEATURE_BOARD_DETECT 1 #else #define AP_FEATURE_BOARD_DETECT 0 @@ -59,11 +59,6 @@ public: // that we're never going to boot properly: static bool in_sensor_config_error(void) { return _in_sensor_config_error; } -#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 - // valid types for BRD_TYPE: these values need to be in sync with the // values from the param description enum px4_board_type { @@ -174,7 +169,7 @@ private: AP_Int8 safety_enable; AP_Int16 safety_option; AP_Int32 ignore_safety_channels; -#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS +#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS AP_Int8 ser1_rtscts; AP_Int8 ser2_rtscts; AP_Int8 sbus_out_rate; @@ -187,15 +182,6 @@ private: #if AP_FEATURE_BOARD_DETECT static enum px4_board_type px4_configured_board; -#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN - void px4_setup_pwm(void); - void px4_setup_safety_mask(void); - void px4_tone_alarm(const char *tone_string); - void px4_setup_px4io(void); - void px4_setup_peripherals(void); -#endif - - 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); diff --git a/libraries/AP_BoardConfig/AP_BoardConfig_CAN.cpp b/libraries/AP_BoardConfig/AP_BoardConfig_CAN.cpp index 68fd6a2f12..a9f045ddf5 100644 --- a/libraries/AP_BoardConfig/AP_BoardConfig_CAN.cpp +++ b/libraries/AP_BoardConfig/AP_BoardConfig_CAN.cpp @@ -23,14 +23,7 @@ #if HAL_WITH_UAVCAN -#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN -#include -#include -#include -#include - -#include -#elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX +#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX #include #elif CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS #include @@ -117,9 +110,7 @@ void AP_BoardConfig_CAN::init() if (hal.can_mgr[drv_num - 1] == nullptr) { // CAN Manager is the driver // So if this driver was not created before for other physical interface - do it - #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN - const_cast (hal).can_mgr[drv_num - 1] = new PX4::PX4CANManager; - #elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX + #if CONFIG_HAL_BOARD == HAL_BOARD_LINUX const_cast (hal).can_mgr[drv_num - 1] = new Linux::CANManager; #elif CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS const_cast (hal).can_mgr[drv_num - 1] = new ChibiOS::CANManager; diff --git a/libraries/AP_BoardConfig/board_drivers.cpp b/libraries/AP_BoardConfig/board_drivers.cpp index df0705c517..75f74f4d30 100644 --- a/libraries/AP_BoardConfig/board_drivers.cpp +++ b/libraries/AP_BoardConfig/board_drivers.cpp @@ -333,11 +333,7 @@ void AP_BoardConfig::board_setup_sbus(void) */ 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(); -#elif CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS +#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS // init needs to be done after boardconfig is read so parameters are set hal.gpio->init(); hal.rcin->init(); diff --git a/libraries/AP_BoardConfig/px4_drivers.cpp b/libraries/AP_BoardConfig/px4_drivers.cpp deleted file mode 100644 index e170da7f85..0000000000 --- a/libraries/AP_BoardConfig/px4_drivers.cpp +++ /dev/null @@ -1,284 +0,0 @@ -/* - 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 . - */ -/* - * AP_BoardConfig - px4 driver loading and setup - */ - -#include -#include "AP_BoardConfig.h" - -#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN -#include -#include -#include -#include -#include -#include -#include -#include - -extern const AP_HAL::HAL& hal; - -/* - declare driver main entry points - */ -extern "C" { - int fmu_main(int, char **); - int px4io_main(int, char **); - int adc_main(int, char **); - int tone_alarm_main(int, char **); -}; - -/* - setup PWM pins - */ -void AP_BoardConfig::px4_setup_pwm() -{ -#if CONFIG_HAL_BOARD_SUBTYPE != HAL_BOARD_SUBTYPE_PX4_AEROFC_V1 - /* configure the FMU driver for the right number of PWMs */ - static const struct { - uint8_t mode_parm; - uint8_t mode_value; - uint8_t num_gpios; - } mode_table[] = { - /* table mapping BRD_PWM_COUNT to ioctl arguments */ - { 0, PWM_SERVO_MODE_NONE, 6 }, - { 2, PWM_SERVO_MODE_2PWM, 4 }, - { 4, PWM_SERVO_MODE_4PWM, 2 }, - { 6, PWM_SERVO_MODE_6PWM, 0 }, - { 7, PWM_SERVO_MODE_3PWM1CAP, 2 }, -#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN - { 8, PWM_SERVO_MODE_12PWM, 0 }, -#endif - }; - uint8_t mode_parm = (uint8_t)pwm_count.get(); - uint8_t i; - for (i=0; iprintf("RCOutput: invalid BRD_PWM_COUNT %u\n", mode_parm); - } else { - int fd = open("/dev/px4fmu", 0); - if (fd == -1) { - AP_HAL::panic("Unable to open /dev/px4fmu"); - } - if (ioctl(fd, PWM_SERVO_SET_MODE, mode_table[i].mode_value) != 0) { - hal.console->printf("RCOutput: unable to setup AUX PWM with BRD_PWM_COUNT %u\n", mode_parm); - } - close(fd); -#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 - if (mode_table[i].num_gpios < 2) { - // reduce change of config mistake where relay and PWM interfere - AP_Param::set_default_by_name("RELAY_PIN", -1); - AP_Param::set_default_by_name("RELAY_PIN2", -1); - } -#endif - } -#endif -} - -/* - setup safety switch - */ -void AP_BoardConfig::px4_setup_safety_mask() -{ -#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 && HAL_HAVE_SAFETY_SWITCH - // 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)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 = state.ignore_safety_channels; - if (px4io_fd != -1) { - mask >>= 8; - } - if (ioctl(px4fmu_fd, PWM_SERVO_IGNORE_SAFETY, (uint16_t)mask) != 0) { - hal.console->printf("IGNORE_SAFETY failed\n"); - } - close(px4fmu_fd); - } - if (px4io_fd != -1) { - close(px4io_fd); - } -#endif -} - -extern "C" int waitpid(pid_t, int *, int); - -/* - start one px4 driver - */ -bool AP_BoardConfig::px4_start_driver(main_fn_t main_function, const char *name, const char *arguments) -{ - char *s = strdup(arguments); - char *args[10]; - uint8_t nargs = 0; - char *saveptr = nullptr; - - // parse into separate arguments - for (char *tok=strtok_r(s, " ", &saveptr); tok; tok=strtok_r(nullptr, " ", &saveptr)) { - args[nargs++] = tok; - if (nargs == ARRAY_SIZE(args)-1) { - break; - } - } - args[nargs++] = nullptr; - - printf("Starting driver %s %s\n", name, arguments); - pid_t pid; - - if (task_spawn(&pid, name, main_function, nullptr, nullptr, - args, nullptr) != 0) { - free(s); - printf("Failed to spawn %s\n", name); - return false; - } - - // wait for task to exit and gather status - int status = -1; - if (waitpid(pid, &status, 0) != pid) { - printf("waitpid failed for %s\n", name); - free(s); - return false; - } - free(s); - return (status >> 8) == 0; -} - -/* - play a tune - */ -void AP_BoardConfig::px4_tone_alarm(const char *tone_string) -{ - px4_start_driver(tone_alarm_main, "tone_alarm", tone_string); -} - -/* - setup px4io, possibly updating firmware - */ -void AP_BoardConfig::px4_setup_px4io(void) -{ - if (px4_start_driver(px4io_main, "px4io", "start norc")) { - printf("px4io started OK\n"); - } else { - // might be in bootloader mode if user held down safety switch - // at power on - printf("Loading /etc/px4io/px4io.bin\n"); - px4_tone_alarm("MBABGP"); -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) - // we need to close uartC to prevent conflict between bootloader and - // uartC reada - hal.uartC->end(); -#endif - if (px4_start_driver(px4io_main, "px4io", "update /etc/px4io/px4io.bin")) { - printf("upgraded PX4IO firmware OK\n"); - px4_tone_alarm("MSPAA"); - } else { - printf("Failed to upgrade PX4IO firmware\n"); - px4_tone_alarm("MNGGG"); - } - hal.scheduler->delay(1000); - if (px4_start_driver(px4io_main, "px4io", "start norc")) { - printf("px4io started OK\n"); - } else { - sensor_config_error("px4io start failed"); - } - } - - /* - see if we need to update px4io firmware - */ - if (px4_start_driver(px4io_main, "px4io", "checkcrc /etc/px4io/px4io.bin")) { - printf("PX4IO CRC OK\n"); - } else { - printf("PX4IO CRC failure\n"); -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) - // we need to close uartC to prevent conflict between bootloader and - // uartC reada - hal.uartC->end(); -#endif - px4_tone_alarm("MBABGP"); - if (px4_start_driver(px4io_main, "px4io", "safety_on")) { - printf("PX4IO disarm OK\n"); - } else { - printf("PX4IO disarm failed\n"); - } - hal.scheduler->delay(1000); - - if (px4_start_driver(px4io_main, "px4io", "forceupdate 14662 /etc/px4io/px4io.bin")) { - hal.scheduler->delay(1000); - if (px4_start_driver(px4io_main, "px4io", "start norc")) { - printf("px4io restart OK\n"); - px4_tone_alarm("MSPAA"); - } else { - px4_tone_alarm("MNGGG"); - sensor_config_error("PX4IO restart failed"); - } - } else { - printf("PX4IO update failed\n"); - px4_tone_alarm("MNGGG"); - } - } -} - -/* - setup required peripherals like adc, rcinput and rcoutput - */ -void AP_BoardConfig::px4_setup_peripherals(void) -{ - // always start adc - if (px4_start_driver(adc_main, "adc", "start")) { - hal.analogin->init(); - printf("ADC started OK\n"); - } else { - sensor_config_error("no ADC found"); - } - -#if HAL_PX4_HAVE_PX4IO - if (state.io_enable.get() != 0) { - px4_setup_px4io(); - } -#endif - -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) - const char *fmu_mode = "mode_serial"; -#elif defined(CONFIG_ARCH_BOARD_AEROFC_V1) - const char *fmu_mode = "mode_rcin"; -#else -#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN - const char *fmu_mode = "mode_pwm"; -#else - const char *fmu_mode = "mode_pwm4"; -#endif -#endif - if (px4_start_driver(fmu_main, "fmu", fmu_mode)) { - printf("fmu %s started OK\n", fmu_mode); - } else { - sensor_config_error("fmu start failed"); - } - - hal.gpio->init(); - hal.rcin->init(); - hal.rcout->init(); -} - -#endif // HAL_BOARD_PX4