Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-01 13:38:38 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Periph: support SafetyState for safety LED

Browse Source
This commit is contained in:
Andrew Tridgell 2019-08-31 14:30:15 +10:00
parent 75696997cc
commit 0b6ef89498
2 changed files with 59 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

58
Tools/AP_Periph/can.cpp
View File

@ -33,6 +33,7 @@
#include <uavcan/equipment/air_data/StaticPressure.h>
#include <uavcan/equipment/air_data/StaticTemperature.h>
#include <uavcan/equipment/indication/BeepCommand.h>
#include <ardupilot/indication/SafetyState.h>
#include <uavcan/protocol/debug/LogMessage.h>
#include <stdio.h>
#include <AP_HAL_ChibiOS/hwdef/common/stm32_util.h>
@ -387,6 +388,49 @@ static void can_buzzer_update(void)
}
#endif // HAL_PERIPH_ENABLE_BUZZER
#ifdef HAL_GPIO_PIN_SAFE_LED
static uint8_t safety_state;
/*
handle SafetyState
*/
static void handle_safety_state(CanardInstance* ins, CanardRxTransfer* transfer)
{
ardupilot_indication_SafetyState req;
if (ardupilot_indication_SafetyState_decode(transfer, transfer->payload_len, &req, nullptr) < 0) {
return;
}
safety_state = req.status;
}
/*
update safety LED
*/
static void can_safety_LED_update(void)
{
static uint32_t last_update_ms;
switch (safety_state) {
case ARDUPILOT_INDICATION_SAFETYSTATE_STATUS_SAFETY_OFF:
palWriteLine(HAL_GPIO_PIN_SAFE_LED, SAFE_LED_ON);
break;
case ARDUPILOT_INDICATION_SAFETYSTATE_STATUS_SAFETY_ON: {
uint32_t now = AP_HAL::millis();
if (now - last_update_ms > 100) {
last_update_ms = now;
static uint8_t led_counter;
const uint16_t led_pattern = 0x5500;
led_counter = (led_counter+1) % 16;
palWriteLine(HAL_GPIO_PIN_SAFE_LED, (led_pattern & (1U << led_counter))?!SAFE_LED_ON:SAFE_LED_ON);
}
break;
}
default:
palWriteLine(HAL_GPIO_PIN_SAFE_LED, !SAFE_LED_ON);
break;
}
}
#endif // HAL_GPIO_PIN_SAFE_LED
/**
* This callback is invoked by the library when a new message or request or response is received.
*/
@ -433,6 +477,12 @@ static void onTransferReceived(CanardInstance* ins,
handle_beep_command(ins, transfer);
break;
#endif
#ifdef HAL_GPIO_PIN_SAFE_LED
case ARDUPILOT_INDICATION_SAFETYSTATE_ID:
handle_safety_state(ins, transfer);
break;
#endif
}
}
@ -486,6 +536,11 @@ static bool shouldAcceptTransfer(const CanardInstance* ins,
case UAVCAN_EQUIPMENT_INDICATION_BEEPCOMMAND_ID:
*out_data_type_signature = UAVCAN_EQUIPMENT_INDICATION_BEEPCOMMAND_SIGNATURE;
return true;
#endif
#ifdef HAL_GPIO_PIN_SAFE_LED
case ARDUPILOT_INDICATION_SAFETYSTATE_ID:
*out_data_type_signature = ARDUPILOT_INDICATION_SAFETYSTATE_SIGNATURE;
return true;
#endif
default:
break;
@ -724,6 +779,9 @@ void AP_Periph_FW::can_update()
can_baro_update();
#ifdef HAL_PERIPH_ENABLE_BUZZER
can_buzzer_update();
#endif
#ifdef HAL_GPIO_PIN_SAFE_LED
can_safety_LED_update();
#endif
processTx();
processRx();

3
Tools/AP_Periph/wscript
View File

@ -45,7 +45,6 @@ def build(bld):
bld(
# build libcanard headers
source=bld.path.ant_glob("modules/uavcan/dsdl/**/*.uavcan libraries/AP_UAVCAN/dsdl/**/*.uavcan"),
rule="python3 ../../modules/libcanard/dsdl_compiler/libcanard_dsdlc --header_only --outdir ${BUILDROOT}/modules/libcanard/dsdlc_generated ../../modules/uavcan/dsdl/uavcan",
rule="python3 ../../modules/libcanard/dsdl_compiler/libcanard_dsdlc --header_only --outdir ${BUILDROOT}/modules/libcanard/dsdlc_generated ../../modules/uavcan/dsdl/uavcan ../../libraries/AP_UAVCAN/dsdl/ardupilot ../../libraries/AP_UAVCAN/dsdl/com",
group='dynamic_sources',
)