增加UAVCAN RGB LED

AP_Notify:增加set_rgb_mode虚函数,传入灯模式;ToneAlarm修改提示音
AP_UAVCAN:增加灯相关的uavcan函数
APM_Config:Disable一些没用到的模块

ArduCopter/APM_Config.h

@@ -13,18 +13,18 @@
 //#define PARACHUTE             DISABLED            // disable parachute release to save 1k of flash
 //#define NAV_GUIDED            DISABLED            // disable external navigation computer ability to control vehicle through MAV_CMD_NAV_GUIDED mission commands
 //#define PRECISION_LANDING     DISABLED            // disable precision landing using companion computer or IRLock sensor
-//#define BEACON_ENABLED        DISABLED            // disable beacon support
-//#define SPRAYER_ENABLED       DISABLED            // disable the crop sprayer feature (two ESC controlled pumps the speed of which depends upon the vehicle's horizontal velocity)
+#define BEACON_ENABLED        DISABLED            // disable beacon support
+#define SPRAYER_ENABLED       DISABLED            // disable the crop sprayer feature (two ESC controlled pumps the speed of which depends upon the vehicle's horizontal velocity)
 //#define WINCH_ENABLED         DISABLED            // disable winch support
-//#define GRIPPER_ENABLED       DISABLED            // disable gripper support
+#define GRIPPER_ENABLED       DISABLED            // disable gripper support
 //#define RPM_ENABLED           DISABLED            // disable rotations per minute sensor support
 //#define STATS_ENABLED         DISABLED            // disable statistics support
 //#define MODE_ACRO_ENABLED     DISABLED            // disable acrobatic mode support
 //#define MODE_AUTO_ENABLED     DISABLED            // disable auto mode support
 //#define MODE_BRAKE_ENABLED    DISABLED            // disable brake mode support
 //#define MODE_CIRCLE_ENABLED   DISABLED            // disable circle mode support
-//#define MODE_DRIFT_ENABLED    DISABLED            // disable drift mode support
-//#define MODE_FLIP_ENABLED     DISABLED            // disable flip mode support
+#define MODE_DRIFT_ENABLED    DISABLED            // disable drift mode support
+#define MODE_FLIP_ENABLED     DISABLED            // disable flip mode support
 //#define MODE_FOLLOW_ENABLED   DISABLED            // disable follow mode support
 //#define MODE_GUIDED_ENABLED   DISABLED            // disable guided mode support
 //#define MODE_GUIDED_NOGPS_ENABLED   DISABLED      // disable guided/nogps mode support
@@ -34,8 +34,8 @@
 //#define MODE_SMARTRTL_ENABLED DISABLED            // disable smartrtl mode support
 //#define MODE_SPORT_ENABLED    DISABLED            // disable sport mode support
 //#define MODE_SYSTEMID_ENABLED DISABLED            // disable system ID mode support
-//#define MODE_THROW_ENABLED    DISABLED            // disable throw mode support
-//#define MODE_ZIGZAG_ENABLED   DISABLED            // disable zigzag mode support
+#define MODE_THROW_ENABLED    DISABLED            // disable throw mode support
+#define MODE_ZIGZAG_ENABLED   DISABLED            // disable zigzag mode support
 //#define OSD_ENABLED           DISABLED            // disable on-screen-display support
 //#define LANDING_GEAR_ENABLED  DISABLED            // disable landing gear support
 

just_build.sh

@@ -1,2 +1,2 @@
-./waf configure --board Pixhawk4
-./waf  copter
+./waf  copter
+cp build/Pixhawk4/bin/arducopter.apj /mnt/hgfs/firmware/编译/c415.px4

libraries/AP_Notify/AP_Notify.cpp

@@ -40,6 +40,8 @@
 #include "ScriptingLED.h"
 #include "DShotLED.h"
 
+#include <GCS_MAVLink/GCS.h>
+
 extern const AP_HAL::HAL& hal;
 
 AP_Notify *AP_Notify::_singleton;
