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 : Simulator for the RPLidarA2 proximity sensor
17 : */
18 :
19 : #include "SIM_config.h"
20 :
21 : #if AP_SIM_PS_LIGHTWARE_SF45B_ENABLED
22 :
23 : #include "SIM_PS_LightWare_SF45B.h"
24 :
25 : #include <GCS_MAVLink/GCS.h>
26 : #include <stdio.h>
27 : #include <errno.h>
28 :
29 : using namespace SITL;
30 :
31 0 : uint32_t PS_LightWare_SF45B::packet_for_location(const Location &location,
32 : uint8_t *data,
33 : uint8_t buflen)
34 : {
35 0 : return 0;
36 : }
37 :
38 52464 : void PS_LightWare_SF45B::move_preamble_in_buffer(uint8_t search_start_pos)
39 : {
40 : uint8_t i;
41 52488 : for (i=search_start_pos; i<_buflen; i++) {
42 64 : if ((uint8_t)_msg.buffer[i] == PREAMBLE) {
43 40 : break;
44 : }
45 : }
46 52464 : if (i == 0) {
47 52440 : return;
48 : }
49 24 : memmove(_msg.buffer, &_msg.buffer[i], _buflen-i);
50 24 : _buflen = _buflen - i;
51 : }
52 :
53 : // handle a valid message in _msg.buffer
54 6264 : void PS_LightWare_SF45B::send(const char *data, uint32_t len)
55 : {
56 6264 : const ssize_t ret = write_to_autopilot(data, len);
57 6264 : if (ret < 0 || (uint32_t)ret != len) {
58 : // abort();
59 : }
60 6264 : }
61 :
62 24 : void PS_LightWare_SF45B::handle_message()
63 : {
64 : // GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Got message %u", (unsigned)_msg.packed_msgstream.msg.msgid);
65 24 : switch (MessageID(_msg.packed_msgstream.msg.msgid)) {
66 8 : case MessageID::STREAM: {
67 8 : stream = _msg.packed_msgstream.msg.stream;
68 8 : send_response.stream = true;
69 8 : return;
70 : }
71 8 : case MessageID::DISTANCE_OUTPUT: {
72 8 : desired_fields = _msg.packed_distance_output.msg.desired_fields;
73 8 : send_response.desired_fields = true;
74 8 : return;
75 : }
76 8 : case MessageID::UPDATE_RATE: {
77 8 : update_rate = _msg.packed_update_rate.msg.rate;
78 8 : send_response.update_rate = true;
79 8 : return;
80 : }
81 0 : case MessageID::DISTANCE_DATA_CM: {
82 0 : ::fprintf(stderr, "Got a distance data. Weird.\n");
83 0 : return;
84 : }
85 : }
86 : // AP_HAL::panic("Unrecognised message (%u)", _msg.common.msgid);
87 0 : ::fprintf(stderr, "Unrecognised message (%u)\n", _msg.packed_msgstream.msg.msgid);
88 : }
89 :
90 52440 : void PS_LightWare_SF45B::update_input()
91 : {
92 52440 : const ssize_t n = read_from_autopilot(&_msg.buffer[_buflen], ARRAY_SIZE(_msg.buffer) - _buflen - 1);
93 52440 : if (n < 0) {
94 : // TODO: do better here
95 0 : if (errno != EAGAIN && errno != EWOULDBLOCK && errno != 0) {
96 0 : AP_HAL::panic("Failed to read from autopilot");
97 : }
98 : } else {
99 52440 : _buflen += n;
100 : }
101 :
102 52440 : switch (_inputstate) {
103 52440 : case InputState::WANT_PREAMBLE:
104 52440 : move_preamble_in_buffer();
105 52440 : if (_buflen == 0) {
106 52416 : return;
107 : }
108 24 : set_inputstate(InputState::WANT_FLAGS);
109 : FALLTHROUGH;
110 24 : case InputState::WANT_FLAGS:
111 24 : if (_buflen < 4) { // 1 preamble, 2 flags, 1 msg
112 0 : return;
113 : }
114 24 : set_inputstate(InputState::WANT_PAYLOAD);
115 : FALLTHROUGH;
116 24 : case InputState::WANT_PAYLOAD: {
117 : // 1 preamble, 2 flags, 1 msg + payload
118 24 : const uint8_t want_len = 4 + payload_length();
119 24 : if (_buflen < want_len) {
120 0 : return;
121 : }
122 24 : set_inputstate(InputState::WANT_CRC);
123 : FALLTHROUGH;
124 : }
125 24 : case InputState::WANT_CRC: {
126 : // 1 preamble, 2 flags, 1 msg + payload + 2 crc
127 24 : const uint8_t want_len = 4 + payload_length() + 2;
128 24 : if (_buflen < want_len) {
129 0 : return;
130 : }
131 24 : const uint16_t got_checksum = UINT16_VALUE(uint8_t(_msg.buffer[want_len-1]), uint8_t(_msg.buffer[want_len-2]));
132 24 : const uint16_t calc_checksum = msg_checksum();
133 24 : if (calc_checksum == got_checksum) {
134 : // GCS_SEND_TEXT(MAV_SEVERITY_INFO, "sim-SF45B: Got one (%u)!", _msg.common.msgid);
135 : // see if this was a read or a write request..
136 24 : handle_message();
137 : } else {
138 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "sim-SF45B: Bad checksum");
139 : }
140 : // consume these bytes
141 24 : move_preamble_in_buffer(want_len-1);
142 24 : set_inputstate(InputState::WANT_PREAMBLE);
143 24 : return;
144 : }
145 : }
146 : }
147 :
148 52440 : void PS_LightWare_SF45B::update(const Location &location)
149 : {
150 52440 : update_input();
151 52440 : update_output(location);
152 52440 : }
153 :
154 52440 : void PS_LightWare_SF45B::update_output(const Location &location)
155 : {
156 52440 : switch (_state) {
157 52440 : case State::SCANNING:
158 52440 : update_output_responses();
159 52440 : update_output_scan(location);
160 52440 : return;
161 : }
162 : }
163 :
164 52440 : void PS_LightWare_SF45B::update_output_responses()
165 : {
166 52440 : if (send_response.stream) {
167 8 : send_response.stream = false;
168 8 : PackedMessage<MsgStream> packed { MsgStream(stream), 0x1 };
169 8 : packed.update_checksum();
170 8 : send((char*)&packed, sizeof(packed));
171 : }
172 52440 : if (send_response.update_rate) {
173 8 : send_response.update_rate = false;
174 8 : PackedMessage<UpdateRate> packed { UpdateRate(update_rate), 0x1 };
175 8 : packed.update_checksum();
176 8 : send((char*)&packed, sizeof(packed));
177 : }
178 52440 : if (send_response.desired_fields) {
179 8 : send_response.desired_fields = false;
180 8 : PackedMessage<DistanceOutput> packed { DistanceOutput(desired_fields), 0x1 };
181 8 : packed.update_checksum();
182 8 : send((char*)&packed, sizeof(packed));
183 : }
184 52440 : }
185 :
186 52440 : void PS_LightWare_SF45B::update_output_scan(const Location &location)
187 : {
188 52440 : const uint32_t now_ms = AP_HAL::millis();
189 52440 : const uint32_t time_delta_ms = (now_ms - last_scan_output_time_ms);
190 52440 : if (time_delta_ms > 1000) {
191 0 : last_scan_output_time_ms = now_ms;
192 0 : return;
193 : }
194 :
195 52440 : const uint32_t samples_per_second = 133;
196 52440 : const float samples_per_ms = samples_per_second / 1000.0f;
197 52440 : const uint32_t sample_count = samples_per_ms * time_delta_ms;
198 52440 : const float degrees_per_ms = 390 / 1000.0f;
199 52440 : const float degrees_per_sample = degrees_per_ms / samples_per_ms;
200 :
201 : // ::fprintf(stderr, "Packing %u samples in for %ums interval (%f degrees/sample)\n", sample_count, time_delta, degrees_per_sample);
202 :
203 52440 : last_scan_output_time_ms += sample_count/samples_per_ms;
204 :
205 58680 : for (uint32_t i=0; i<sample_count; i++) {
206 :
207 6240 : const float ANGLE_MIN_DEG = -170;
208 6240 : const float ANGLE_MAX_DEG = +170;
209 6240 : float current_degrees_bf = last_degrees_bf + (last_dir * degrees_per_sample);
210 6240 : if (current_degrees_bf < ANGLE_MIN_DEG) {
211 27 : current_degrees_bf += (ANGLE_MIN_DEG - current_degrees_bf);
212 27 : last_dir = -last_dir;
213 : }
214 6240 : if (current_degrees_bf > ANGLE_MAX_DEG) {
215 27 : current_degrees_bf += (ANGLE_MAX_DEG - current_degrees_bf);
216 27 : last_dir = -last_dir;
217 : }
218 6240 : last_degrees_bf = current_degrees_bf;
219 :
220 :
221 6240 : const float MAX_RANGE = 53.0f;
222 6240 : float distance = measure_distance_at_angle_bf(location, current_degrees_bf);
223 : // ::fprintf(stderr, "SIM: %f=%fm\n", current_degrees_bf, distance);
224 6240 : if (distance > MAX_RANGE) {
225 : // sensor returns -1 for out-of-range
226 817 : distance = -1.0f;
227 : }
228 :
229 : PackedMessage<DistanceDataCM> packed_distance_data {
230 : DistanceDataCM(
231 6240 : uint16_t(distance*100.0),
232 6240 : wrap_180(current_degrees_bf) * 100
233 6240 : ), 0x1 };
234 6240 : packed_distance_data.update_checksum();
235 6240 : send((char*)&packed_distance_data, sizeof(packed_distance_data));
236 : }
237 : }
238 :
239 : #endif // AP_SIM_PS_LIGHTWARE_SF45B_ENABLED
|
