mirror of https://github.com/ArduPilot/ardupilot
AP_RangeFinder: add serial driver for JRE
This commit is contained in:
jfbblue0922
Randy Mackay
parent
265f19b396
commit
fef47303d2
|
|
@ -58,6 +58,7 @@
|
|||
#include "AP_RangeFinder_TOFSenseP_CAN.h"
|
||||
#include "AP_RangeFinder_NRA24_CAN.h"
|
||||
#include "AP_RangeFinder_TOFSenseF_I2C.h"
|
||||
#include "AP_RangeFinder_JRE_Serial.h"
|
||||
|
||||
#include <AP_BoardConfig/AP_BoardConfig.h>
|
||||
#include <AP_Logger/AP_Logger.h>
|
||||
|
|
@ -575,6 +576,12 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)
|
|||
break;
|
||||
}
|
||||
#endif
|
||||
#if AP_RANGEFINDER_JRE_SERIAL_ENABLED
|
||||
case Type::JRE_Serial:
|
||||
serial_create_fn = AP_RangeFinder_JRE_Serial::create;
|
||||
break;
|
||||
#endif
|
||||
|
||||
case Type::NONE:
|
||||
break;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -171,6 +171,9 @@ public:
|
|||
#if AP_RANGEFINDER_TOFSENSEF_I2C_ENABLED
|
||||
TOFSenseF_I2C = 40,
|
||||
#endif
|
||||
#if AP_RANGEFINDER_JRE_SERIAL_ENABLED
|
||||
JRE_Serial = 41,
|
||||
#endif
|
||||
#if AP_RANGEFINDER_SIM_ENABLED
|
||||
SIM = 100,
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -0,0 +1,107 @@
|
|||
/*
|
||||
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_config.h"
|
||||
|
||||
#if AP_RANGEFINDER_JRE_SERIAL_ENABLED
|
||||
|
||||
#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
|
||||
|
||||
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;
|
||||
// max distance the sensor can reliably measure - read from parameters
|
||||
const int16_t distance_cm_max = max_distance_cm();
|
||||
|
||||
// buffer read
|
||||
const ssize_t num_read = uart->read(read_buff, ARRAY_SIZE(read_buff));
|
||||
for (read_buff_idx = 0; read_buff_idx < num_read; read_buff_idx++) {
|
||||
if (data_buff_idx == 0) { // header first byte check
|
||||
// header data search
|
||||
if (read_buff[read_buff_idx] == FRAME_HEADER_1) {
|
||||
data_buff[0] = FRAME_HEADER_1;
|
||||
data_buff_idx = 1; // next data_buff
|
||||
}
|
||||
} else if (data_buff_idx == 1) { // header second byte check
|
||||
data_buff[1] = read_buff[read_buff_idx];
|
||||
data_buff_idx = 2; // next data_buff
|
||||
switch (read_buff[read_buff_idx]) {
|
||||
case FRAME_HEADER_2_A:
|
||||
data_buff_len = 16;
|
||||
break;
|
||||
case FRAME_HEADER_2_B:
|
||||
data_buff_len = 32;
|
||||
break;
|
||||
case FRAME_HEADER_2_C:
|
||||
data_buff_len = 48;
|
||||
break;
|
||||
default:
|
||||
data_buff_idx = 0; // data index clear
|
||||
break;
|
||||
}
|
||||
} else { // data set
|
||||
data_buff[data_buff_idx++] = read_buff[read_buff_idx];
|
||||
|
||||
if (data_buff_idx >= data_buff_len) { // 1 data set complete
|
||||
// crc check
|
||||
uint16_t crc = crc16_ccitt_r(data_buff, data_buff_len - 2, 0xffff, 0xffff);
|
||||
if ((HIGHBYTE(crc) == data_buff[data_buff_len-1]) && (LOWBYTE(crc) == data_buff[data_buff_len-2])) {
|
||||
// status check
|
||||
if (data_buff[data_buff_len-3] & 0x02) { // NTRK
|
||||
invalid_count++;
|
||||
} else { // TRK
|
||||
reading_m = (data_buff[4] * 256 + data_buff[5]) * 0.01f;
|
||||
sum += reading_m;
|
||||
valid_count++;
|
||||
}
|
||||
}
|
||||
data_buff_idx = 0; // data index clear
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 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, distance_cm_max + OUT_OF_RANGE_ADD_CM), UINT16_MAX) * 0.01f;
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
#endif // AP_RANGEFINDER_JRE_SERIAL_ENABLED
|
||||
|
|
@ -0,0 +1,105 @@
|
|||
#pragma once
|
||||
|
||||
#include "AP_RangeFinder_config.h"
|
||||
|
||||
#if AP_RANGEFINDER_JRE_SERIAL_ENABLED
|
||||
|
||||
#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 | |
|
||||
|------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|
|
||||
*/
|
||||
#define DATA_LENGTH 16
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
bool get_signal_quality_pct(int8_t &quality_pct) const override {
|
||||
quality_pct = no_signal ? 0 : 100;
|
||||
return true;
|
||||
}
|
||||
|
||||
private:
|
||||
|
||||
// get a reading
|
||||
bool get_reading(float &reading_m) override;
|
||||
|
||||
uint8_t read_buff[DATA_LENGTH * 10];
|
||||
uint8_t read_buff_idx;
|
||||
uint8_t data_buff[DATA_LENGTH * 3]; // maximum frame length
|
||||
uint8_t data_buff_idx;
|
||||
uint8_t data_buff_len;
|
||||
|
||||
bool no_signal; // true if the latest read attempt found no valid distances
|
||||
};
|
||||
#endif // AP_RANGEFINDER_JRE_SERIAL_ENABLED
|
||||
|
|
@ -10,7 +10,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,35:TeraRangerSerial,36:Lua_Scripting,37:NoopLoop_TOFSense,38:NoopLoop_TOFSense_CAN,39:NRA24_CAN,40:NoopLoop_TOFSenseF_I2C,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,35:TeraRangerSerial,36:Lua_Scripting,37:NoopLoop_TOFSense,38:NoopLoop_TOFSense_CAN,39:NRA24_CAN,40:NoopLoop_TOFSenseF_I2C,41:JRE_Serial,100:SITL
|
||||
// @User: Standard
|
||||
AP_GROUPINFO_FLAGS("TYPE", 1, AP_RangeFinder_Params, type, 0, AP_PARAM_FLAG_ENABLE),
|
||||
|
||||
|
|
|
|||
|
|
@ -173,3 +173,6 @@
|
|||
#define AP_RANGEFINDER_WASP_ENABLED AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED
|
||||
#endif
|
||||
|
||||
#ifndef AP_RANGEFINDER_JRE_SERIAL_ENABLED
|
||||
#define AP_RANGEFINDER_JRE_SERIAL_ENABLED AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED && BOARD_FLASH_SIZE > 1024
|
||||
#endif
|
||||
|
|
|
|||
Loading…
Reference in New Issue