update JFB110 board definition (modified servorail adc)

jfbblue0922 · Feb 26, 2024 · 5f040b2 · 5f040b2
1 parent b7d8c27
commit 5f040b2
Show file tree

Hide file tree

Showing 29 changed files with 289 additions and 33 deletions.
diff --git a/build/JFB110/bin/antennatracker b/build/JFB110/bin/antennatracker
diff --git a/build/JFB110/bin/antennatracker.apj b/build/JFB110/bin/antennatracker.apj
diff --git a/build/JFB110/bin/antennatracker.bin b/build/JFB110/bin/antennatracker.bin
diff --git a/build/JFB110/bin/arducopter b/build/JFB110/bin/arducopter
diff --git a/build/JFB110/bin/arducopter-heli b/build/JFB110/bin/arducopter-heli
diff --git a/build/JFB110/bin/arducopter-heli.apj b/build/JFB110/bin/arducopter-heli.apj
diff --git a/build/JFB110/bin/arducopter-heli.bin b/build/JFB110/bin/arducopter-heli.bin
diff --git a/build/JFB110/bin/arducopter.apj b/build/JFB110/bin/arducopter.apj
diff --git a/build/JFB110/bin/arducopter.bin b/build/JFB110/bin/arducopter.bin
diff --git a/build/JFB110/bin/arduplane b/build/JFB110/bin/arduplane
diff --git a/build/JFB110/bin/arduplane.apj b/build/JFB110/bin/arduplane.apj
diff --git a/build/JFB110/bin/arduplane.bin b/build/JFB110/bin/arduplane.bin
diff --git a/build/JFB110/bin/ardurover b/build/JFB110/bin/ardurover
diff --git a/build/JFB110/bin/ardurover.apj b/build/JFB110/bin/ardurover.apj
diff --git a/build/JFB110/bin/ardurover.bin b/build/JFB110/bin/ardurover.bin
diff --git a/build/JFB110/bin/ardusub b/build/JFB110/bin/ardusub
diff --git a/build/JFB110/bin/ardusub.apj b/build/JFB110/bin/ardusub.apj
diff --git a/build/JFB110/bin/ardusub.bin b/build/JFB110/bin/ardusub.bin
diff --git a/build/JFB110/bin/blimp b/build/JFB110/bin/blimp
diff --git a/build/JFB110/bin/blimp.apj b/build/JFB110/bin/blimp.apj
diff --git a/build/JFB110/bin/blimp.bin b/build/JFB110/bin/blimp.bin
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/JFB110/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/JFB110/hwdef.dat
@@ -120,11 +120,8 @@ define HAL_HAVE_BOARD_VOLTAGE 1
 PB3 VBUS_RESERVED INPUT
 
 # JFB110 has SERVORAIL ADC
-# Set SENSOR_3.3V power signal insted.
 PC1  SCALED_V3V3 ADC1 SCALE(2)       # ADC1_11
-PA3  FMU_SERVORAIL_VCC ADC1 SCALE(2) # ADC1_15
-define FMU_SERVORAIL_ADC_PIN 15
-define HAL_HAVE_SERVO_VOLTAGE 1
+PA3  FMU_SERVORAIL_VCC_SENS ADC1 SCALE(2) # ADC1_15
 
 PC0  RSSI_IN ADC1 SCALE(1)       # ADC1_10
 define RSSI_ANA_PIN 10

diff --git a/libraries/AP_Math/crc.cpp b/libraries/AP_Math/crc.cpp
@@ -372,6 +372,26 @@ uint16_t crc16_ccitt(const uint8_t *buf, uint32_t len, uint16_t crc)
     return crc;
 }
 