@@ -252,7 +254,7 @@ void AP_Notify::add_backends(void)
     }
 
     for (uint32_t i = 1; i < Notify_LED_MAX; i = i << 1) {
-        switch(_led_type & i) {
+        switch(_led_type & i) {            
             case Notify_LED_None:
                 break;
             case Notify_LED_Board:
@@ -422,6 +424,15 @@ void AP_Notify::handle_led_control(const mavlink_message_t &msg)
     }
 }
 
+void AP_Notify::set_rgb_mode(uint8_t mode) 
+{
+    for (uint8_t i = 0; i < _num_devices; i++) {
+        if (_devices[i] != nullptr) {
+            _devices[i]->set_rgb_mode(mode);
+        }
+    }
+}
+
 // handle RGB from Scripting or AP_Periph
 void AP_Notify::handle_rgb(uint8_t r, uint8_t g, uint8_t b, uint8_t rate_hz)
 {

libraries/AP_Notify/AP_Notify.h

@@ -186,6 +186,8 @@ public:
     uint8_t get_buzz_volume() const  { return _buzzer_volume; }
     uint8_t get_led_len() const { return _led_len; }
     int8_t get_rgb_led_brightness_percent() const;
+    
+    static void set_rgb_mode(uint8_t mode); // @Brown
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
     HAL_Semaphore sf_window_mutex;

libraries/AP_Notify/NotifyDevice.h

@@ -30,6 +30,8 @@ public:
     // RGB control multiple leds independently
     // give RGB value for single led
     virtual void rgb_set_id(uint8_t r, uint8_t g, uint8_t b, uint8_t id) {}
+    
+    virtual void set_rgb_mode(uint8_t mode)  {}   // @Brown
 
     // this pointer is used to read the parameters relative to devices
     const AP_Notify *pNotify;

libraries/AP_Notify/RGBLed.cpp

@@ -35,16 +35,25 @@ RGBLed::RGBLed(uint8_t led_off, uint8_t led_bright, uint8_t led_medium, uint8_t
 // set_rgb - set color as a combination of red, green and blue values
 void RGBLed::_set_rgb(uint8_t red, uint8_t green, uint8_t blue)
 {
-    if (red != _red_curr ||
-        green != _green_curr ||
-        blue != _blue_curr) {
-        // call the hardware update routine
+    if(rgb_source() == use_ws2812){
         if (hw_set_rgb(red, green, blue)) {
             _red_curr = red;
             _green_curr = green;
             _blue_curr = blue;
         }
+    }else{
+        if (red != _red_curr ||
+            green != _green_curr ||
+            blue != _blue_curr) {
+            // call the hardware update routine
+            if (hw_set_rgb(red, green, blue)) {
+                _red_curr = red;
+                _green_curr = green;
+                _blue_curr = blue;
+            }
+        }
     }
+
 }
 
 RGBLed::rgb_source_t RGBLed::rgb_source() const
@@ -204,6 +213,9 @@ void RGBLed::update()
     case traffic_light:
         current_colour_sequence = get_colour_sequence_traffic_light();
         break;
+    case use_ws2812:
+        get_colour_ws2812();
+        break;
     }
 
     const uint8_t brightness = get_brightness();
@@ -218,10 +230,19 @@ void RGBLed::update()
 
     const uint8_t colour = (current_colour_sequence >> (step*3)) & 7;
 
-    uint8_t red_des = (colour & RED) ? brightness : _led_off;
-    uint8_t green_des = (colour & GREEN) ? brightness : _led_off;
-    uint8_t blue_des = (colour & BLUE) ? brightness : _led_off;
-
+    uint8_t red_des ;
+    uint8_t green_des ;
+    uint8_t blue_des;
+
+    if(rgb_source() == use_ws2812){
+        red_des = (_red_ws !=  _led_off) ? brightness : 0;
+        green_des = (_green_ws !=  _led_off) ? brightness : 0;
+        blue_des = (_blue_ws !=  _led_off) ? brightness : 0;
+    }else{
+        red_des = (colour & RED) ? brightness : _led_off;
+        green_des = (colour & GREEN) ? brightness : _led_off;
+        blue_des = (colour & BLUE) ? brightness : _led_off;
+    }
     set_rgb(red_des, green_des, blue_des);
 }
 
@@ -292,3 +313,123 @@ void RGBLed::rgb_control(uint8_t r, uint8_t g, uint8_t b, uint8_t rate_hz)
     _led_override.g = g;
     _led_override.b = b;
 }
+
+void RGBLed::set_rgb_mode(uint8_t mode)
+{
+    if (!pNotify->_rgb_led_override) {
+        // ignore LED_CONTROL commands if not in LED_OVERRIDE mode
+        return;
+    }
+}
+
+void RGBLed::get_colour_ws2812(void) 
+{
+    // initialising pattern
+    if (AP_Notify::flags.initialising) {
+        // return sequence_initialising;
+        _red_ws = _led_medium;
+        _blue_ws = _led_off;
+        _green_ws = _led_medium;
+        set_rgb_mode(1);
+        return;
+    }
+
+    // save trim and esc calibration pattern
+    if (AP_Notify::flags.save_trim || AP_Notify::flags.esc_calibration) {
+        // return sequence_trim_or_esc;
+        _red_ws = _led_medium;
+        _blue_ws = _led_medium;
+        _green_ws = _led_medium;
+        set_rgb_mode(1);  // flow
+        return;
+    }
+
+    // radio and battery failsafe patter: flash yellow
+    // gps failsafe pattern : flashing yellow and blue
+    // ekf_bad pattern : flashing yellow and red
+    if (AP_Notify::flags.failsafe_radio ||
+        AP_Notify::flags.failsafe_battery ||
+        AP_Notify::flags.ekf_bad ||
+        AP_Notify::flags.gps_glitching ||
+        AP_Notify::flags.leak_detected) {
+
+        if (AP_Notify::flags.leak_detected) {
+            // purple if leak detected
+            // return sequence_failsafe_leak;
+        } else if (AP_Notify::flags.ekf_bad) {
+            // red on if ekf bad
+            // return sequence_failsafe_ekf;
+        } else if (AP_Notify::flags.gps_glitching) {
+            // blue on gps glitch
+            // return sequence_failsafe_gps_glitching;
+        }
+        // all off for radio or battery failsafe
+        // return sequence_failsafe_radio_or_battery;
+        _red_ws = _led_medium;
+        _blue_ws = _led_medium;
+        _green_ws = _led_off;
+        set_rgb_mode(1);
+        return;
+    }
+    static uint16_t arm_delay_cnt;
+    // solid green or blue if armed
+    if (AP_Notify::flags.armed) {
+        // solid green if armed with GPS 3d lock
+        if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D) {
+            // return sequence_armed;
+            _red_ws = _led_off;
+            _blue_ws = _led_off;
+            _green_ws = _led_medium;
+        }
+        // solid blue if armed with no GPS lock
+        // return sequence_armed_nogps;
+        else{    _red_ws = _led_off;
+            _blue_ws = _led_medium;
+            _green_ws = _led_medium;
+            set_rgb_mode(0);}
+        
+        if(arm_delay_cnt>500){
+             _red_ws = _led_off;
+            _blue_ws = _led_off;
+            _green_ws = _led_off;
+            set_rgb_mode(0);
+        }else{
+           arm_delay_cnt+=1;
+
+        }
+        return;
+    }else{
+        arm_delay_cnt = 0;
+    }
+
+    // double flash yellow if failing pre-arm checks
+    if (!AP_Notify::flags.pre_arm_check) {
+        // return sequence_prearm_failing;
+            _red_ws = _led_medium;
+            _blue_ws = _led_off;
+            _green_ws = _led_medium;
+            set_rgb_mode(1);
+        return;
+    }
+    if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D_DGPS && AP_Notify::flags.pre_arm_gps_check) {
+        // return sequence_disarmed_good_dgps;
+                _red_ws = _led_off;
+                _blue_ws = _led_off;
+                _green_ws = _led_medium;
+                set_rgb_mode(2); //  green breath 
+    }else if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D && AP_Notify::flags.pre_arm_gps_check) {
+        // return sequence_disarmed_good_gps;
+                _red_ws = _led_off;
+                _blue_ws = _led_off;
+                _green_ws = _led_medium;
+                set_rgb_mode(2); //  blue green breath 
+    }else{
+                _red_ws = _led_off;
+                _blue_ws = _led_medium;
+                _green_ws = _led_medium;
+                set_rgb_mode(2); //  blue breath 
+
+    }
+
+    // return sequence_disarmed_bad_gps;
+}

