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 LightWare S45B proximity sensor
17 :
18 : ./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --serial5=sim:sf45b --speedup=1 -l 51.8752066,14.6487840,54.15,0
19 :
20 : param set SERIAL5_PROTOCOL 11 # proximity
21 : param set PRX1_TYPE 8 # s45b
22 : reboot
23 :
24 : arm throttle
25 : rc 3 1600
26 :
27 : # for avoidance:
28 : param set DISARM_DELAY 0
29 : param set AVOID_ENABLE 2 # use proximity sensor
30 : param set AVOID_MARGIN 2.00 # 2m
31 : param set AVOID_BEHAVE 0 # slide
32 : reboot
33 : mode loiter
34 : script /tmp/post-locations.scr
35 : arm throttle
36 : rc 3 1600
37 : rc 3 1500
38 : rc 2 1450
39 :
40 : */
41 :
42 : #pragma once
43 :
44 : #include "SIM_config.h"
45 :
46 : #if AP_SIM_PS_LIGHTWARE_SF45B_ENABLED
47 :
48 : #include "SIM_PS_LightWare.h"
49 :
50 : #include <AP_Math/crc.h>
51 : #include <AP_InternalError/AP_InternalError.h>
52 :
53 : namespace SITL {
54 :
55 : class PS_LightWare_SF45B : public PS_LightWare {
56 : public:
57 :
58 : using PS_LightWare::PS_LightWare;
59 :
60 : uint32_t packet_for_location(const Location &location,
61 : uint8_t *data,
62 : uint8_t buflen) override;
63 :
64 : void update(const Location &location) override;
65 :
66 : private:
67 :
68 : template <typename T>
69 : class PACKED PackedMessage {
70 : public:
71 6264 : PackedMessage(T _msg, uint16_t _flags) :
72 6264 : flags(_flags),
73 6264 : msg(_msg)
74 : {
75 6264 : flags |= (sizeof(T)) << 6;
76 6264 : }
77 : uint8_t preamble { PREAMBLE };
78 : uint16_t flags;
79 : T msg;
80 : uint16_t checksum;
81 :
82 6264 : uint16_t calculate_checksum(uint16_t len) const WARN_IF_UNUSED {
83 6264 : uint16_t ret = 0;
84 56352 : for (uint8_t i=0; i<len; i++) {
85 50088 : ret = crc_xmodem_update(ret, ((const char*)this)[i]);
86 : }
87 6264 : return ret;
88 : }
89 6264 : uint16_t calculate_checksum() const WARN_IF_UNUSED {
90 6264 : return calculate_checksum(3+sizeof(T));
91 : }
92 :
93 6264 : void update_checksum() {
94 6264 : checksum = calculate_checksum();
95 6264 : }
96 : };
97 :
98 : class PACKED MsgStream {
99 : public:
100 8 : MsgStream(uint32_t _stream) :
101 8 : stream(_stream)
102 8 : { }
103 : uint8_t msgid { (uint8_t)MessageID::STREAM };
104 : uint32_t stream;
105 : };
106 :
107 : class PACKED DistanceOutput {
108 : public:
109 8 : DistanceOutput(uint32_t _desired_fields) :
110 8 : desired_fields(_desired_fields)
111 8 : { }
112 : uint8_t msgid { (uint8_t)MessageID::DISTANCE_OUTPUT };
113 : uint32_t desired_fields;
114 : };
115 :
116 : class PACKED UpdateRate {
117 : public:
118 8 : UpdateRate(uint8_t _rate) :
119 8 : rate(_rate)
120 8 : { }
121 : uint8_t msgid { (uint8_t)MessageID::UPDATE_RATE };
122 : uint8_t rate;
123 : };
124 :
125 : class PACKED DistanceDataCM {
126 : public:
127 6240 : DistanceDataCM(uint16_t _distance_cm, uint16_t _angle_cd) :
128 6240 : distance_cm(_distance_cm),
129 6240 : angle_cd(_angle_cd)
130 6240 : { }
131 : uint8_t msgid { (uint8_t)MessageID::DISTANCE_DATA_CM };
132 : uint16_t distance_cm;
133 : uint16_t angle_cd;
134 : };
135 :
136 : // message ids
137 : enum class MessageID : uint8_t {
138 : // PRODUCT_NAME = 0,
139 : // HARDWARE_VERSION = 1,
140 : // FIRMWARE_VERSION = 2,
141 : // SERIAL_NUMBER = 3,
142 : // TEXT_MESSAGE = 7,
143 : // USER_DATA = 9,
144 : // TOKEN = 10,
145 : // SAVE_PARAMETERS = 12,
146 : // RESET = 14,
147 : // STAGE_FIRMWARE = 16,
148 : // COMMIT_FIRMWARE = 17,
149 : DISTANCE_OUTPUT = 27,
150 : STREAM = 30,
151 : DISTANCE_DATA_CM = 44,
152 : // DISTANCE_DATA_MM = 45,
153 : // LASER_FIRING = 50,
154 : // TEMPERATURE = 57,
155 : UPDATE_RATE = 66,
156 : // NOISE = 74,
157 : // ZERO_OFFSET = 75,
158 : // LOST_SIGNAL_COUNTER = 76,
159 : // BAUD_RATE = 79,
160 : // I2C_ADDRESS = 80,
161 : // STEPPER_STATUS = 93,
162 : // SCAN_ON_STARTUP = 94,
163 : // SCAN_ENABLE = 96,
164 : // SCAN_POSITION = 97,
165 : // SCAN_LOW_ANGLE = 98,
166 : // SCAN_HIGH_ANGLE = 99
167 : };
168 :
169 :
170 : /*
171 : * Input Handling
172 : */
173 : void update_input();
174 :
175 : // make the message in the buffer a read message and return to sender
176 : void boomerang_buffer_message(uint8_t len);
177 :
178 : // handle a complete checksummed message
179 : void handle_message();
180 :
181 : void send(const char *data, uint32_t len);
182 :
183 : union u {
184 1 : u() {}
185 : char buffer[256]; // from-autopilot
186 : PackedMessage<MsgStream> packed_msgstream;
187 : PackedMessage<DistanceOutput> packed_distance_output;
188 : PackedMessage<UpdateRate> packed_update_rate;
189 : } _msg;
190 : uint8_t _buflen;
191 :
192 24 : static uint16_t checksum_bytes(const char *buffer, uint8_t len) {
193 24 : uint16_t crc = 0;
194 192 : for (uint8_t i=0; i<len; i++) {
195 168 : crc = crc_xmodem_update(crc, buffer[i]);
196 : }
197 24 : return crc;
198 : }
199 :
200 24 : uint16_t msg_checksum() const {
201 : // 4 is 1 preamble, 2 flags, 1 msgid
202 24 : return checksum_bytes(_msg.buffer, payload_length() + 4);
203 : }
204 :
205 72 : uint8_t payload_length() const {
206 72 : if (_buflen < 3) {
207 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
208 : }
209 72 : return (_msg.packed_msgstream.flags >> 6) - 1;
210 : }
211 :
212 : static const uint8_t PREAMBLE = 0xAA;
213 :
214 : void move_preamble_in_buffer(uint8_t search_start_pos=0);
215 :
216 : enum class InputState {
217 : WANT_PREAMBLE = 45,
218 : WANT_FLAGS = 46,
219 : WANT_PAYLOAD = 47,
220 : WANT_CRC = 48,
221 : };
222 : InputState _inputstate = InputState::WANT_PREAMBLE;
223 96 : void set_inputstate(InputState newstate) {
224 : // ::fprintf(stderr, "Moving from inputstate (%u) to (%u)\n", (uint8_t)_inputstate, (uint8_t)newstate);
225 96 : _inputstate = newstate;
226 96 : }
227 :
228 : /*
229 : * SETTINGS
230 : */
231 : uint32_t stream;
232 : uint32_t desired_fields;
233 : uint8_t update_rate;
234 :
235 : /*
236 : * OUTPUT HANDLING
237 : */
238 :
239 : struct {
240 : bool stream;
241 : bool desired_fields;
242 : bool update_rate;
243 : } send_response;
244 :
245 : enum class State {
246 : SCANNING = 21,
247 : };
248 : State _state = State::SCANNING;
249 : void set_state(State newstate) {
250 : // ::fprintf(stderr, "Moving from state (%u) to (%u)\n", (uint8_t)_state, (uint8_t)newstate);
251 : _state = newstate;
252 : }
253 :
254 : void update_output(const Location &location);
255 : void update_output_responses();
256 : void update_output_scan(const Location &location);
257 :
258 : uint32_t last_scan_output_time_ms;
259 :
260 : float last_degrees_bf; // previous iteration's lidar angle
261 : float last_dir = 1; // previous iterations movement direction. +1 CW, -1 for CCW
262 :
263 : };
264 :
265 : };
266 :
267 : #endif // AP_SIM_PS_LIGHTWARE_SF45B_ENABLED
|