+// CRC16_CCITT algorithm using right shift
+uint16_t crc16_ccitt_r(const uint8_t *buf, uint32_t len, uint16_t crc, uint16_t out)
+{
+	for (uint32_t i = 0; i < len; i++) {
+		crc ^= *buf++;                      // XOR byte into least sig. byte of crc
+		for (uint8_t j = 0; j < 8; j++) {   // loop over each bit
+            if ((crc & 0x0001) != 0) {      // if the LSB is set
+                crc >>= 1;                  // shift right and XOR 0x8408
+                crc ^= 0x8408;
+            } else {
+                crc >>= 1;                  // just shift right
+            }
+		}
+	}
+
+    // output xor
+    crc = crc ^ out;
+	return crc;
+}
+
 uint16_t crc16_ccitt_GDL90(const uint8_t *buf, uint32_t len, uint16_t crc)
 {
     for (uint32_t i = 0; i < len; i++) {

diff --git a/libraries/AP_Math/crc.h b/libraries/AP_Math/crc.h
@@ -38,6 +38,7 @@ uint8_t crc_sum8(const uint8_t *p, uint8_t len);
 // Copyright (C) 2010 Swift Navigation Inc.
 // Contact: Fergus Noble <[email protected]>
 uint16_t crc16_ccitt(const uint8_t *buf, uint32_t len, uint16_t crc);
+uint16_t crc16_ccitt_r(const uint8_t *buf, uint32_t len, uint16_t crc, uint16_t out);
 
 // CRC16_CCITT algorithm using the GDL90 parser method which is non-standard
 // https://www.faa.gov/nextgen/programs/adsb/archival/media/gdl90_public_icd_reva.pdf

diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.cpp b/libraries/AP_RangeFinder/AP_RangeFinder.cpp
@@ -49,6 +49,7 @@
 #include "AP_RangeFinder_MSP.h"
 #include "AP_RangeFinder_USD1_CAN.h"
 #include "AP_RangeFinder_Benewake_CAN.h"
+#include "AP_RangeFinder_JRE_Serial.h"
 
 #include <AP_BoardConfig/AP_BoardConfig.h>
 #include <AP_Logger/AP_Logger.h>
@@ -584,6 +585,11 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)
         _add_backend(new AP_RangeFinder_Benewake_CAN(state[instance], params[instance]), instance);
         break;
 #endif
+    case Type::JRE_Serial:
+        if (AP_RangeFinder_JRE_Serial::detect(serial_instance)) {
+            _add_backend(new AP_RangeFinder_JRE_Serial(state[instance], params[instance]), instance, serial_instance++);
+        }
+        break;
     case Type::NONE:
     default:
         break;

diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.h b/libraries/AP_RangeFinder/AP_RangeFinder.h
@@ -89,6 +89,8 @@ class RangeFinder
         MSP = 32,
         USD1_CAN = 33,
         Benewake_CAN = 34,
+
+        JRE_Serial = 41,
         SIM = 100,
     };
 

diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_JRE_Serial.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_JRE_Serial.cpp
@@ -0,0 +1,138 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "AP_RangeFinder_JRE_Serial.h"
+#include <AP_Math/AP_Math.h>
+
+#define FRAME_HEADER_1    'R'    // 0x52
+#define FRAME_HEADER_2_A  'A'    // 0x41
+#define FRAME_HEADER_2_B  'B'    // 0x42
+#define FRAME_HEADER_2_C  'C'    // 0x43
+
+#define DIST_MAX_CM 5000
+#define OUT_OF_RANGE_ADD_CM   100
+
+void AP_RangeFinder_JRE_Serial::move_preamble_in_buffer(uint8_t search_start_pos)
+{
+    uint8_t i;
+    for (i=search_start_pos; i<data_buff_ofs; i++) {
+        if (data_buff[i] == FRAME_HEADER_1) {
+            break;
+        }
+    }
+    if (i == 0) {
+        return;
+    }
+    memmove(data_buff, &data_buff[i], data_buff_ofs-i);
+    data_buff_ofs = data_buff_ofs - i;
+}
+
+bool AP_RangeFinder_JRE_Serial::get_reading(float &reading_m)
+{
+    // uart instance check
+    if (uart == nullptr) {
+        return false;  // not update
+    }
+
+    uint16_t valid_count = 0;   // number of valid readings
+    uint16_t invalid_count = 0; // number of invalid readings
+    float sum = 0;
+
+    // read a maximum of 8192 bytes per call to this function:
+    uint16_t bytes_available = MIN(uart->available(), 8192U);
+
+    while (bytes_available > 0) {
+        // fill buffer
+        const auto num_bytes_to_read = MIN(bytes_available, ARRAY_SIZE(data_buff) - data_buff_ofs);
+        const auto num_bytes_read = uart->read(&data_buff[data_buff_ofs], num_bytes_to_read);
+        if (num_bytes_read == 0) {
+            break;
+        }
+        if (bytes_available < num_bytes_read) {
+            // this is a bug in the uart call.
+            break;
+        }
+        bytes_available -= num_bytes_read;
+        data_buff_ofs += num_bytes_read;
+
+        // move header frame header in buffer
+        move_preamble_in_buffer(0);
+
+        // ensure we have a packet type:
+        if (data_buff_ofs < 2) {
+            continue;
+        }
+
+        // determine packet length for incoming packet:
+        uint8_t packet_length;
+        switch (data_buff[1]) {
+        case FRAME_HEADER_2_A:                   
+            packet_length = 16;
+            break;
+        case FRAME_HEADER_2_B:
+            packet_length = 32;
+            break;
+        case FRAME_HEADER_2_C:
+            packet_length = 48;
+            break;
+        default:
+            move_preamble_in_buffer(1);
+            continue;
+        }
+
+        // check there are enough bytes for message type
+        if (data_buff_ofs < packet_length) {
+            continue;
+        }
+
+        // check the checksum
+        const uint16_t crc = crc16_ccitt_r(data_buff, packet_length - 2, 0xffff, 0xffff);
+        if (crc != ((data_buff[packet_length-1] << 8) | data_buff[packet_length-2])) {
+            // CRC failure
+            move_preamble_in_buffer(1);
+            continue;
+        }
+
+        // check random bit to for magic value:
+        if (data_buff[packet_length-3] & 0x02) { // NTRK
+            invalid_count++;
+            // discard entire packet:
+            move_preamble_in_buffer(packet_length);
+            continue;
+        }
+
+        // good message, extract rangefinder reading:
+        reading_m = (data_buff[4] * 256 + data_buff[5]) * 0.01f;
+        sum += reading_m;
+        valid_count++;
+        move_preamble_in_buffer(packet_length);
+    }
+
+    // return average of all valid readings
+    if (valid_count > 0) {
+        no_signal = false;
+        reading_m = sum / valid_count;
+        return true;
+    }
+
+    // all readings were invalid so return out-of-range-high value
+    if (invalid_count > 0) {
+        no_signal = true;
+        reading_m = MIN(MAX(DIST_MAX_CM, max_distance_cm() + OUT_OF_RANGE_ADD_CM), UINT16_MAX) * 0.01f;
+        return true;
+    }
+
+    return false;
+}
diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_JRE_Serial.h b/libraries/AP_RangeFinder/AP_RangeFinder_JRE_Serial.h
@@ -0,0 +1,92 @@
+#pragma once
+
+#include "AP_RangeFinder.h"
+#include "AP_RangeFinder_Backend_Serial.h"
+
+/*
+The data received from the radio altimeter is as follows.
+The format of the received data frame varies depending on the mode, and is stored in "read_buff" with a fixed value of 16 bytes, 32 bytes, or 48 bytes.
+
+[1]
+Measurement mode: 1 data mode
+Packet length: 16 bytes
+Altitude data used: 4,5 bytes
+|----------------------------------------------------------------------------------------------------------------------------------------------------|
+| BYTE |   0    |   1    |    2     |    3     |    4     |    5     | 6 | 7 | 8 | 9 |     10    |     11    |    12     |    13     |  14   |  15   |
+|------|---------------------------------------------------------------------------------------------------------------------------------------------|
+| NAME | header | header | version  |  frame   | altitude | altitude |    reserved   | FFT value | FFT value |  status   |  status   |  CRC  |  CRC  |
+|      |  (H)   |  (L)   |          |  count   |    (H)   |    (L)   |               |    (H)    |    (L)    |   (H)     |   (L)     |  (L)  |  (H)  |
+|------|---------------------------------------------------------------------------------------------------------------------------------------------|
+|      |        |        |          |          |                     |               |                       | BIT 15to8: reserved   |               |
+|      |        |        |          | unsigned |       unsigned      |               |       unsigned        | BIT 7to4: GAIN detail |   CRC result  |
+| DATA |  'R'   |  'A'   | 0 to 255 | 0 to 255 |      LSB;0.01 m     |      N/A      |        LSB:1          | BIT 3-2: reserved     |   from BYTE   |
+|      |        |        |          |          |      0 to 65535     |               |      0 to 65535       | BIT 1: TRK / NTRK     |    0 to 13    |
+|      |        |        |          |          |                     |               |                       | BIT 0: GAIN LOW/HIGH  |               |
+|----------------------------------------------------------------------------------------------------------------------------------------------------|
+
+[2]
+Measurement mode: 3 data mode
+Packet length: 32 bytes
+Altitude data used: 4,5 bytes
+|--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|
+| BYTE |   0    |   1    |    2     |    3     |    4     |    5     | 6 | 7 | 8 | 9 |     10    |     11    |    12    |    13    | 14| 15| 16| 17|     18    |     19    |    20    |    21    | 22| 23| 24| 25|     26    |     27    |    28     |    29     |  30   |  31   |
+|------|-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|
+| NAME | header | header | version  |  frame   | altitude | altitude |    reserved   | FFT value | FFT value | altitude | altitude |    reserved   | FFT value | FFT value | altitude | altitude |    reserved   | FFT value | FFT value |  status   |  status   |  CRC  |  CRC  |
+|      |  (H)   |  (L)   |          |  count   |    (H)   |    (L)   |               |    (H)    |    (L)    |    (H)   |    (L)   |               |    (H)    |    (L)    |    (H)   |    (L)   |               |    (H)    |    (L)    |    (H)    |    (L)    |  (L)  |  (H)  |
+|------|-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|
+|      |        |        |          |          |                     |               |                       |                     |               |                       |                     |               |                       | BIT 15to8: reserved   |               |
+|      |        |        |          | unsigned |       unsigned      |               |       unsigned        |       unsigned      |               |       unsigned        |       unsigned      |               |       unsigned        | BIT 7to4: GAIN detail |   CRC result  |
+| DATA |  'R'   |  'B'   | 0 to 255 | 0 to 255 |      LSB;0.01 m     |      N/A      |        LSB:1          |      LSB;0.01 m     |      N/A      |        LSB:1          |      LSB;0.01 m     |      N/A      |        LSB:1          | BIT 3-2: reserved     |   from BYTE   |
+|      |        |        |          |          |      0 to 65535     |               |      0 to 65535       |      0 to 65535     |               |      0 to 65535       |      0 to 65535     |               |      0 to 65535       | BIT 1: TRK / NTRK     |    0 to 13    |
+|      |        |        |          |          |                     |               |                       |                     |               |                       |                     |               |                       | BIT 0: GAIN LOW/HIGH  |               |
+|--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|
+
+[3]
+Measurement mode: 5 data mode
+Packet length: 48 bytes
+Altitude data used: 4,5 bytes
+|------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|
+| BYTE |   0    |   1    |    2     |    3     |    4     |    5     | 6 | 7 | 8 | 9 |     10    |     11    |    12    |    13    | 14| 15| 16| 17|     18    |     19    |    20    |    21    | 22| 23| 24| 25|     26    |     27    |    28    |    29    | 30| 31| 32| 33|     34    |     35    |    36    |    37    | 38| 39| 40| 41|     42    |     43    |    44     |    45     |  46   |  47   |
+|------|-----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|
+| NAME | header | header | version  |  frame   | altitude | altitude |    reserved   | FFT value | FFT value | altitude | altitude |    reserved   | FFT value | FFT value | altitude | altitude |    reserved   | FFT value | FFT value | altitude | altitude |    reserved   | FFT value | FFT value | altitude | altitude |    reserved   | FFT value | FFT value |  status   |  status   |  CRC  |  CRC  |
+|      |  (H)   |  (L)   |          |  count   |    (H)   |    (L)   |               |    (H)    |    (L)    |    (H)   |    (L)   |               |    (H)    |    (L)    |    (H)   |    (L)   |               |    (H)    |    (L)    |    (H)   |    (L)   |               |    (H)    |    (L)    |    (H)   |    (L)   |               |    (H)    |    (L)    |    (H)    |    (L)    |  (L)  |  (H)  |
+|------|-----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|
+|      |        |        |          |          |                     |               |                       |                     |               |                       |                     |               |                       |                     |               |                       |                     |               |                       | BIT 15to8: reserved   |               |
+|      |        |        |          | unsigned |       unsigned      |               |       unsigned        |       unsigned      |               |       unsigned        |       unsigned      |               |       unsigned        |       unsigned      |               |       unsigned        |       unsigned      |               |       unsigned        | BIT 7to4: GAIN detail |   CRC result  |
+| DATA |  'R'   |  'C'   | 0 to 255 | 0 to 255 |      LSB;0.01 m     |      N/A      |        LSB:1          |      LSB;0.01 m     |      N/A      |        LSB:1          |      LSB;0.01 m     |      N/A      |        LSB:1          |      LSB;0.01 m     |      N/A      |        LSB:1          |      LSB;0.01 m     |      N/A      |        LSB:1          | BIT 3-2: reserved     |   from BYTE   |
+|      |        |        |          |          |      0 to 65535     |               |      0 to 65535       |      0 to 65535     |               |      0 to 65535       |      0 to 65535     |               |      0 to 65535       |      0 to 65535     |               |      0 to 65535       |      0 to 65535     |               |      0 to 65535       | BIT 1: TRK / NTRK     |    0 to 13    |
+|      |        |        |          |          |                     |               |                       |                     |               |                       |                     |               |                       |                     |               |                       |                     |               |                       | BIT 0: GAIN LOW/HIGH  |               |
+|------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|
+*/
+class AP_RangeFinder_JRE_Serial : public AP_RangeFinder_Backend_Serial
+{
+
+public:
+    static AP_RangeFinder_Backend_Serial *create(
+        RangeFinder::RangeFinder_State &_state,
+        AP_RangeFinder_Params &_params)
+    {
+        return new AP_RangeFinder_JRE_Serial(_state, _params);
+    }
+
+protected:
+
+    using AP_RangeFinder_Backend_Serial::AP_RangeFinder_Backend_Serial;
+
+    MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override
+    {
+        return MAV_DISTANCE_SENSOR_RADAR;
+    }
+
+private:
+
+    // get a reading
+    bool get_reading(float &reading_m) override;
+
+    void move_preamble_in_buffer(uint8_t search_start_pos);
+
+    uint8_t data_buff[48 * 3];  // 48 is longest possible packet
+    uint8_t data_buff_ofs;      // index where next item will be added in data_buff
+
+    bool no_signal;     // true if the latest read attempt found no valid distances
+};
diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp
@@ -6,7 +6,7 @@ const AP_Param::GroupInfo AP_RangeFinder_Params::var_info[] = {
     // @Param: TYPE
     // @DisplayName: Rangefinder type
     // @Description: Type of connected rangefinder
-    // @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLite-I2C,5:PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:USD1_Serial,12:LeddarOne,13:MaxbotixSerial,14:TeraRangerI2C,15:LidarLiteV3-I2C,16:VL53L0X or VL53L1X,17:NMEA,18:WASP-LRF,19:BenewakeTF02,20:Benewake-Serial,21:LidarLightV3HP,22:PWM,23:BlueRoboticsPing,24:DroneCAN,25:BenewakeTFminiPlus-I2C,26:LanbaoPSK-CM8JL65-CC5,27:BenewakeTF03,28:VL53L1X-ShortRange,29:LeddarVu8-Serial,30:HC-SR04,31:GYUS42v2,32:MSP,33:USD1_CAN,34:Benewake_CAN,100:SITL
+    // @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLite-I2C,5:PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:USD1_Serial,12:LeddarOne,13:MaxbotixSerial,14:TeraRangerI2C,15:LidarLiteV3-I2C,16:VL53L0X or VL53L1X,17:NMEA,18:WASP-LRF,19:BenewakeTF02,20:Benewake-Serial,21:LidarLightV3HP,22:PWM,23:BlueRoboticsPing,24:DroneCAN,25:BenewakeTFminiPlus-I2C,26:LanbaoPSK-CM8JL65-CC5,27:BenewakeTF03,28:VL53L1X-ShortRange,29:LeddarVu8-Serial,30:HC-SR04,31:GYUS42v2,32:MSP,33:USD1_CAN,34:Benewake_CAN,41:JRE_Serial,100:SITL
     // @User: Standard
     AP_GROUPINFO_FLAGS("TYPE", 1, AP_RangeFinder_Params, type, 0, AP_PARAM_FLAG_ENABLE),