From a2140b50764e2378488932133a0fb7dd897c2a2e Mon Sep 17 00:00:00 2001 From: Daniel Hiepler Date: Sat, 28 Sep 2024 09:00:50 +0200 Subject: [PATCH] AP_HAL_Linux: add PilotPi support Adds "pilotpi" board to support https://docs.px4.io/main/en/flight_controller/raspberry_pi_pilotpi.html --- libraries/AP_HAL_Linux/GPIO.h | 2 + libraries/AP_HAL_Linux/GPIO_PilotPi.cpp | 74 +++++++++++++++++++ libraries/AP_HAL_Linux/GPIO_PilotPi.h | 62 ++++++++++++++++ libraries/AP_HAL_Linux/GPIO_RPI.cpp | 3 +- libraries/AP_HAL_Linux/HAL_Linux_Class.cpp | 12 ++- libraries/AP_HAL_Linux/RCInput_Multi.cpp | 3 +- libraries/AP_HAL_Linux/RCInput_Multi.h | 3 +- libraries/AP_HAL_Linux/RCInput_RCProtocol.cpp | 3 +- libraries/AP_HAL_Linux/RCInput_RCProtocol.h | 3 +- libraries/AP_HAL_Linux/SPIDevice.cpp | 4 + libraries/AP_HAL_Linux/Scheduler.cpp | 3 +- libraries/AP_HAL_Linux/Util_RPI.cpp | 3 +- 12 files changed, 166 insertions(+), 9 deletions(-) create mode 100644 libraries/AP_HAL_Linux/GPIO_PilotPi.cpp create mode 100644 libraries/AP_HAL_Linux/GPIO_PilotPi.h diff --git a/libraries/AP_HAL_Linux/GPIO.h b/libraries/AP_HAL_Linux/GPIO.h index 4b8ec5fa22..15dcf31d96 100644 --- a/libraries/AP_HAL_Linux/GPIO.h +++ b/libraries/AP_HAL_Linux/GPIO.h @@ -45,4 +45,6 @@ private: #include "GPIO_Disco.h" #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_AERO #include "GPIO_Aero.h" +#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PILOTPI +#include "GPIO_PilotPi.h" #endif diff --git a/libraries/AP_HAL_Linux/GPIO_PilotPi.cpp b/libraries/AP_HAL_Linux/GPIO_PilotPi.cpp new file mode 100644 index 0000000000..53c114f643 --- /dev/null +++ b/libraries/AP_HAL_Linux/GPIO_PilotPi.cpp @@ -0,0 +1,74 @@ +#include "GPIO_PilotPi.h" + +uint8_t GPIO_PilotPi::read(uint8_t pin) +{ + static uint8_t real_pin; + + /* MainOut (raspberry pi GPIOs)? */ + if(mainOutPin(pin, &real_pin)) { + return GPIO_RPI::read(real_pin); + } + /* AUXOUT? (rcout) */ + else if(auxOutPin(pin, &real_pin) && hal.rcout->supports_gpio()) { + return (uint8_t)hal.rcout->read(real_pin); + } + return 0; +} + +void GPIO_PilotPi::write(uint8_t pin, uint8_t value) +{ + static uint8_t real_pin; + + /* MainOut (raspberry pi GPIOs)? */ + if(mainOutPin(pin, &real_pin)) { + GPIO_RPI::write(real_pin, value); + return; + } + /* AUXOUT? (rcout) */ + else if(auxOutPin(pin, &real_pin) && hal.rcout->supports_gpio()) { + hal.rcout->write_gpio(real_pin, value); + } +} + +void GPIO_PilotPi::pinMode(uint8_t pin, uint8_t output) +{ + static uint8_t real_pin; + + /* MainOut (raspberry pi GPIOs)? */ + if(mainOutPin(pin, &real_pin)) { + GPIO_RPI::pinMode(real_pin, output); + } +} + +void GPIO_PilotPi::pinMode(uint8_t pin, uint8_t output, uint8_t alt) +{ + static uint8_t real_pin; + + /* MainOut (raspberry pi GPIOs)? */ + if(mainOutPin(pin, &real_pin)) { + GPIO_RPI::pinMode(real_pin, output, alt); + } +} + +/** convert ardupilot RELAYx_PIN to raspberry pi gpio pin */ +bool GPIO_PilotPi::mainOutPin(uint8_t ap_pin, uint8_t *pin) { + if(100 >= ap_pin || ap_pin >= 200) { + return false; + } + /* GPIO available? */ + if(ap_pin-101 >= (uint8_t) sizeof(PilotPiGPIOS)) { + return false; + } + + *pin = PilotPiGPIOS[ap_pin-101]; + return true; +} + +/* convert ardupilot RELAYx_PIN to RCOUT GPIO pin */ +bool GPIO_PilotPi::auxOutPin(uint8_t ap_pin, uint8_t *pin) { + if(50 > ap_pin || ap_pin > 100) { + return false; + } + *pin = ap_pin-50; + return true; +} diff --git a/libraries/AP_HAL_Linux/GPIO_PilotPi.h b/libraries/AP_HAL_Linux/GPIO_PilotPi.h new file mode 100644 index 0000000000..44abacd90b --- /dev/null +++ b/libraries/AP_HAL_Linux/GPIO_PilotPi.h @@ -0,0 +1,62 @@ +#pragma once +#include "GPIO_RPI.h" + +using namespace Linux; + +extern const AP_HAL::HAL& hal; + +/** + * Provide unused Raspberry PI pins as GPIOs in the order of the PCB pinheader: + * AP GPIO Pin RPI BCM GPIO + * ============================= + * 101 4 + * 102 14 + * 103 17 + * 104 27 + * 105 22 + * 106 23 + * 107 7 + * 108 5 + * 109 6 + * 110 12 + * 111 13 + * 112 16 + * 113 26 + * + * and those SERVO outputs that are configured as GPIO: + * AP GPIO Pin SERVO + * ============================== + * 50 0 + * 51 1 + * ... + */ +class GPIO_PilotPi : public GPIO_RPI +{ + +public: + void pinMode(uint8_t pin, uint8_t output) override; + void pinMode(uint8_t pin, uint8_t output, uint8_t alt) override; + uint8_t read(uint8_t pin) override; + void write(uint8_t pin, uint8_t value) override; + +private: + uint8_t PilotPiGPIOS[13] = { + RPI_GPIO_<4>(), + RPI_GPIO_<14>(), + RPI_GPIO_<17>(), + RPI_GPIO_<27>(), + RPI_GPIO_<22>(), + RPI_GPIO_<23>(), + RPI_GPIO_<7>(), + RPI_GPIO_<5>(), + RPI_GPIO_<6>(), + RPI_GPIO_<12>(), + RPI_GPIO_<13>(), + RPI_GPIO_<16>(), + RPI_GPIO_<26>() + }; + bool mainOutPin(uint8_t ap_pin, uint8_t *pin); + bool auxOutPin(uint8_t ap_pin, uint8_t *pin); +}; + + diff --git a/libraries/AP_HAL_Linux/GPIO_RPI.cpp b/libraries/AP_HAL_Linux/GPIO_RPI.cpp index 68d0bc9ea0..9e4adbcf53 100644 --- a/libraries/AP_HAL_Linux/GPIO_RPI.cpp +++ b/libraries/AP_HAL_Linux/GPIO_RPI.cpp @@ -6,7 +6,8 @@ CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXFMINI || \ CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIGATOR || \ CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_OBAL_V1 || \ - CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_CANZERO + CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_CANZERO || \ + CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PILOTPI #include "GPIO.h" #include "GPIO_RPI.h" diff --git a/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp b/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp index bd7dc4122d..6b086da5a1 100644 --- a/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp +++ b/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp @@ -58,7 +58,8 @@ using namespace Linux; CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXFMINI || \ CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIGATOR || \ CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_OBAL_V1 || \ - CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_CANZERO + CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_CANZERO || \ + CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PILOTPI static UtilRPI utilInstance; #else static Util utilInstance; @@ -106,7 +107,8 @@ static UARTDriver* serialDrivers[] = { CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXFMINI || \ CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIGATOR|| \ ((CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_OBAL_V1) && (OBAL_ALLOW_ADC ==1)) || \ - CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_CANZERO + CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_CANZERO || \ + CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PILOTPI static AnalogIn_ADS1115 analogIn; #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF || \ CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBOARD || \ @@ -152,6 +154,8 @@ static GPIO_Sysfs gpioDriver; CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO || \ CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_AERO static GPIO_Sysfs gpioDriver; +#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PILOTPI +static GPIO_PilotPi gpioDriver; #else static Empty::GPIO gpioDriver; #endif @@ -188,6 +192,8 @@ static RCInput_SoloLink rcinDriver; static RCInput_Navio2 rcinDriver; #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RST_ZYNQ static RCInput_RCProtocol rcinDriver{"/dev/ttyPS0", NULL}; +#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PILOTPI +static RCInput_Multi rcinDriver{2, NEW_NOTHROW RCInput_RCProtocol(NULL, NULL), NEW_NOTHROW RCInput_UDP()}; #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_VNAV || \ CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIGATOR // this is needed to allow for RC input using SERIALn_PROTOCOL=23. No fd is opened @@ -239,6 +245,8 @@ static RCOutput_Sysfs rcoutDriver(0, 0, 8); static RCOutput_PCA9685 rcoutDriver(i2c_mgr_instance.get_device(1, PCA9685_PRIMARY_ADDRESS), 0, 0, RPI_GPIO_<17>()); #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_CANZERO static RCOutput_Sysfs rcoutDriver(0, 0, 2); +#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PILOTPI +static RCOutput_PCA9685 rcoutDriver(i2c_mgr_instance.get_device(1, PCA9685_PRIMARY_ADDRESS), 24576000, 0, -1); #else static Empty::RCOutput rcoutDriver; #endif diff --git a/libraries/AP_HAL_Linux/RCInput_Multi.cpp b/libraries/AP_HAL_Linux/RCInput_Multi.cpp index 093f513c0b..6dad386952 100644 --- a/libraries/AP_HAL_Linux/RCInput_Multi.cpp +++ b/libraries/AP_HAL_Linux/RCInput_Multi.cpp @@ -19,7 +19,8 @@ #include #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO || \ - CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BLUE + CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BLUE || \ + CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PILOTPI #include "RCInput_Multi.h" extern const AP_HAL::HAL& hal; diff --git a/libraries/AP_HAL_Linux/RCInput_Multi.h b/libraries/AP_HAL_Linux/RCInput_Multi.h index c2fabf0509..7df2752cce 100644 --- a/libraries/AP_HAL_Linux/RCInput_Multi.h +++ b/libraries/AP_HAL_Linux/RCInput_Multi.h @@ -17,7 +17,8 @@ #include #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO || \ - CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BLUE + CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BLUE || \ + CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PILOTPI #include "RCInput.h" #include diff --git a/libraries/AP_HAL_Linux/RCInput_RCProtocol.cpp b/libraries/AP_HAL_Linux/RCInput_RCProtocol.cpp index 73cc14c0f7..56e3f1d902 100644 --- a/libraries/AP_HAL_Linux/RCInput_RCProtocol.cpp +++ b/libraries/AP_HAL_Linux/RCInput_RCProtocol.cpp @@ -34,7 +34,8 @@ #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO || \ CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BLUE || \ CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_VNAV || \ - CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIGATOR + CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIGATOR || \ + CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PILOTPI extern const AP_HAL::HAL& hal; diff --git a/libraries/AP_HAL_Linux/RCInput_RCProtocol.h b/libraries/AP_HAL_Linux/RCInput_RCProtocol.h index 0f64683459..ff52986cf8 100644 --- a/libraries/AP_HAL_Linux/RCInput_RCProtocol.h +++ b/libraries/AP_HAL_Linux/RCInput_RCProtocol.h @@ -23,7 +23,8 @@ #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO || \ CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BLUE || \ CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_VNAV || \ - CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIGATOR + CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIGATOR || \ + CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PILOTPI namespace Linux { diff --git a/libraries/AP_HAL_Linux/SPIDevice.cpp b/libraries/AP_HAL_Linux/SPIDevice.cpp index 1f66cc7231..5c4802b33e 100644 --- a/libraries/AP_HAL_Linux/SPIDevice.cpp +++ b/libraries/AP_HAL_Linux/SPIDevice.cpp @@ -102,6 +102,10 @@ SPIDesc SPIDeviceManager::_device[] = { SPIDesc("mpu9250", 0, 1, SPI_MODE_0, 8, SPI_CS_KERNEL, 1*MHZ, 11*MHZ), SPIDesc("ms5611", 0, 0, SPI_MODE_0, 8, SPI_CS_KERNEL, 10*MHZ, 10*MHZ), }; +#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PILOTPI +SPIDesc SPIDeviceManager::_device[] = { + SPIDesc("icm42605", 0, 0, SPI_MODE_0, 8, SPI_CS_KERNEL, 1*MHZ, 24*MHZ), +}; #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BBBMINI SPIDesc SPIDeviceManager::_device[] = { SPIDesc("mpu9250", 2, 0, SPI_MODE_3, 8, SPI_CS_KERNEL, 1*MHZ, 11*MHZ), diff --git a/libraries/AP_HAL_Linux/Scheduler.cpp b/libraries/AP_HAL_Linux/Scheduler.cpp index 8e35ba97b4..7893bf6cac 100644 --- a/libraries/AP_HAL_Linux/Scheduler.cpp +++ b/libraries/AP_HAL_Linux/Scheduler.cpp @@ -39,7 +39,8 @@ extern const AP_HAL::HAL& hal; CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BH || \ CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DARK || \ CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXFMINI || \ - CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_CANZERO + CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_CANZERO || \ + CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PILOTPI #define APM_LINUX_RCIN_RATE 500 #define APM_LINUX_IO_RATE 50 #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_OBAL_V1 diff --git a/libraries/AP_HAL_Linux/Util_RPI.cpp b/libraries/AP_HAL_Linux/Util_RPI.cpp index f39606675e..20afb76316 100644 --- a/libraries/AP_HAL_Linux/Util_RPI.cpp +++ b/libraries/AP_HAL_Linux/Util_RPI.cpp @@ -9,7 +9,8 @@ CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXFMINI || \ CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIGATOR || \ CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_OBAL_V1 || \ - CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_CANZERO + CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_CANZERO || \ + CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PILOTPI #include #include