123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417 |
- #include "HAL_Linux_Class.h"
- #include <assert.h>
- #include <signal.h>
- #include <stdio.h>
- #include <stdlib.h>
- #include <unistd.h>
- #include <AP_HAL/AP_HAL.h>
- #include <AP_HAL/utility/RCOutput_Tap.h>
- #include <AP_HAL/utility/getopt_cpp.h>
- #include <AP_HAL_Empty/AP_HAL_Empty.h>
- #include <AP_HAL_Empty/AP_HAL_Empty_Private.h>
- #include <AP_Module/AP_Module.h>
- #include "AnalogIn_ADS1115.h"
- #include "AnalogIn_IIO.h"
- #include "AnalogIn_Navio2.h"
- #include "GPIO.h"
- #include "I2CDevice.h"
- #include "OpticalFlow_Onboard.h"
- #include "RCInput.h"
- #include "RCInput_AioPRU.h"
- #include "RCInput_Navio2.h"
- #include "RCInput_PRU.h"
- #include "RCInput_RPI.h"
- #include "RCInput_SoloLink.h"
- #include "RCInput_UART.h"
- #include "RCInput_UDP.h"
- #include "RCInput_Multi.h"
- #include "RCInput_ZYNQ.h"
- #include "RCInput_RCProtocol.h"
- #include "RCOutput_AioPRU.h"
- #include "RCOutput_Bebop.h"
- #include "RCOutput_Disco.h"
- #include "RCOutput_PCA9685.h"
- #include "RCOutput_PRU.h"
- #include "RCOutput_Sysfs.h"
- #include "RCOutput_ZYNQ.h"
- #include "SPIDevice.h"
- #include "SPIUARTDriver.h"
- #include "Scheduler.h"
- #include "Storage.h"
- #include "UARTDriver.h"
- #include "Util.h"
- #include "Util_RPI.h"
- using namespace Linux;
- #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO || \
- CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO2 || \
- CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_EDGE || \
- CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBRAIN2 || \
- 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_NAVIGATOR
- static UtilRPI utilInstance;
- #else
- static Util utilInstance;
- #endif
- // 5 serial ports on Linux
- static UARTDriver uartADriver(true);
- static UARTDriver uartCDriver(false);
- static UARTDriver uartDDriver(false);
- static UARTDriver uartEDriver(false);
- static UARTDriver uartFDriver(false);
- static UARTDriver uartGDriver(false);
- static UARTDriver uartHDriver(false);
- static I2CDeviceManager i2c_mgr_instance;
- static SPIDeviceManager spi_mgr_instance;
- #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO || \
- CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO2 || \
- CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BH
- static SPIUARTDriver uartBDriver;
- #else
- static UARTDriver uartBDriver(false);
- #endif
- #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO || \
- CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBRAIN2 || \
- CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BH || \
- CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXFMINI || \
- CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIGATOR
- static AnalogIn_ADS1115 analogIn;
- #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF || \
- CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBOARD || \
- CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BBBMINI || \
- CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BLUE || \
- CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_POCKET
- static AnalogIn_IIO analogIn;
- #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO2 || \
- CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_EDGE
- static AnalogIn_Navio2 analogIn;
- #else
- static Empty::AnalogIn analogIn;
- #endif
- static Storage storageDriver;
- /*
- use the BBB gpio driver on ERLE, PXF, BBBMINI, BLUE and PocketPilot
- */
- #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF || \
- CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBOARD || \
- CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BBBMINI || \
- CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BLUE || \
- CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_POCKET
- static GPIO_BBB gpioDriver;
- /*
- use the RPI gpio driver on Navio
- */
- #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBRAIN2 || \
- 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_NAVIGATOR
- static GPIO_RPI gpioDriver;
- #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO || \
- CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO2 || \
- CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_EDGE
- static GPIO_Sysfs gpioDriver;
- #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP || \
- CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO || \
- CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_AERO
- static GPIO_Sysfs gpioDriver;
- #else
- static Empty::GPIO gpioDriver;
- #endif
- /*
- use the PRU based RCInput driver on ERLE and PXF
- */
- #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF || CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBOARD
- static RCInput_PRU rcinDriver;
- #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BBBMINI || \
- CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_POCKET
- static RCInput_AioPRU rcinDriver;
- #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BLUE
- static RCInput_Multi rcinDriver{2, new RCInput_AioPRU, new RCInput_RCProtocol(NULL, "/dev/ttyO4")};
- #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO || \
- CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBRAIN2 || \
- 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
- static RCInput_RPI rcinDriver;
- #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ZYNQ || \
- CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_OCPOC_ZYNQ
- static RCInput_ZYNQ rcinDriver;
- #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP
- static RCInput_UDP rcinDriver;
- #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO
- static RCInput_Multi rcinDriver{2, new RCInput_RCProtocol("/dev/uart-sbus", "/dev/uart-sumd"), new RCInput_UDP()};
- #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_AERO
- static RCInput_SoloLink rcinDriver;
- #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO2 || \
- CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_EDGE
- static RCInput_Navio2 rcinDriver;
- #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RST_ZYNQ
- static RCInput_RCProtocol rcinDriver{"/dev/ttyPS0", NULL};
- #else
- static RCInput rcinDriver;
- #endif
- /*
- use the PRU based RCOutput driver on ERLE and PXF
- */
- #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF || CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBOARD
- static RCOutput_PRU rcoutDriver;
- #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BBBMINI || \
- CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BLUE || \
- CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_POCKET
- static RCOutput_AioPRU rcoutDriver;
- /*
- use the PCA9685 based RCOutput driver on Navio and Erle-Brain 2
- */
- #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBRAIN2 || \
- CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXFMINI
- static RCOutput_PCA9685 rcoutDriver(i2c_mgr_instance.get_device(1, PCA9685_PRIMARY_ADDRESS), true, 3, RPI_GPIO_27);
- #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO
- static RCOutput_PCA9685 rcoutDriver(i2c_mgr_instance.get_device(1, PCA9685_PRIMARY_ADDRESS), true, 3, NAVIO_GPIO_PCA_OE);
- #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BH
- static RCOutput_PCA9685 rcoutDriver(i2c_mgr_instance.get_device(1, PCA9685_QUATENARY_ADDRESS), false, 0, RPI_GPIO_4);
- #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DARK
- static RCOutput_PCA9685 rcoutDriver(i2c_mgr_instance.get_device(1, PCA9685_QUINARY_ADDRESS), false, 0, RPI_GPIO_27);
- #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIGATOR
- static RCOutput_PCA9685 rcoutDriver(i2c_mgr_instance.get_device(1, PCA9685_PRIMARY_ADDRESS), false, 0, RPI_GPIO_27);
- #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ZYNQ || \
- CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_OCPOC_ZYNQ
- static RCOutput_ZYNQ rcoutDriver;
- #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP
- static RCOutput_Bebop rcoutDriver(i2c_mgr_instance.get_device(HAL_RCOUT_BEBOP_BLDC_I2C_BUS, HAL_RCOUT_BEBOP_BLDC_I2C_ADDR));
- #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO
- static RCOutput_Disco rcoutDriver(i2c_mgr_instance.get_device(HAL_RCOUT_DISCO_BLDC_I2C_BUS, HAL_RCOUT_DISCO_BLDC_I2C_ADDR));
- #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO2
- static RCOutput_Sysfs rcoutDriver(0, 0, 14);
- #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_AERO
- static ap::RCOutput_Tap rcoutDriver;
- #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_EDGE
- static RCOutput_Sysfs rcoutDriver(0, 0, 15);
- #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RST_ZYNQ
- static RCOutput_Sysfs rcoutDriver(0, 0, 8);
- #else
- static Empty::RCOutput rcoutDriver;
- #endif
- static Scheduler schedulerInstance;
- #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP
- static OpticalFlow_Onboard opticalFlow;
- #else
- static Empty::OpticalFlow opticalFlow;
- #endif
- static Empty::Flash flashDriver;
- HAL_Linux::HAL_Linux() :
- AP_HAL::HAL(
- &uartADriver,
- &uartBDriver,
- &uartCDriver,
- &uartDDriver,
- &uartEDriver,
- &uartFDriver,
- &uartGDriver,
- &uartHDriver,
- &i2c_mgr_instance,
- &spi_mgr_instance,
- &analogIn,
- &storageDriver,
- &uartADriver,
- &gpioDriver,
- &rcinDriver,
- &rcoutDriver,
- &schedulerInstance,
- &utilInstance,
- &opticalFlow,
- &flashDriver,
- nullptr)
- {}
- void _usage(void)
- {
- printf("Usage: -A uartAPath -B uartBPath -C uartCPath -D uartDPath -E uartEPath -F uartFPath -G uartGpath -H uartHpath\n");
- printf("Options:\n");
- printf("\tserial:\n");
- printf(" -A /dev/ttyO4\n");
- printf("\t -B /dev/ttyS1\n");
- printf("\tnetworking tcp:\n");
- printf("\t -C tcp:192.168.2.15:1243:wait\n");
- printf("\t -A tcp:11.0.0.2:5678\n");
- printf("\t -A udp:11.0.0.2:14550\n");
- printf("\tnetworking UDP:\n");
- printf("\t -A udp:11.0.0.255:14550:bcast\n");
- printf("\t -A udpin:0.0.0.0:14550\n");
- printf("\tcustom log path:\n");
- printf("\t --log-directory /var/APM/logs\n");
- printf("\t -l /var/APM/logs\n");
- printf("\tcustom terrain path:\n");
- printf("\t --terrain-directory /var/APM/terrain\n");
- printf("\t -t /var/APM/terrain\n");
- #if AP_MODULE_SUPPORTED
- printf("\tmodule support:\n");
- printf("\t --module-directory %s\n", AP_MODULE_DEFAULT_DIRECTORY);
- printf("\t -M %s\n", AP_MODULE_DEFAULT_DIRECTORY);
- #endif
- }
- void HAL_Linux::run(int argc, char* const argv[], Callbacks* callbacks) const
- {
- #if AP_MODULE_SUPPORTED
- const char *module_path = AP_MODULE_DEFAULT_DIRECTORY;
- #endif
-
- assert(callbacks);
- int opt;
- const struct GetOptLong::option options[] = {
- {"uartA", true, 0, 'A'},
- {"uartB", true, 0, 'B'},
- {"uartC", true, 0, 'C'},
- {"uartD", true, 0, 'D'},
- {"uartE", true, 0, 'E'},
- {"uartF", true, 0, 'F'},
- {"uartG", true, 0, 'G'},
- {"uartH", true, 0, 'H'},
- {"log-directory", true, 0, 'l'},
- {"terrain-directory", true, 0, 't'},
- {"storage-directory", true, 0, 's'},
- {"module-directory", true, 0, 'M'},
- {"help", false, 0, 'h'},
- {0, false, 0, 0}
- };
- GetOptLong gopt(argc, argv, "A:B:C:D:E:F:l:t:s:he:SM:",
- options);
- /*
- parse command line options
- */
- while ((opt = gopt.getoption()) != -1) {
- switch (opt) {
- case 'A':
- uartADriver.set_device_path(gopt.optarg);
- break;
- case 'B':
- uartBDriver.set_device_path(gopt.optarg);
- break;
- case 'C':
- uartCDriver.set_device_path(gopt.optarg);
- break;
- case 'D':
- uartDDriver.set_device_path(gopt.optarg);
- break;
- case 'E':
- uartEDriver.set_device_path(gopt.optarg);
- break;
- case 'F':
- uartFDriver.set_device_path(gopt.optarg);
- break;
- case 'G':
- uartGDriver.set_device_path(gopt.optarg);
- break;
- case 'H':
- uartHDriver.set_device_path(gopt.optarg);
- break;
- case 'l':
- utilInstance.set_custom_log_directory(gopt.optarg);
- break;
- case 't':
- utilInstance.set_custom_terrain_directory(gopt.optarg);
- break;
- case 's':
- utilInstance.set_custom_storage_directory(gopt.optarg);
- break;
- #if AP_MODULE_SUPPORTED
- case 'M':
- module_path = gopt.optarg;
- break;
- #endif
- case 'h':
- _usage();
- exit(0);
- default:
- printf("Unknown option '%c'\n", (char)opt);
- exit(1);
- }
- }
- setup_signal_handlers();
- scheduler->init();
- gpio->init();
- rcout->init();
- rcin->init();
- uartA->begin(115200);
- uartE->begin(115200);
- uartF->begin(115200);
- uartG->begin(115200);
- uartH->begin(115200);
- analogin->init();
- utilInstance.init(argc+gopt.optind-1, &argv[gopt.optind-1]);
- // NOTE: See commit 9f5b4ffca ("AP_HAL_Linux_Class: Correct
- // deadlock, and infinite loop in setup()") for details about the
- // order of scheduler initialize and setup on Linux.
- scheduler->system_initialized();
- // possibly load external modules
- #if AP_MODULE_SUPPORTED
- if (module_path != nullptr) {
- AP_Module::init(module_path);
- }
- #endif
- #if AP_MODULE_SUPPORTED
- AP_Module::call_hook_setup_start();
- #endif
- callbacks->setup();
- #if AP_MODULE_SUPPORTED
- AP_Module::call_hook_setup_complete();
- #endif
- while (!_should_exit) {
- callbacks->loop();
- }
- rcin->teardown();
- I2CDeviceManager::from(i2c_mgr)->teardown();
- SPIDeviceManager::from(spi)->teardown();
- Scheduler::from(scheduler)->teardown();
- }
- void HAL_Linux::setup_signal_handlers() const
- {
- struct sigaction sa = { };
- sa.sa_flags = SA_NOCLDSTOP;
- sa.sa_handler = HAL_Linux::exit_signal_handler;
- sigaction(SIGTERM, &sa, NULL);
- sigaction(SIGINT, &sa, NULL);
- }
- static HAL_Linux halInstance;
- void HAL_Linux::exit_signal_handler(int signum)
- {
- halInstance._should_exit = true;
- }
- const AP_HAL::HAL &AP_HAL::get_HAL()
- {
- return halInstance;
- }
|