libraries/AP_Notify/RGBLed.h

@@ -39,6 +39,8 @@ public:
     // give RGB and flash rate, used with scripting
     virtual void rgb_control(uint8_t r, uint8_t g, uint8_t b, uint8_t rate_hz) override;
 
+    virtual void set_rgb_mode(uint8_t mode) override;  // @Brown
+
 protected:
     // methods implemented in hardware specific classes
     virtual bool hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue) = 0;
@@ -60,6 +62,8 @@ protected:
         uint8_t rate_hz;
         uint32_t start_ms;
     } _led_override;
+
+    uint8_t _red_ws, _green_ws, _blue_ws;     // color requested by timed update, @Brown
     
 private:
     void update_colours();
@@ -68,6 +72,7 @@ private:
     uint32_t get_colour_sequence_traffic_light() const;
 
     uint8_t get_brightness(void) const;
+    void get_colour_ws2812();
 
 #define DEFINE_COLOUR_SEQUENCE(S0, S1, S2, S3, S4, S5, S6, S7, S8, S9)  \
     ((S0) << (0*3) | (S1) << (1*3) | (S2) << (2*3) | (S3) << (3*3) | (S4) << (4*3) | (S5) << (5*3) | (S6) << (6*3) | (S7) << (7*3) | (S8) << (8*3) | (S9) << (9*3))
