Line data Source code
1 : #pragma once
2 :
3 : #include "AP_Proximity_config.h"
4 :
5 : #if AP_PROXIMITY_LIGHTWARE_SF40C_ENABLED
6 :
7 : #include "AP_Proximity_Backend_Serial.h"
8 :
9 : #define PROXIMITY_SF40C_TIMEOUT_MS 200 // requests timeout after 0.2 seconds
10 : #define PROXIMITY_SF40C_PAYLOAD_LEN_MAX 256 // maximum payload size we can accept (in some configurations sensor may send as large as 1023)
11 : #define PROXIMITY_SF40C_COMBINE_READINGS 7 // combine this many readings together to improve efficiency
12 :
13 : class AP_Proximity_LightWareSF40C : public AP_Proximity_Backend_Serial
14 : {
15 :
16 : public:
17 : // constructor
18 : using AP_Proximity_Backend_Serial::AP_Proximity_Backend_Serial;
19 :
20 0 : uint16_t rxspace() const override {
21 0 : return 1280;
22 : };
23 :
24 : // update state
25 : void update(void) override;
26 :
27 : // get maximum and minimum distances (in meters) of sensor
28 0 : float distance_max_m() const override { return 100.0f; }
29 0 : float distance_min_m() const override { return 0.20f; }
30 :
31 : private:
32 :
33 : // initialise sensor
34 : void initialise();
35 :
36 : // restart sensor and re-init our state
37 : void restart_sensor();
38 :
39 : // message ids
40 : enum class MessageID : uint8_t {
41 : PRODUCT_NAME = 0,
42 : HARDWARE_VERSION = 1,
43 : FIRMWARE_VERSION = 2,
44 : SERIAL_NUMBER = 3,
45 : TEXT_MESSAGE = 7,
46 : USER_DATA = 9,
47 : TOKEN = 10,
48 : SAVE_PARAMETERS = 12,
49 : RESET = 14,
50 : STAGE_FIRMWARE = 16,
51 : COMMIT_FIRMWARE = 17,
52 : INCOMING_VOLTAGE = 20,
53 : STREAM = 30,
54 : DISTANCE_OUTPUT = 48,
55 : LASER_FIRING = 50,
56 : TEMPERATURE = 55,
57 : BAUD_RATE = 90,
58 : DISTANCE = 105,
59 : MOTOR_STATE = 106,
60 : MOTOR_VOLTAGE = 107,
61 : OUTPUT_RATE = 108,
62 : FORWARD_OFFSET = 109,
63 : REVOLUTIONS = 110,
64 : ALARM_STATE = 111,
65 : ALARM1 = 112,
66 : ALARM2 = 113,
67 : ALARM3 = 114,
68 : ALARM4 = 115,
69 : ALARM5 = 116,
70 : ALARM6 = 117,
71 : ALARM7 = 118
72 : };
73 :
74 : // motor states
75 : enum class MotorState : uint8_t {
76 : UNKNOWN = 0,
77 : PREPARING_FOR_STARTUP = 1,
78 : WAITING_FOR_FIVE_REVS = 2,
79 : RUNNING_NORMALLY = 3,
80 : FAILED_TO_COMMUNICATE = 4
81 : };
82 :
83 : // send message to sensor
84 : void send_message(MessageID msgid, bool write, const uint8_t *payload, uint16_t payload_len);
85 :
86 : // request motor state
87 : void request_motor_state();
88 :
89 : // request start of streaming of distances
90 : void request_stream_start();
91 :
92 : // request token of sensor (required for reset)
93 : void request_token();
94 0 : bool got_token() const { return (_sensor_state.token[0] != 0 || _sensor_state.token[1] != 0); }
95 0 : void clear_token() { memset(_sensor_state.token, 0, ARRAY_SIZE(_sensor_state.token)); }
96 :
97 : // request reset of sensor
98 : void request_reset();
99 :
100 : // check and process replies from sensor
101 : void process_replies();
102 :
103 : // process one byte received on serial port
104 : // state is stored in msg structure. when a full package is received process_message is called
105 : void parse_byte(uint8_t b);
106 :
107 : // process the latest message held in the msg structure
108 : void process_message();
109 :
110 : // internal variables
111 : uint32_t _last_request_ms; // system time of last request
112 : uint32_t _last_reply_ms; // system time of last valid reply
113 : uint32_t _last_restart_ms; // system time we restarted the sensor
114 : uint32_t _last_distance_received_ms; // system time of last distance measurement received from sensor
115 : AP_Proximity_Boundary_3D::Face _face; // face of _face_distance
116 : float _face_distance; // shortest distance (in meters) on face
117 : float _face_yaw_deg; // yaw angle (in degrees) of shortest distance on face
118 : bool _face_distance_valid; // true if face has at least one valid distance
119 :
120 : // state of sensor
121 : struct {
122 : MotorState motor_state; // motor state (1=starting-up,2=waiting for first 5 revs, 3=normal, 4=comm failure)
123 : uint8_t output_rate; // output rate number (0 = 20010, 1 = 10005, 2 = 6670, 3 = 2001)
124 : bool streaming; // true if distance messages are being streamed
125 : uint8_t token[2]; // token (supplied by sensor) required for reset
126 : } _sensor_state;
127 :
128 : enum class ParseState {
129 : HEADER = 0,
130 : FLAGS_L,
131 : FLAGS_H,
132 : MSG_ID,
133 : PAYLOAD,
134 : CRC_L,
135 : CRC_H
136 : };
137 :
138 : // structure holding latest message contents
139 : struct {
140 : ParseState state; // state of incoming message processing
141 : uint8_t flags_low; // flags low byte
142 : uint8_t flags_high; // flags high byte
143 : uint16_t payload_len; // latest message payload length (1+ bytes in payload)
144 : uint8_t payload[PROXIMITY_SF40C_PAYLOAD_LEN_MAX]; // payload
145 : MessageID msgid; // latest message's message id
146 : uint16_t payload_recv; // number of message's payload bytes received so far
147 : uint8_t crc_low; // crc low byte
148 : uint8_t crc_high; // crc high byte
149 : uint16_t crc_expected; // latest message's expected crc
150 : } _msg;
151 :
152 : // convert buffer to uint32, uint16
153 : uint32_t buff_to_uint32(uint8_t b0, uint8_t b1, uint8_t b2, uint8_t b3) const;
154 : uint16_t buff_to_uint16(uint8_t b0, uint8_t b1) const;
155 : };
156 :
157 : #endif // AP_PROXIMITY_LIGHTWARE_SF40C_ENABLED
|
