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.
AP_RangeFinder: add Ainstein radar driver
- Loading branch information
Showing
6 changed files
with
196 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
119 changes: 119 additions & 0 deletions
119
libraries/AP_RangeFinder/AP_RangeFinder_Ainstein_LR_D1.cpp
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,119 @@ | ||
| /* | ||
| 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_AINSTEIN_LR_D1_ENABLED | ||
|
|
||
| #include "AP_RangeFinder_Ainstein_LR_D1.h" | ||
| #include <GCS_MAVLink/GCS.h> | ||
|
|
||
| // get_reading - read a value from the sensor | ||
| bool AP_RangeFinder_Ainstein_LR_D1::get_reading(float &reading_m) | ||
| { | ||
| if (uart == nullptr || uart->available() == 0) { | ||
| return false; | ||
| } | ||
|
|
||
| bool has_data = false; | ||
|
|
||
| uint32_t available = MAX(uart->available(), static_cast<unsigned int>(PACKET_SIZE*4)); | ||
| while (available >= PACKET_SIZE) { | ||
| // --------------- | ||
| // Sync up with the header | ||
| const uint8_t header[] = { | ||
| 0xEB, // Header MSB | ||
| 0x90, // Header LSB | ||
| 0x00 // Device ID | ||
| }; | ||
| for (uint8_t i = 0; i<ARRAY_SIZE(header); i++) { | ||
| available--; | ||
| if (uart->read() != header[i]) { | ||
| continue; | ||
| } | ||
| } | ||
|
|
||
| const uint8_t rest_of_packet_size = (PACKET_SIZE - ARRAY_SIZE(header)); | ||
| if (available < rest_of_packet_size) { | ||
| return false; | ||
| } | ||
|
|
||
| // --------------- | ||
| // header is aligned! | ||
| // --------------- | ||
|
|
||
| uint8_t buffer[rest_of_packet_size]; | ||
| available -= uart->read(buffer, ARRAY_SIZE(buffer)); | ||
|
|
||
| const uint8_t checksum = buffer[ARRAY_SIZE(buffer)-1]; // last byte is a checksum | ||
| if (crc_sum_of_bytes(buffer, ARRAY_SIZE(buffer)-1) != checksum) { | ||
| // bad Checksum | ||
| continue; | ||
| } | ||
|
|
||
| const uint8_t malfunction_alert = buffer[1]; | ||
| reading_m = UINT16_VALUE(buffer[3], buffer[4]) * 0.01; | ||
| const uint8_t snr = buffer[5]; | ||
|
|
||
| has_data = true; | ||
|
|
||
| if (malfunction_alert_prev != malfunction_alert) { | ||
| malfunction_alert_prev = malfunction_alert; | ||
| report_malfunction(malfunction_alert); | ||
| } | ||
|
|
||
| /* From datasheet: | ||
| Altitude measurements associated with a SNR value | ||
| of 13dB or lower are considered erroneous. | ||
| SNR values of 0 are considered out of maximum range (655 metres) | ||
| The altitude measurements should not in any circumstances be used as true | ||
| measurements independently of the corresponding SNR values. | ||
| */ | ||
| signal_quality_pct = (snr <= 13 || malfunction_alert != 0) ? RangeFinder::SIGNAL_QUALITY_MIN : RangeFinder::SIGNAL_QUALITY_MAX; | ||
|
|
||
| if (snr <= 13) { | ||
| has_data = false; | ||
| if (snr == 0) { | ||
| state.status = RangeFinder::Status::OutOfRangeHigh; | ||
| reading_m = MAX(656, max_distance_cm() * 0.01 + 1); | ||
| } else { | ||
| state.status = RangeFinder::Status::NoData; | ||
| } | ||
| } else { | ||
| state.status = RangeFinder::Status::Good; | ||
| } | ||
| } | ||
|
|
||
| return has_data; | ||
| } | ||
|
|
||
| void AP_RangeFinder_Ainstein_LR_D1::report_malfunction(const uint8_t _malfunction_alert_) { | ||
| if (_malfunction_alert_ & static_cast<uint8_t>(MalfunctionAlert::Temperature)) { | ||
| GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "RangeFinder: Temperature alert"); | ||
| } | ||
| if (_malfunction_alert_ & static_cast<uint8_t>(MalfunctionAlert::Voltage)) { | ||
| GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "RangeFinder: Voltage alert"); | ||
| } | ||
| if (_malfunction_alert_ & static_cast<uint8_t>(MalfunctionAlert::IFSignalSaturation)) { | ||
| GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "RangeFinder: IF signal saturation alert"); | ||
| } | ||
| if (_malfunction_alert_ & static_cast<uint8_t>(MalfunctionAlert::AltitudeReading)) { | ||
| GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "RangeFinder: Altitude reading overflow alert"); | ||
| } | ||
| } | ||
|
|
||
| #endif |
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,62 @@ | ||
| #pragma once | ||
|
|
||
| #include "AP_RangeFinder_config.h" | ||
|
|
||
| /* | ||
| questions: | ||
| - bug on page 22 on malfunction codes | ||
| - fixed length seems strange at 28 bytes | ||
| - definition of snr field is missing in documentation | ||
| - roll/pitch limits are in conflict, 3.2 vs | ||
| */ | ||
| #if AP_RANGEFINDER_AINSTEIN_LR_D1_ENABLED | ||
|
|
||
| #include "AP_RangeFinder.h" | ||
| #include "AP_RangeFinder_Backend_Serial.h" | ||
|
|
||
| class AP_RangeFinder_Ainstein_LR_D1 : public AP_RangeFinder_Backend_Serial | ||
| { | ||
|
|
||
| public: | ||
|
|
||
| static AP_RangeFinder_Backend_Serial *create( | ||
| RangeFinder::RangeFinder_State &_state, | ||
| AP_RangeFinder_Params &_params) { | ||
| return new AP_RangeFinder_Ainstein_LR_D1(_state, _params); | ||
| } | ||
|
|
||
| protected: | ||
|
|
||
| MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override { | ||
| return MAV_DISTANCE_SENSOR_RADAR; | ||
| } | ||
|
|
||
| uint32_t initial_baudrate(uint8_t serial_instance) const override { | ||
| return 115200; | ||
| } | ||
|
|
||
| private: | ||
|
|
||
| using AP_RangeFinder_Backend_Serial::AP_RangeFinder_Backend_Serial; | ||
|
|
||
| // get a reading | ||
| bool get_reading(float &reading_m) override; | ||
|
|
||
| // 0 is no return value, 100 is perfect. false means signal | ||
| // quality is not available | ||
| int8_t get_signal_quality_pct() const override { return signal_quality_pct; }; | ||
|
|
||
| static void report_malfunction(const uint8_t _malfunction_alert_); | ||
|
|
||
| enum class MalfunctionAlert : uint8_t { | ||
| Temperature = (1U << 0), // 0x01 | ||
| Voltage = (1U << 1), // 0x02 | ||
| IFSignalSaturation= (1U << 6), // 0x40 | ||
| AltitudeReading = (1U << 7), // 0x80 | ||
| }; | ||
|
|
||
| static constexpr uint8_t PACKET_SIZE = 32; | ||
| uint8_t malfunction_alert_prev; | ||
| int8_t signal_quality_pct = RangeFinder::SIGNAL_QUALITY_UNKNOWN; | ||
| }; | ||
| #endif |
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