@@ -108,6 +113,7 @@ private:
         mavlink = 1,
         obc = 2,
         traffic_light = 3,
+        use_ws2812 = 4
     };
     rgb_source_t rgb_source() const;
 

libraries/AP_Notify/ToneAlarm.cpp

@@ -47,9 +47,11 @@ const AP_ToneAlarm::Tone AP_ToneAlarm::_tones[] {
 #define AP_NOTIFY_TONE_LOUD_POS_FEEDBACK 5
     { "MFT100L4>A#B#", false },
 #define AP_NOTIFY_TONE_LOUD_READY_OR_FINISHED 6
-    { "MFT100L4>G#6A#6B#4", false },
+    // { "MFT100L4>G#6A#6B#4", false },
+    { "MFT200L4<G#6A#6B#6O4C#6D#4O3G#6A#6B#6O4C#6D#1", false }, // modify by @Brown 
 #define AP_NOTIFY_TONE_QUIET_READY_OR_FINISHED 7
-    { "MFT200L4<G#6A#6B#4", false },
+    // { "MFT200L4<G#6A#6B#4", false },
+    { "MFT200L4<G#6A#6B#6O4C#6D#4O3G#6A#6B#6O4C#6D#1", false },
 #define AP_NOTIFY_TONE_LOUD_ATTENTION_NEEDED 8
     { "MFT100L4>A#A#A#A#", false },
 #define AP_NOTIFY_TONE_QUIET_ARMING_WARNING 9
@@ -93,7 +95,8 @@ const AP_ToneAlarm::Tone AP_ToneAlarm::_tones[] {
 #define AP_NOTIFY_TONE_QUIET_NOT_READY_OR_NOT_FINISHED 28
     { "MFT200L4<B#4A#6G#6", false },
 #define AP_NOTIFY_TONE_STARTUP 29
-    { "MFT240L8O4aO5dcO4aO5dcO4aO5dcL16dcdcdcdc", false },
+    // { "MFT240L8O4aO5dcO4aO5dcO4aO5dcL16dcdcdcdc", false },
+    { "MFT240L8 O4L4geL2b", false }, // modify by @Brown 
 #define AP_NOTIFY_TONE_NO_SDCARD 30
     { "MNBGG", false },
 #define AP_NOTIFY_TONE_EKF_ALERT 31

libraries/AP_Notify/ToshibaLED_I2C.cpp

@@ -95,3 +95,8 @@ void ToshibaLED_I2C::_timer(void)
 
     _dev->transfer(val, sizeof(val), nullptr, 0);
 }
+
+void ToshibaLED_I2C::set_rgb_mode(uint8_t mode)
+{
+    return;
+}

libraries/AP_Notify/ToshibaLED_I2C.h

@@ -26,6 +26,7 @@ public:
     bool init(void) override;
 protected:
     bool hw_set_rgb(uint8_t r, uint8_t g, uint8_t b) override;
+    void set_rgb_mode(uint8_t mode) override;
 
 private:
     AP_HAL::OwnPtr<AP_HAL::I2CDevice> _dev;

libraries/AP_Notify/UAVCAN_RGB_LED.cpp

@@ -19,9 +19,7 @@
 
 #if HAL_CANMANAGER_ENABLED
 #include "UAVCAN_RGB_LED.h"
-
 #include <AP_UAVCAN/AP_UAVCAN.h>
-
 #include <AP_CANManager/AP_CANManager.h>
 
 #define LED_OFF 0
@@ -62,12 +60,34 @@ bool UAVCAN_RGB_LED::hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue)
     bool success = false;
     uint8_t can_num_drivers = AP::can().get_num_drivers();
 
