Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 :
16 : #include "AP_RangeFinder_config.h"
17 :
18 : #if AP_RANGEFINDER_AINSTEIN_LR_D1_ENABLED
19 :
20 : #ifndef AP_RANGEFINDER_AINSTEIN_LR_D1_SHOW_MALFUNCTIONS
21 : #define AP_RANGEFINDER_AINSTEIN_LR_D1_SHOW_MALFUNCTIONS 1
22 : #endif
23 :
24 : #include "AP_RangeFinder_Ainstein_LR_D1.h"
25 : #include <GCS_MAVLink/GCS.h>
26 :
27 : #include <AP_HAL/utility/sparse-endian.h>
28 :
29 : static const uint8_t PACKET_HEADER_MSB = 0xEB;
30 : static const uint8_t PACKET_HEADER_LSB = 0x90;
31 :
32 : // make sure we know what size the packet object is:
33 : // assert_storage_size<AP_RangeFinder_Ainstein_LR_D1::LRD1Union::LRD1Packet, 32> _assert_storage_lrd1_packet;
34 :
35 : // ensures that there is a packet starting at offset 0 in the buffer.
36 : // If that's not the case this returns false. Search starts at offset
37 : // start in the buffer - if a packet header is found at a non-zero
38 : // offset then the data is moved to the start of the buffer.
39 7114 : bool AP_RangeFinder_Ainstein_LR_D1::move_signature_in_buffer(uint8_t start)
40 : {
41 7114 : for (uint8_t i=start; i<buffer_used; i++) {
42 3579 : if (u.buffer[i] == PACKET_HEADER_MSB) {
43 3579 : memmove(&u.buffer[0], &u.buffer[i], buffer_used-i);
44 3579 : buffer_used -= i;
45 3579 : return true;
46 : }
47 : }
48 : // header byte not in buffer
49 3535 : buffer_used = 0;
50 3535 : return false;
51 : }
52 :
53 3557 : uint8_t AP_RangeFinder_Ainstein_LR_D1::LRD1Union::calculate_checksum() const
54 : {
55 : // the -4 here is 3 bytes of header and 1 byte of checksum
56 3557 : return crc_sum_of_bytes(&buffer[3], sizeof(u.packet)-4);
57 : }
58 :
59 : // get_reading - read all samples, return last. The device sends at
60 : // 40Hz, so there should only ever be one sample available. Assuming
61 : // that something's gone wrong with scheduling, we will simply return
62 : // the last.
63 7065 : bool AP_RangeFinder_Ainstein_LR_D1::get_reading(float &reading_m)
64 : {
65 7065 : bool ret = false;
66 10622 : for (uint8_t i=0; i<10; i++) {
67 10621 : float tmp_reading = 0;
68 10621 : if (!get_one_reading(tmp_reading)) {
69 7064 : break;
70 : }
71 3557 : ret = true;
72 3557 : reading_m = tmp_reading;
73 : }
74 7065 : return ret;
75 : }
76 :
77 : // get_one_reading - read a value from the sensor
78 10621 : bool AP_RangeFinder_Ainstein_LR_D1::get_one_reading(float &reading_m)
79 : {
80 10621 : if (uart == nullptr) {
81 0 : return false;
82 : }
83 :
84 10621 : const uint8_t num_read = uart->read(&u.buffer[buffer_used], ARRAY_SIZE(u.buffer)-buffer_used);
85 10621 : buffer_used += num_read;
86 :
87 10621 : if (buffer_used == 0) {
88 7064 : return false;
89 : }
90 :
91 3557 : if (!move_signature_in_buffer(0)) {
92 0 : return false;
93 : }
94 :
95 3557 : if (buffer_used < sizeof(u.packet)) {
96 0 : return false;
97 : }
98 :
99 : // sanity checks; see data sheet on these fixed values.
100 10671 : if (u.packet.header_lsb != PACKET_HEADER_LSB ||
101 3557 : u.packet.device_id != 0x00 ||
102 10671 : u.packet.length != 28 ||
103 3557 : u.calculate_checksum() != u.packet.checksum) {
104 : // sanity checks failed - discard and try again next time we're called:
105 0 : move_signature_in_buffer(1);
106 0 : return false;
107 : }
108 :
109 : // distinguish between old and new packet format by inspecting
110 : // "byte 12" (offset 11); in the old format this will always be
111 : // 0xff. This was reserved for "offset count", now is altitude-valid.
112 3557 : const bool is_v19000 = (u.packet_v19000.altitude_valid != 0xff);
113 :
114 : // same for both packet formats:
115 3557 : reading_m = be16toh(u.packet.object1_alt) * 0.01;
116 :
117 : #if AP_RANGEFINDER_AINSTEIN_LR_D1_SHOW_MALFUNCTIONS
118 3557 : const uint32_t now_ms = AP_HAL::millis();
119 : uint16_t malfunction_alert;
120 3557 : if (is_v19000) {
121 : // new packet format allows for 16 bits:
122 1433 : malfunction_alert = be16toh(u.packet_v19000.malfunction_alert);
123 : } else {
124 2124 : malfunction_alert = u.packet.malfunction_alert;
125 : }
126 3557 : if (malfunction_alert_prev != malfunction_alert && now_ms - malfunction_alert_last_send_ms >= 1000) {
127 4 : if (is_v19000) {
128 2 : report_malfunction_v19000(malfunction_alert, malfunction_alert_prev);
129 : } else {
130 2 : report_malfunction(malfunction_alert, malfunction_alert_prev);
131 : }
132 4 : malfunction_alert_prev = malfunction_alert;
133 4 : malfunction_alert_last_send_ms = now_ms;
134 : }
135 : #endif
136 :
137 3557 : const uint8_t snr = u.packet.object1_snr;
138 :
139 3557 : signal_quality_pct = linear_interpolate(
140 : RangeFinder::SIGNAL_QUALITY_MIN, RangeFinder::SIGNAL_QUALITY_MAX,
141 : snr,
142 : 0, 255
143 : );
144 :
145 3557 : if (is_v19000 && (!u.packet_v19000.altitude_valid ||
146 1073 : (malfunction_alert & uint16_t(MalfunctionAlert_v19000::AltitudeReadingOverflow)))) {
147 : // the device has specifically told us that the reading is not
148 : // valid, we can't interepret it further. It's still a
149 : // reading, so we still return true from this function, but we
150 : // can't say anything about our altitude so we mark ourselves
151 : // as out-of-range-high:
152 360 : reading_m = max_distance() + 1;
153 : } else {
154 : // try to infer out-of-range from the SNR field:
155 :
156 : /* From datasheet:
157 : Altitude measurements associated with a SNR value
158 : of 13dB or lower are considered erroneous.
159 :
160 : SNR values of 0 are considered out of maximum range (655 metres)
161 :
162 : The altitude measurements should not in any circumstances be used as true
163 : measurements independently of the corresponding SNR values.
164 : */
165 :
166 3197 : if (snr <= 13) {
167 358 : if (snr == 0) {
168 : // out-of-range high:
169 358 : reading_m = max_distance() + 1;
170 : }
171 : }
172 : }
173 :
174 : // consume this packet:
175 3557 : move_signature_in_buffer(sizeof(u.packet));
176 :
177 3557 : return true;
178 : }
179 :
180 : #if AP_RANGEFINDER_AINSTEIN_LR_D1_SHOW_MALFUNCTIONS
181 2 : void AP_RangeFinder_Ainstein_LR_D1::report_malfunction(const uint16_t _malfunction_alert_, const uint16_t _malfunction_alert_prev_)
182 : {
183 : static const struct {
184 : MalfunctionAlert bit;
185 : const char *name;
186 : } alerts[] {
187 : { MalfunctionAlert::Temperature, "Temperature" },
188 : { MalfunctionAlert::Voltage, "Voltage" },
189 : { MalfunctionAlert::IFSignalSaturation, "IF signal saturation" },
190 : { MalfunctionAlert::AltitudeReading, "Altitude reading overflow" },
191 : };
192 :
193 10 : for (const auto &alert : alerts) {
194 8 : if ((_malfunction_alert_ & uint8_t(alert.bit)) == 0) {
195 : // alert not current
196 5 : continue;
197 : }
198 3 : if ((_malfunction_alert_prev_ & uint8_t(alert.bit)) != 0) {
199 : // alert is not new
200 1 : continue;
201 : }
202 2 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "RangeFinder: %s alert", alert.name);
203 : }
204 2 : }
205 2 : void AP_RangeFinder_Ainstein_LR_D1::report_malfunction_v19000(const uint16_t malfunction_alert, const uint16_t malfunction_alert_prev)
206 : {
207 : static const struct {
208 : MalfunctionAlert_v19000 bit;
209 : const char *name;
210 : } alerts[] {
211 : { MalfunctionAlert_v19000::MCUTemperature, "MCU Temperature" },
212 : { MalfunctionAlert_v19000::MCUVoltage, "MCU Voltage" },
213 : { MalfunctionAlert_v19000::IFTemperature, "IF Temperature" },
214 : { MalfunctionAlert_v19000::IFSignalSaturation, "IF Signal Saturation" },
215 : { MalfunctionAlert_v19000::Software, "Software" },
216 : { MalfunctionAlert_v19000::AltitudeReadingOverflow, "Altitude Reading Overflow" },
217 : { MalfunctionAlert_v19000::Voltage, "Voltage" },
218 : };
219 :
220 16 : for (const auto &alert : alerts) {
221 14 : if ((malfunction_alert & uint16_t(alert.bit)) == 0) {
222 : // alert not current
223 11 : continue;
224 : }
225 3 : if ((malfunction_alert_prev & uint16_t(alert.bit)) != 0) {
226 : // alert is not new
227 1 : continue;
228 : }
229 2 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "RangeFinder: %s alert", alert.name);
230 : }
231 2 : }
232 : #endif // AP_RANGEFINDER_AINSTEIN_LR_D1_SHOW_MALFUNCTIONS
233 :
234 : #endif // AP_RANGEFINDER_AINSTEIN_LR_D1_ENABLED
|
