diff --git a/Tools/AP_Bootloader/can.cpp b/Tools/AP_Bootloader/can.cpp index 17cbbac31e..756ff91163 100644 --- a/Tools/AP_Bootloader/can.cpp +++ b/Tools/AP_Bootloader/can.cpp @@ -29,6 +29,8 @@ #include #include #include +#include +#include #include "can.h" #include "bl_protocol.h" #include diff --git a/Tools/AP_Periph/AP_Periph.cpp b/Tools/AP_Periph/AP_Periph.cpp index 87acfa7309..64d0f56d2b 100644 --- a/Tools/AP_Periph/AP_Periph.cpp +++ b/Tools/AP_Periph/AP_Periph.cpp @@ -213,6 +213,9 @@ void AP_Periph_FW::init() notify.init(); #endif +#ifdef ENABLE_SCRIPTING + scripting.init(); +#endif start_ms = AP_HAL::native_millis(); } diff --git a/Tools/AP_Periph/AP_Periph.h b/Tools/AP_Periph/AP_Periph.h index 4bc39d3444..2a08832d47 100644 --- a/Tools/AP_Periph/AP_Periph.h +++ b/Tools/AP_Periph/AP_Periph.h @@ -16,7 +16,7 @@ #include "../AP_Bootloader/app_comms.h" #include "hwing_esc.h" #include - +#include #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS #include #elif CONFIG_HAL_BOARD == HAL_BOARD_SITL @@ -32,7 +32,7 @@ #endif #ifdef HAL_PERIPH_ENABLE_NOTIFY - #ifndef HAL_PERIPH_ENABLE_RC_OUT + #if !defined(HAL_PERIPH_ENABLE_RC_OUT) && !defined(HAL_PERIPH_NOTIFY_WITHOUT_RCOUT) #error "HAL_PERIPH_ENABLE_NOTIFY requires HAL_PERIPH_ENABLE_RC_OUT" #endif #ifdef HAL_PERIPH_ENABLE_BUZZER_WITHOUT_NOTIFY @@ -212,6 +212,21 @@ public: #ifdef HAL_PERIPH_ENABLE_NOTIFY // notification object for LEDs, buzzers etc AP_Notify notify; + uint64_t vehicle_state = 1; // default to initialisation + float yaw_earth; + uint32_t last_vehicle_state; + + // Handled under LUA script to control LEDs + float get_yaw_earth() { return yaw_earth; } + uint32_t get_vehicle_state() { return vehicle_state; } +#elif defined(ENABLE_SCRIPTING) + // create dummy methods for the case when the user doesn't want to use the notify object + float get_yaw_earth() { return 0.0; } + uint32_t get_vehicle_state() { return 0.0; } +#endif + +#ifdef ENABLE_SCRIPTING + AP_Scripting scripting; #endif #if HAL_LOGGING_ENABLED diff --git a/Tools/AP_Periph/Parameters.cpp b/Tools/AP_Periph/Parameters.cpp index 3e3b078bcd..b4f596f615 100644 --- a/Tools/AP_Periph/Parameters.cpp +++ b/Tools/AP_Periph/Parameters.cpp @@ -347,6 +347,11 @@ const AP_Param::Info AP_Periph_FW::var_info[] = { GOBJECT(serial_manager, "SERIAL", AP_SerialManager), #endif +#ifdef ENABLE_SCRIPTING + // @Group: SCR_ + // @Path: ../libraries/AP_Scripting/AP_Scripting.cpp + GOBJECT(scripting, "SCR_", AP_Scripting), +#endif AP_VAREND }; diff --git a/Tools/AP_Periph/Parameters.h b/Tools/AP_Periph/Parameters.h index 6af734ae65..690c312b5e 100644 --- a/Tools/AP_Periph/Parameters.h +++ b/Tools/AP_Periph/Parameters.h @@ -49,6 +49,7 @@ public: k_param_sysid_this_mav, k_param_serial_manager, k_param_gps_mb_only_can_port, + k_param_scripting, }; AP_Int16 format_version; diff --git a/Tools/AP_Periph/can.cpp b/Tools/AP_Periph/can.cpp index f3f0b699dd..b6ad204329 100644 --- a/Tools/AP_Periph/can.cpp +++ b/Tools/AP_Periph/can.cpp @@ -43,6 +43,7 @@ #include #include #include +#include #include #include #include @@ -687,7 +688,9 @@ static void set_rgb_led(uint8_t red, uint8_t green, uint8_t blue) { #ifdef HAL_PERIPH_ENABLE_NOTIFY periph.notify.handle_rgb(red, green, blue); +#ifdef HAL_PERIPH_ENABLE_RC_OUT periph.rcout_has_new_data_to_update = true; +#endif // HAL_PERIPH_ENABLE_RC_OUT #endif // HAL_PERIPH_ENABLE_NOTIFY #ifdef HAL_PERIPH_NEOPIXEL_COUNT_WITHOUT_NOTIFY @@ -849,6 +852,25 @@ static void handle_act_command(CanardInstance* ins, CanardRxTransfer* transfer) } #endif // HAL_PERIPH_ENABLE_RC_OUT +#if defined(HAL_PERIPH_ENABLE_NOTIFY) +static void handle_notify_state(CanardInstance* ins, CanardRxTransfer* transfer) +{ + ardupilot_indication_NotifyState msg; + uint8_t arraybuf[ARDUPILOT_INDICATION_NOTIFYSTATE_MAX_SIZE]; + uint8_t *arraybuf_ptr = arraybuf; + if (ardupilot_indication_NotifyState_decode(transfer, transfer->payload_len, &msg, &arraybuf_ptr) < 0) { + return; + } + if (msg.aux_data.len == 2 && msg.aux_data_type == ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_YAW_EARTH_CENTIDEGREES) { + uint16_t tmp = 0; + memcpy(&tmp, msg.aux_data.data, sizeof(tmp)); + periph.yaw_earth = radians((float)tmp * 0.01f); + } + periph.vehicle_state = msg.vehicle_state; + periph.last_vehicle_state = AP_HAL::millis(); +} +#endif // HAL_PERIPH_ENABLE_NOTIFY + #ifdef HAL_GPIO_PIN_SAFE_LED /* update safety LED @@ -1012,6 +1034,12 @@ static void onTransferReceived(CanardInstance* ins, handle_act_command(ins, transfer); break; #endif + +#ifdef HAL_PERIPH_ENABLE_NOTIFY + case ARDUPILOT_INDICATION_NOTIFYSTATE_ID: + handle_notify_state(ins, transfer); + break; +#endif } } @@ -1098,6 +1126,11 @@ static bool shouldAcceptTransfer(const CanardInstance* ins, case UAVCAN_EQUIPMENT_ACTUATOR_ARRAYCOMMAND_ID: *out_data_type_signature = UAVCAN_EQUIPMENT_ACTUATOR_ARRAYCOMMAND_SIGNATURE; return true; +#endif +#if defined(HAL_PERIPH_ENABLE_NOTIFY) + case ARDUPILOT_INDICATION_NOTIFYSTATE_ID: + *out_data_type_signature = ARDUPILOT_INDICATION_NOTIFYSTATE_SIGNATURE; + return true; #endif default: break;