+    static uint8_t cnt;
+    static uint8_t l_mode,l_red,l_green,l_blue;
+    rgb = {red, green, blue};
+    
+    cnt++;
+    if(l_mode != rgb_mode || l_red != rgb.r || l_green != rgb.g || l_blue != rgb.b || cnt > 50)
+    {
+        cnt =0;
+        _need_update = true;
+        l_mode = rgb_mode ;
+        l_red = rgb.r ;
+        l_green = rgb.g ;
+        l_blue = rgb.b;
+    }else{
+        return success;
+    }
+
     for (uint8_t i = 0; i < can_num_drivers; i++) {
         AP_UAVCAN *uavcan = AP_UAVCAN::get_uavcan(i);
         if (uavcan != nullptr) {
-            success = uavcan->led_write(_led_index, red, green, blue) || success;
+            // success = uavcan->led_write(_led_index, red, green, blue) || success;
+            success = uavcan->set_zr_rgb_led(rgb_mode,red, green, blue) || success;//(_led_index, red, green, blue) || success
         }
     }
     return success;
 }
+void UAVCAN_RGB_LED::set_rgb_mode(uint8_t mode)
+{
+    rgb_mode = mode;
+}
 #endif

libraries/AP_Notify/UAVCAN_RGB_LED.h

@@ -11,8 +11,13 @@ public:
 
 protected:
     virtual bool hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue) override;
-
+    void set_rgb_mode(uint8_t mode) override;
 private:
     uint8_t _led_index;
+     uint8_t rgb_mode;
+    bool _need_update;
+    struct {
+        uint8_t r, g, b;
+    } rgb;
 };
 

libraries/AP_UAVCAN/AP_UAVCAN.cpp

@@ -61,6 +61,8 @@
 #include <AP_Notify/AP_Notify.h>
 #include "AP_UAVCAN_pool.h"
 
+#include <zrzk/equipment/indication/ZrRGB.hpp>
+
 #define LED_DELAY_US 50000
 
 extern const AP_HAL::HAL& hal;
@@ -175,6 +177,8 @@ static uavcan::ServiceClient<uavcan::protocol::param::ExecuteOpcode, ParamExecut
 static uavcan::protocol::param::ExecuteOpcode::Request param_save_req[HAL_MAX_CAN_PROTOCOL_DRIVERS];
 
 
+static uavcan::Publisher<zrzk::equipment::indication::ZrRGB>* zr_rgb_led[HAL_MAX_CAN_PROTOCOL_DRIVERS];
+
 // subscribers
 
 // handler SafteyButton
@@ -390,6 +394,10 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters)
     param_get_set_client[driver_index] = new uavcan::ServiceClient<uavcan::protocol::param::GetSet, ParamGetSetCb>(*_node, ParamGetSetCb(this, &AP_UAVCAN::handle_param_get_set_response));
 
     param_execute_opcode_client[driver_index] = new uavcan::ServiceClient<uavcan::protocol::param::ExecuteOpcode, ParamExecuteOpcodeCb>(*_node, ParamExecuteOpcodeCb(this, &AP_UAVCAN::handle_param_save_response));
