diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp index d808fd91f7..31c354aff5 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput.cpp +++ b/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<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 } /* diff --git a/libraries/AP_Notify/NeoPixel.cpp b/libraries/AP_Notify/NeoPixel.cpp index 024f2b77b3..2e24818c26 100644 --- a/libraries/AP_Notify/NeoPixel.cpp +++ b/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 diff --git a/libraries/AP_Notify/RGBLed.cpp b/libraries/AP_Notify/RGBLed.cpp index a20c17d6fa..5707c09951 100644 --- a/libraries/AP_Notify/RGBLed.cpp +++ b/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 || - green != _green_curr || - blue != _blue_curr) { + // if (red != _red_curr || + // green != _green_curr || + // blue != _blue_curr) { + if(1){ // call the hardware update routine if (hw_set_rgb(red, green, blue)) { _red_curr = red; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.cpp index 2a13702d4b..4d91e4293d 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.cpp +++ b/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 + + } /* diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.h b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.h index 287a360f3e..f0591ca07a 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.h +++ b/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;//校验数值存放 }; diff --git a/libraries/AP_RangeFinder/RangeFinder.cpp b/libraries/AP_RangeFinder/RangeFinder.cpp index 71aa7e7bf0..2903f82076 100644 --- a/libraries/AP_RangeFinder/RangeFinder.cpp +++ b/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; } diff --git a/libraries/AP_RangeFinder/RangeFinder.h b/libraries/AP_RangeFinder/RangeFinder.h index e117e27bb2..3a3cf034cf 100644 --- a/libraries/AP_RangeFinder/RangeFinder.h +++ b/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 {