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 : Base class for RPLidar support
16 : */
17 : /*
18 : Simulator for the RPLidar proximity sensor
19 :
20 : ./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --serial5=sim:rplidara2 --speedup=1 -l 51.8752066,14.6487840,54.15,0 --map
21 :
22 : param set SERIAL5_PROTOCOL 11
23 : param set PRX1_TYPE 5
24 : reboot
25 :
26 : arm throttle
27 : rc 3 1600
28 :
29 : # for avoidance:
30 : param set DISARM_DELAY 0
31 : param set AVOID_ENABLE 2 # use proximity sensor
32 : param set AVOID_MARGIN 2.00 # 2m
33 : param set AVOID_BEHAVE 0 # slide
34 : param set OA_DB_OUTPUT 3
35 : param set OA_TYPE 2
36 : reboot
37 : mode loiter
38 : script /tmp/post-locations.scr
39 : arm throttle
40 : rc 3 1600
41 : rc 3 1500
42 : rc 2 1450
43 :
44 : */
45 :
46 : #pragma once
47 :
48 : #include "SIM_config.h"
49 :
50 : #if AP_SIM_PS_RPLIDARA2_ENABLED || AP_SIM_PS_RPLIDARA1_ENABLED || AP_SIM_PS_RPLIDARS2_ENABLED
51 :
52 : #include "SIM_SerialProximitySensor.h"
53 :
54 : #include <stdio.h>
55 :
56 : namespace SITL {
57 :
58 : class PS_RPLidar : public SerialProximitySensor {
59 : public:
60 :
61 : using SerialProximitySensor::SerialProximitySensor;
62 :
63 : uint32_t packet_for_location(const Location &location,
64 : uint8_t *data,
65 : uint8_t buflen) override;
66 :
67 : void update(const Location &location) override;
68 :
69 : private:
70 :
71 : void update_input();
72 : void update_output(const Location &location);
73 :
74 : // 5-byte scan output
75 : void update_output_scan(const Location &location);
76 :
77 : // dense express scan output (40 samples per packet)
78 : void update_output_express_dense(const Location &location);
79 :
80 : uint32_t last_scan_output_time_ms;
81 :
82 : float last_degrees_bf;
83 :
84 : char _buffer[256]; // from-autopilot
85 : uint8_t _buflen;
86 :
87 : // TODO: see what happens on real device on partial input
88 : enum class State {
89 : IDLE = 17,
90 : SCANNING = 18,
91 : };
92 : State _state = State::IDLE;
93 3 : void set_state(State newstate) {
94 3 : ::fprintf(stderr, "Moving from state (%u) to (%u)\n", (uint8_t)_state, (uint8_t)newstate);
95 3 : _state = newstate;
96 3 : }
97 :
98 : enum class InputState {
99 : WAITING_FOR_PREAMBLE = 45,
100 : GOT_PREAMBLE = 46,
101 : RESETTING_START = 47,
102 : RESETTING_SEND_FIRMWARE_INFO = 48,
103 : };
104 : InputState _inputstate = InputState::WAITING_FOR_PREAMBLE;
105 12 : void set_inputstate(InputState newstate) {
106 12 : ::fprintf(stderr, "Moving from inputstate (%u) to (%u)\n", (uint8_t)_inputstate, (uint8_t)newstate);
107 12 : _inputstate = newstate;
108 12 : }
109 :
110 : static const uint8_t PREAMBLE = 0xA5;
111 :
112 : enum class Command : uint8_t {
113 : STOP = 0x25,
114 : SCAN = 0x20,
115 : FORCE_SCAN = 0x21,
116 : RESET = 0x40,
117 : GET_DEVICE_INFO = 0x50,
118 : GET_HEALTH = 0x52,
119 : EXPRESS_SCAN = 0x82,
120 : };
121 :
122 : void move_preamble_in_buffer();
123 :
124 : enum class DataType : uint8_t {
125 : Unknown04 = 0x04, // device info
126 : Unknown06 = 0x06, // health
127 : Unknown81 = 0x81, // scan (5-byte)
128 : Unknown85 = 0x85 // dense express scan
129 : };
130 :
131 : enum class SendMode {
132 : SRSR = 0x00,
133 : SRMR = 0x40, // no idea why this isn't 0x01
134 : };
135 :
136 : void send_response_descriptor(uint32_t data_response_length,
137 : SendMode sendmode,
138 : DataType datatype);
139 :
140 :
141 : // the driver expects to see an "R" followed by 62 bytes more crap.
142 : static const constexpr char *FIRMWARE_INFO = "R12345678901234567890123456789012345678901234567890123456789012";
143 : uint8_t _firmware_info_offset;
144 :
145 : // scan type
146 : enum class ScanMode {
147 : SCAN = 0,
148 : EXPRESS_SCAN_DENSE = 1,
149 : };
150 : ScanMode _scan_mode = ScanMode::SCAN;
151 :
152 : // state for dense express packets (current start angle in degrees)
153 : float _express_w_i_deg = 0.0f;
154 :
155 : // methods for sub-classes to implement:
156 : virtual uint8_t device_info_model() const = 0;
157 : virtual uint8_t max_range() const = 0;
158 : };
159 :
160 : };
161 :
162 : #endif // AP_SIM_PS_RPLIDARA2_ENABLED || AP_SIM_PS_RPLIDARA1_ENABLED || AP_SIM_PS_RPLIDARS2_ENABLED
|
