diff --git a/Tools/AP_Periph/AP_Periph.cpp b/Tools/AP_Periph/AP_Periph.cpp index 312afa3876..2dbb809736 100644 --- a/Tools/AP_Periph/AP_Periph.cpp +++ b/Tools/AP_Periph/AP_Periph.cpp @@ -22,10 +22,12 @@ */ #include #include "AP_Periph.h" -#include "hal.h" #include + +#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS #include #include +#endif extern const AP_HAL::HAL &hal; @@ -36,6 +38,11 @@ void loop(); const AP_HAL::HAL& hal = AP_HAL::get_HAL(); +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL +void stm32_watchdog_init() {} +void stm32_watchdog_pat() {} +#endif + void setup(void) { periph.init(); @@ -51,10 +58,15 @@ static uint32_t start_ms; /* declare constant app_descriptor in flash */ +#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS const struct app_descriptor app_descriptor __attribute__((section(".app_descriptor"))); +#else +const struct app_descriptor app_descriptor; +#endif void AP_Periph_FW::init() { + // always run with watchdog enabled. This should have already been // setup by the bootloader, but if not then enable now stm32_watchdog_init(); @@ -208,7 +220,9 @@ void AP_Periph_FW::update() uint32_t now = AP_HAL::millis(); if (now - last_led_ms > 1000) { last_led_ms = now; +#ifdef HAL_GPIO_PIN_LED palToggleLine(HAL_GPIO_PIN_LED); +#endif #if 0 #ifdef HAL_PERIPH_ENABLE_GPS hal.uartA->printf("GPS status: %u\n", (unsigned)gps.status()); diff --git a/Tools/AP_Periph/AP_Periph.h b/Tools/AP_Periph/AP_Periph.h index e166e7c092..74129a8964 100644 --- a/Tools/AP_Periph/AP_Periph.h +++ b/Tools/AP_Periph/AP_Periph.h @@ -1,3 +1,5 @@ +#pragma once + #include #include #include @@ -15,8 +17,11 @@ #endif #include "Parameters.h" -#include "ch.h" +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL +void stm32_watchdog_init(); +void stm32_watchdog_pat(); +#endif /* app descriptor compatible with MissionPlanner */ diff --git a/Tools/AP_Periph/can.cpp b/Tools/AP_Periph/can.cpp index 13e34ccc20..76e5119541 100644 --- a/Tools/AP_Periph/can.cpp +++ b/Tools/AP_Periph/can.cpp @@ -18,7 +18,6 @@ #include #include #include "AP_Periph.h" -#include "hal.h" #include #include #include @@ -45,14 +44,20 @@ #include #include #include -#include -#include #include #include -#include "../AP_Bootloader/app_comms.h" #include #include + +#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS +#include "../AP_Bootloader/app_comms.h" #include +#include +#include +#elif CONFIG_HAL_BOARD == HAL_BOARD_SITL +#include +#endif + #include "i2c.h" #include @@ -80,8 +85,11 @@ static uint8_t transfer_id; #define CAN_PROBE_CONTINUOUS 0 #endif +#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS static ChibiOS::CANIface can_iface(0); - +#elif CONFIG_HAL_BOARD == HAL_BOARD_SITL +static HALSITL::CANIface can_iface(0); +#endif /* * Variables used for dynamic node ID allocation. * RTFM at http://uavcan.org/Specification/6._Application_level_functions/#dynamic-node-id-allocation @@ -366,8 +374,10 @@ static void handle_begin_firmware_update(CanardInstance* ins, CanardRxTransfer* // instant reboot, with backup register used to give bootloader // the node_id +#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS set_fast_reboot((rtc_boot_magic)(RTC_BOOT_CANBL | canardGetLocalNodeID(ins))); NVIC_SystemReset(); +#endif } static void handle_allocation_response(CanardInstance* ins, CanardRxTransfer* transfer) @@ -669,7 +679,11 @@ static void onTransferReceived(CanardInstance* ins, case UAVCAN_PROTOCOL_RESTARTNODE_ID: printf("RestartNode\n"); hal.scheduler->delay(10); +#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS NVIC_SystemReset(); +#elif CONFIG_HAL_BOARD == HAL_BOARD_SITL + HAL_SITL::actually_reboot(); +#endif break; case UAVCAN_PROTOCOL_PARAM_GETSET_ID: @@ -909,13 +923,14 @@ static void process1HzTasks(uint64_t timestamp_usec) while (true) ; } #endif - +#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS if (AP_HAL::millis() > 30000) { // use RTC to mark that we have been running fine for // 30s. This is used along with watchdog resets to ensure the // user has a chance to load a fixed firmware set_fast_reboot(RTC_BOOT_FWOK); } +#endif } /* @@ -950,7 +965,12 @@ static void can_wait_node_id(void) if (now - last_led_change > led_change_period) { // blink LED in recognisable pattern while waiting for DNA +#ifdef HAL_GPIO_PIN_LED palWriteLine(HAL_GPIO_PIN_LED, (led_pattern & (1U<