+    
+    zr_rgb_led[driver_index] = new uavcan::Publisher<zrzk::equipment::indication::ZrRGB>(*_node);
+    zr_rgb_led[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20));
+    zr_rgb_led[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest);
 
     safety_button_listener[driver_index] = new uavcan::Subscriber<ardupilot::indication::Button, ButtonCb>(*_node);
     if (safety_button_listener[driver_index]) {
@@ -490,6 +498,7 @@ void AP_UAVCAN::loop(void)
         send_parameter_request();
         send_parameter_save_request();
         _dna_server->verify_nodes();
+        zr_rgb_led_send();
     }
 }
 
@@ -1211,4 +1220,32 @@ bool AP_UAVCAN::prearm_check(char* fail_msg, uint8_t fail_msg_len) const
     return _dna_server->prearm_check(fail_msg, fail_msg_len);
 }
 
+bool AP_UAVCAN::set_zr_rgb_led(uint8_t mode, uint8_t red, uint8_t green, uint8_t blue)
+{
+    WITH_SEMAPHORE(_zr_rgb_led.sem);
+
+    _zr_rgb_led.mode = mode;
+    _zr_rgb_led.red = red;
+    _zr_rgb_led.green = green;
+    _zr_rgb_led.blue = blue;
+    _zr_rgb_led.need_send = true;
+    return true;
+}
+void AP_UAVCAN::zr_rgb_led_send()
+{
+    if(!_zr_rgb_led.need_send)
+    {
+        return;
+    }
+
+  zrzk::equipment::indication::ZrRGB  msg;
+  WITH_SEMAPHORE(_zr_rgb_led.sem);
+
+  msg.rgb_mode = _zr_rgb_led.mode;
+  msg.rgb_r =_zr_rgb_led. red;
+  msg.rgb_g = _zr_rgb_led.green;
+  msg.rgb_b = _zr_rgb_led.blue;
+  zr_rgb_led[_driver_index]->broadcast(msg);
+  _zr_rgb_led.need_send =false;
+}
 #endif // HAL_NUM_CAN_IFACES

libraries/AP_UAVCAN/AP_UAVCAN.h

@@ -130,6 +130,8 @@ public:
 
     // send RTCMStream packets
     void send_RTCMStream(const uint8_t *data, uint32_t len);
+    
+    bool set_zr_rgb_led(uint8_t mode, uint8_t red, uint8_t green, uint8_t blue);
 
     // Send Reboot command
     // Note: Do not call this from outside UAVCAN thread context,
@@ -330,7 +332,15 @@ private:
 
     // notify vehicle state
     uint32_t _last_notify_state_ms;
-
+    struct
+    {
+        HAL_Semaphore sem;
+        uint8_t mode;
+        uint8_t red;
+        uint8_t green;
+        uint8_t blue;
+        bool need_send;
+    } _zr_rgb_led;
     // incoming button handling
     static void handle_button(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ButtonCb &cb);
     static void handle_traffic_report(AP_UAVCAN* ap_uavcan, uint8_t node_id, const TrafficReportCb &cb);
@@ -340,6 +350,7 @@ private:
     static void handle_debug(AP_UAVCAN* ap_uavcan, uint8_t node_id, const DebugCb &cb);
     static void handle_param_get_set_response(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ParamGetSetCb &cb);
     static void handle_param_save_response(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ParamExecuteOpcodeCb &cb);
+    void  zr_rgb_led_send();
 };
 
 #endif // #if HAL_ENABLE_LIBUAVCAN_DRIVERS

libraries/AP_UAVCAN/dsdl/zrzk/equipment/indication/30100.ZrRGB.uavcan

@@ -0,0 +1,4 @@
+uint8 rgb_mode      # RGBLED mode
+uint8 rgb_r         # brightness of red light
+uint8 rgb_g         # brightness of red green
+uint8 rgb_b         # brightness of red blue