forked from ArduPilot/ardupilot
-
Notifications
You must be signed in to change notification settings - Fork 1
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
- Loading branch information
1 parent
4b0958f
commit 6e81258
Showing
8 changed files
with
279 additions
and
1 deletion.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
|
|
@@ -41,6 +41,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 | ||
|
|
||
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,112 @@ | ||
| /* | ||
| 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 50000 | ||
| #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 | ||
| for (; read_buff_idx < num_read; read_buff_idx++) { | ||
| if (read_buff[read_buff_idx] == FRAME_HEADER_1) { | ||
| data_buff[0] = FRAME_HEADER_1; | ||
| data_buff_idx = 1; // next data_buff | ||
| break; | ||
| } | ||
| } | ||
|
|
||
| } 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 | ||
| 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 | ||
| read_buff_idx--; // exclude current index | ||
| } else { | ||
| data_buff[data_buff_idx++] = read_buff[read_buff_idx]; | ||
| } | ||
| } | ||
| } | ||
|
|
||
| if (valid_count > 0) { | ||
| no_signal = false; | ||
| reading_m = sum / valid_count; | ||
| return true; | ||
| } | ||
|
|
||
| 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 |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,106 @@ | ||
| #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_num; | ||
| 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; | ||
| }; | ||
| #endif // AP_RANGEFINDER_JRE_SERIAL_ENABLED |
Oops, something went wrong.