backup commit 202001

libraries/AP_HAL_ChibiOS/RCOutput.cpp

@@ -1633,8 +1633,20 @@ bool RCOutput::set_neopixel_num_LEDs(const uint16_t chan, uint8_t num_leds)
   setup neopixel (WS2812B) output data for a given output channel
   and mask of LEDs on the channel
 */
+void ArrayShift2(uint8_t a[], int n, int m)
+{
+	while (m--)
+	{
+	    int temp = a[n - 1];
+		for (int i = n - 1; i >= 1; i--)
+			a[i] = a[i - 1];
+	  	a[0] = temp;
+	}
+}
+
 void RCOutput::set_neopixel_rgb_data(const uint16_t chan, uint32_t ledmask, uint8_t red, uint8_t green, uint8_t blue)
 {
+#if 0
     uint8_t i;
     pwm_group *grp = find_chan(chan, i);
     if (!grp) {
@@ -1662,6 +1674,50 @@ void RCOutput::set_neopixel_rgb_data(const uint16_t chan, uint32_t ledmask, uint
         ledmask >>= 1;
         led++;
     }
+
+#else
+    uint8_t i;
+    pwm_group *grp = find_chan(chan, i);
+    if (!grp) {
+        return;
+    }
+
+    // mask out for enabled LEDs
+    static uint8_t data_flow[24]={5,15,50,100,  100,50,15,5,  0,0,0,0,  0,0,0,0};// 0,0,0,0,  5,20,50,100,  100,50,20,5,  0,0,0,0};
+    ledmask &= (1U<<grp->neopixel_nleds)-1;
+    uint8_t led = 0;
+    int k = 0;
+    // static uint8_t delay_cnt;
+    // delay_cnt++;
+    // if(delay_cnt>10){
+    //     ArrayShift2(data_flow,16,1); 
+    //     delay_cnt = 0;
+    // }    
+        ArrayShift2(data_flow,24,1); 
+
+    while (ledmask) {
+        if (ledmask & 1) {
+            red = 0;
+            green = data_flow[k];
+            blue = 0;
+            k++;
+            const uint8_t pad_start_bits = 1;
+            const uint8_t neopixel_bit_length = 24;
+            const uint8_t stride = 4;
+            uint32_t *buf = grp->dma_buffer + (led * neopixel_bit_length + pad_start_bits) * stride + i;
+            uint32_t bits = (green<<16) | (red<<8) | blue;
+            const uint32_t BIT_0 = 7 * grp->bit_width_mul;
+            const uint32_t BIT_1 = 14 * grp->bit_width_mul;
+            for (uint16_t b=0; b < 24; b++) {
+                buf[b * stride] = (bits & 0x800000) ? BIT_1 : BIT_0;
+                bits <<= 1;
+            }
+        }
+        ledmask >>= 1;
+        led++;
+    }
+    
+#endif
 }
 
 /*

libraries/AP_Notify/NeoPixel.cpp

@@ -21,7 +21,7 @@
 #define AP_NOTIFY_NEOPIXEL_MAX_INSTANCES        4
 
 #ifndef HAL_NEOPIXEL_COUNT
-#define HAL_NEOPIXEL_COUNT 1
+#define HAL_NEOPIXEL_COUNT 24
 #endif
 
 // Datasheet: https://cdn-shop.adafruit.com/datasheets/WS2812B.pdf

libraries/AP_Notify/RGBLed.cpp

@@ -40,9 +40,10 @@ bool RGBLed::init()
 // 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 ||
+    // if (red != _red_curr ||
-        green != _green_curr ||
+    //     green != _green_curr ||
-        blue != _blue_curr) {
+    //     blue != _blue_curr) {
+    if(1){
         // call the hardware update routine
         if (hw_set_rgb(red, green, blue)) {
             _red_curr = red;

libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.cpp

@@ -58,18 +58,26 @@ bool AP_RangeFinder_MaxsonarSerialLV::get_reading(uint16_t &reading_cm)
         return false;
     }
 
-    int32_t sum = 0;
+#if 1
+    float sum = 0;
     int16_t nbytes = uart->available();
     uint16_t count = 0;
 
     while (nbytes-- > 0) {
         char c = uart->read();
+            // gcs().send_text(MAV_SEVERITY_INFO, "getc #%c",c);
+            // gcs().send_text(MAV_SEVERITY_INFO, "getd #%d",(int)atoi(c);
+            // gcs().send_text(MAV_SEVERITY_INFO, "getf #%f",(float)atof(c));
+
         if (c == '\r') {
             linebuf[linebuf_len] = 0;
-            sum += (int)atoi(linebuf);
+            // sum += (int)atoi(linebuf);
+            sum += (float)atof(linebuf);
             count++;
             linebuf_len = 0;
-        } else if (isdigit(c)) {
+            // gcs().send_text(MAV_SEVERITY_INFO, "dist #%3.2f",sum);
+        // } else if (isdigit() {
+        } else {
             linebuf[linebuf_len++] = c;
             if (linebuf_len == sizeof(linebuf)) {
                 // too long, discard the line
@@ -83,9 +91,47 @@ bool AP_RangeFinder_MaxsonarSerialLV::get_reading(uint16_t &reading_cm)
     }
 
     // This sonar gives the metrics in inches, so we have to transform this to centimeters
-    reading_cm = 2.54f * sum / count;
+    // reading_cm = 2.54f * sum / count;
+    reading_cm = 100.0f * sum / count;
+
+    return true;
+#else 
+
+    uint8_t frameOK = 0;   
+    uint16_t Range  = 0;                                                //雷达实测距离
+    int16_t  nbytes = uart->available(); 
+
+    while ( nbytes-- > 0 ) {
+        if( uart->read() == StartSequence ){                            //判断数据包帧头0xAA    
+            string[0] = StartSequence;
+            if( uart->read() == StartSequence ){                         //判断数据包帧头0xAA
+                string[1] = StartSequence;
+                for( int i = 2;i < 14;i++ ){                            //存储数据到数组      
+                    string[i] = uart->read();
+                }
+                CheckSum = string[4]+string[5]+string[6]+string[7]+string[8]+string[9]+string[10];       
+                if( string[11] == (CheckSum&0xFF)&&string[12] == EndSequence && string[13] == EndSequence ){         //按照协议对收到的数据进行校验  
+                    frameOK = 1; 
+                    Range = (string[6]*256+string[7]);                    //cm
+                    // testdata = (string[8]*256+string[9]);    
+                }
+            }
+        }
+    }
 
+    if (frameOK == 0) 
+    {
+       return false;
+    }
+    
+    if(Range != 0)
+    {
+        reading_cm = Range;
+    }
     return true;
+#endif
+
+
 }
 
 /* 

libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.h

@@ -31,4 +31,8 @@ private:
     AP_HAL::UARTDriver *uart = nullptr;
     char linebuf[10];
     uint8_t linebuf_len = 0;
+
+    const int StartSequence=0xAA;  //数据包帧头
+    const int EndSequence=0X55; 
+    uint8_t string[14],CheckSum;//校验数值存放  
 };

libraries/AP_RangeFinder/RangeFinder.cpp

@@ -18,6 +18,7 @@
 #include "AP_RangeFinder_PulsedLightLRF.h"
 #include "AP_RangeFinder_MaxsonarI2CXL.h"
 #include "AP_RangeFinder_MaxsonarSerialLV.h"
+#include "AP_RangeFinder_insighticaSerial.h"
 #include "AP_RangeFinder_BBB_PRU.h"
 #include "AP_RangeFinder_LightWareI2C.h"
 #include "AP_RangeFinder_LightWareSerial.h"
@@ -492,6 +493,11 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)
             drivers[instance] = new AP_RangeFinder_Lanbao(state[instance], params[instance], serial_instance++);
         }
         break;
+    case RangeFinder_InsighticaSerial:
+        if (AP_RangeFinder_insighticaSerial::detect(serial_instance)) {
+            drivers[instance] = new AP_RangeFinder_insighticaSerial(state[instance], params[instance], serial_instance++);
+        }
+        break;
     default:
         break;
     }

libraries/AP_RangeFinder/RangeFinder.h

@@ -77,6 +77,7 @@ public:
         RangeFinder_TYPE_BenewakeTFminiPlus = 25,
         RangeFinder_TYPE_Lanbao = 26,
         RangeFinder_TYPE_BenewakeTF03 = 27,
+        RangeFinder_InsighticaSerial = 28,
     };
 
     enum RangeFinder_Function {