AP_Periph: add support for handling Notify State message

Tools/AP_Bootloader/can.cpp

@@ -29,6 +29,8 @@
 #include <uavcan/protocol/NodeStatus.h>
 #include <uavcan/protocol/RestartNode.h>
 #include <uavcan/protocol/GetNodeInfo.h>
+#include <uavcan/equipment/indication/LightsCommand.h>
+#include <ardupilot/indication/NotifyState.h>
 #include "can.h"
 #include "bl_protocol.h"
 #include <drivers/stm32/canard_stm32.h>

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();
 }
 

Tools/AP_Periph/AP_Periph.h

@@ -16,7 +16,7 @@
 #include "../AP_Bootloader/app_comms.h"
 #include "hwing_esc.h"
 #include <AP_CANManager/AP_CANManager.h>
-
+#include <AP_Scripting/AP_Scripting.h>
 #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
 #include <AP_HAL_ChibiOS/CANIface.h>
 #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

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
 };
 

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;

Tools/AP_Periph/can.cpp

@@ -43,6 +43,7 @@
 #include <uavcan/equipment/esc/Status.h>
 #include <uavcan/equipment/safety/ArmingStatus.h>
 #include <ardupilot/indication/SafetyState.h>
+#include <ardupilot/indication/NotifyState.h>
 #include <ardupilot/indication/Button.h>
 #include <ardupilot/equipment/trafficmonitor/TrafficReport.h>
 #include <ardupilot/gnss/Status.h>
@@ -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;