Line data Source code
1 : #include <AP_BoardConfig/AP_BoardConfig.h>
2 : #include "AP_OpticalFlow.h"
3 :
4 : #if AP_OPTICALFLOW_ENABLED
5 :
6 : #include "AP_OpticalFlow_Onboard.h"
7 : #include "AP_OpticalFlow_SITL.h"
8 : #include "AP_OpticalFlow_Pixart.h"
9 : #include "AP_OpticalFlow_PX4Flow.h"
10 : #include "AP_OpticalFlow_CXOF.h"
11 : #include "AP_OpticalFlow_MAV.h"
12 : #include "AP_OpticalFlow_HereFlow.h"
13 : #include "AP_OpticalFlow_MSP.h"
14 : #include "AP_OpticalFlow_UPFLOW.h"
15 : #include <AP_Logger/AP_Logger.h>
16 : #include <GCS_MAVLink/GCS.h>
17 : #include <AP_AHRS/AP_AHRS.h>
18 :
19 : extern const AP_HAL::HAL& hal;
20 :
21 : #ifndef OPTICAL_FLOW_TYPE_DEFAULT
22 : #define OPTICAL_FLOW_TYPE_DEFAULT Type::NONE
23 : #endif
24 :
25 : const AP_Param::GroupInfo AP_OpticalFlow::var_info[] = {
26 : // @Param: _TYPE
27 : // @DisplayName: Optical flow sensor type
28 : // @Description: Optical flow sensor type
29 : // @SortValues: AlphabeticalZeroAtTop
30 : // @Values: 0:None, 1:PX4Flow, 2:Pixart, 3:Bebop, 4:CXOF, 5:MAVLink, 6:DroneCAN, 7:MSP, 8:UPFLOW, 10:SITL
31 : // @User: Standard
32 : // @RebootRequired: True
33 : AP_GROUPINFO_FLAGS("_TYPE", 0, AP_OpticalFlow, _type, (float)OPTICAL_FLOW_TYPE_DEFAULT, AP_PARAM_FLAG_ENABLE),
34 :
35 : // @Param: _FXSCALER
36 : // @DisplayName: X axis optical flow scale factor correction
37 : // @Description: This sets the parts per thousand scale factor correction applied to the flow sensor X axis optical rate. It can be used to correct for variations in effective focal length. Each positive increment of 1 increases the scale factor applied to the X axis optical flow reading by 0.1%. Negative values reduce the scale factor.
38 : // @Range: -800 +800
39 : // @Increment: 1
40 : // @User: Standard
41 : AP_GROUPINFO("_FXSCALER", 1, AP_OpticalFlow, _flowScalerX, 0),
42 :
43 : // @Param: _FYSCALER
44 : // @DisplayName: Y axis optical flow scale factor correction
45 : // @Description: This sets the parts per thousand scale factor correction applied to the flow sensor Y axis optical rate. It can be used to correct for variations in effective focal length. Each positive increment of 1 increases the scale factor applied to the Y axis optical flow reading by 0.1%. Negative values reduce the scale factor.
46 : // @Range: -800 +800
47 : // @Increment: 1
48 : // @User: Standard
49 : AP_GROUPINFO("_FYSCALER", 2, AP_OpticalFlow, _flowScalerY, 0),
50 :
51 : // @Param: _ORIENT_YAW
52 : // @DisplayName: Flow sensor yaw alignment
53 : // @Description: Specifies the number of centi-degrees that the flow sensor is yawed relative to the vehicle. A sensor with its X-axis pointing to the right of the vehicle X axis has a positive yaw angle.
54 : // @Units: cdeg
55 : // @Range: -17999 +18000
56 : // @Increment: 10
57 : // @User: Standard
58 : AP_GROUPINFO("_ORIENT_YAW", 3, AP_OpticalFlow, _yawAngle_cd, 0),
59 :
60 : // @Param: _POS_X
61 : // @DisplayName: X position offset
62 : // @Description: X position of the optical flow sensor focal point in body frame. Positive X is forward of the origin.
63 : // @Units: m
64 : // @Range: -5 5
65 : // @Increment: 0.01
66 : // @User: Advanced
67 :
68 : // @Param: _POS_Y
69 : // @DisplayName: Y position offset
70 : // @Description: Y position of the optical flow sensor focal point in body frame. Positive Y is to the right of the origin.
71 : // @Units: m
72 : // @Range: -5 5
73 : // @Increment: 0.01
74 : // @User: Advanced
75 :
76 : // @Param: _POS_Z
77 : // @DisplayName: Z position offset
78 : // @Description: Z position of the optical flow sensor focal point in body frame. Positive Z is down from the origin.
79 : // @Units: m
80 : // @Range: -5 5
81 : // @Increment: 0.01
82 : // @User: Advanced
83 : AP_GROUPINFO("_POS", 4, AP_OpticalFlow, _pos_offset, 0.0f),
84 :
85 : // @Param: _ADDR
86 : // @DisplayName: Address on the bus
87 : // @Description: This is used to select between multiple possible I2C addresses for some sensor types. For PX4Flow you can choose 0 to 7 for the 8 possible addresses on the I2C bus.
88 : // @Range: 0 127
89 : // @User: Advanced
90 : AP_GROUPINFO("_ADDR", 5, AP_OpticalFlow, _address, 0),
91 :
92 : // @Param{Rover}: _HGT_OVR
93 : // @DisplayName: Height override of sensor above ground
94 : // @Description: This is used in rover vehicles, where the sensor is a fixed height above the ground
95 : // @Units: m
96 : // @Range: 0 2
97 : // @Increment: 0.01
98 : // @User: Advanced
99 : AP_GROUPINFO_FRAME("_HGT_OVR", 6, AP_OpticalFlow, _height_override, 0.0f, AP_PARAM_FRAME_ROVER),
100 :
101 : // @Param: _OPTIONS
102 : // @DisplayName: Optical flow options
103 : // @Description: Optical flow options. Bit 0 should be set if the sensor is stabilised (e.g. mounted on a gimbal)
104 : // @Bitmask: 0:Roll/Pitch stabilised
105 : // @User: Standard
106 : AP_GROUPINFO("_OPTIONS", 7, AP_OpticalFlow, _options, 0),
107 :
108 : AP_GROUPEND
109 : };
110 :
111 : // default constructor
112 2061 : AP_OpticalFlow::AP_OpticalFlow()
113 : {
114 2061 : _singleton = this;
115 :
116 2061 : AP_Param::setup_object_defaults(this, var_info);
117 2061 : }
118 :
119 1367 : void AP_OpticalFlow::init(uint32_t log_bit)
120 : {
121 1367 : _log_bit = log_bit;
122 :
123 : // return immediately if not enabled or backend already created
124 1367 : if ((_type == Type::NONE) || (backend != nullptr)) {
125 1353 : return;
126 : }
127 :
128 14 : switch ((Type)_type) {
129 0 : case Type::NONE:
130 0 : break;
131 : #if AP_OPTICALFLOW_PX4FLOW_ENABLED
132 0 : case Type::PX4FLOW:
133 0 : backend = AP_OpticalFlow_PX4Flow::detect(*this);
134 0 : break;
135 : #endif // AP_OPTICALFLOW_PX4FLOW_ENABLED
136 : #if AP_OPTICALFLOW_PIXART_ENABLED
137 0 : case Type::PIXART:
138 0 : backend = AP_OpticalFlow_Pixart::detect("pixartflow", *this);
139 0 : if (backend == nullptr) {
140 0 : backend = AP_OpticalFlow_Pixart::detect("pixartPC15", *this);
141 : }
142 0 : break;
143 : #endif // AP_OPTICALFLOW_PIXART_ENABLED
144 : #if AP_OPTICALFLOW_ONBOARD_ENABLED
145 : case Type::BEBOP:
146 : backend = NEW_NOTHROW AP_OpticalFlow_Onboard(*this);
147 : break;
148 : #endif // AP_OPTICALFLOW_ONBOARD_ENABLED
149 : #if AP_OPTICALFLOW_CXOF_ENABLED
150 0 : case Type::CXOF:
151 0 : backend = AP_OpticalFlow_CXOF::detect(*this);
152 0 : break;
153 : #endif // AP_OPTICALFLOW_CXOF_ENABLED
154 : #if AP_OPTICALFLOW_MAV_ENABLED
155 0 : case Type::MAVLINK:
156 0 : backend = AP_OpticalFlow_MAV::detect(*this);
157 0 : break;
158 : #endif // AP_OPTICALFLOW_MAV_ENABLED
159 : #if AP_OPTICALFLOW_HEREFLOW_ENABLED
160 0 : case Type::UAVCAN:
161 0 : backend = NEW_NOTHROW AP_OpticalFlow_HereFlow(*this);
162 0 : break;
163 : #endif // AP_OPTICALFLOW_HEREFLOW_ENABLED
164 : #if HAL_MSP_OPTICALFLOW_ENABLED
165 0 : case Type::MSP:
166 0 : backend = AP_OpticalFlow_MSP::detect(*this);
167 0 : break;
168 : #endif // HAL_MSP_OPTICALFLOW_ENABLED
169 : #if AP_OPTICALFLOW_UPFLOW_ENABLED
170 0 : case Type::UPFLOW:
171 0 : backend = AP_OpticalFlow_UPFLOW::detect(*this);
172 0 : break;
173 : #endif // AP_OPTICALFLOW_UPFLOW_ENABLED
174 : #if AP_OPTICALFLOW_SITL_ENABLED
175 14 : case Type::SITL:
176 14 : backend = NEW_NOTHROW AP_OpticalFlow_SITL(*this);
177 14 : break;
178 : #endif // AP_OPTICALFLOW_SITL_ENABLED
179 : }
180 :
181 14 : if (backend != nullptr) {
182 14 : backend->init();
183 : }
184 : }
185 :
186 19661679 : void AP_OpticalFlow::update(void)
187 : {
188 : // exit immediately if not enabled
189 19661679 : if (!enabled()) {
190 19455788 : return;
191 : }
192 205891 : if (backend != nullptr) {
193 202181 : backend->update();
194 : }
195 :
196 : // only healthy if the data is less than 0.5s old
197 205891 : _flags.healthy = (AP_HAL::millis() - _last_update_ms < 500);
198 :
199 : #if AP_OPTICALFLOW_CALIBRATOR_ENABLED
200 : // update calibrator and save resulting scaling
201 205891 : if (_calibrator != nullptr) {
202 18751 : if (_calibrator->update()) {
203 : // apply new calibration values
204 1 : const Vector2f new_scaling = _calibrator->get_scalars();
205 1 : const float flow_scalerx_as_multiplier = (1.0 + (_flowScalerX * 0.001)) * new_scaling.x;
206 1 : const float flow_scalery_as_multiplier = (1.0 + (_flowScalerY * 0.001)) * new_scaling.y;
207 1 : _flowScalerX.set_and_save_ifchanged((flow_scalerx_as_multiplier - 1.0) * 1000.0);
208 1 : _flowScalerY.set_and_save_ifchanged((flow_scalery_as_multiplier - 1.0) * 1000.0);
209 1 : _flowScalerX.notify();
210 1 : _flowScalerY.notify();
211 1 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "FlowCal: FLOW_FXSCALER=%d, FLOW_FYSCALER=%d", (int)_flowScalerX, (int)_flowScalerY);
212 : }
213 : }
214 : #endif
215 : }
216 :
217 0 : void AP_OpticalFlow::handle_msg(const mavlink_message_t &msg)
218 : {
219 : // exit immediately if not enabled
220 0 : if (!enabled()) {
221 0 : return;
222 : }
223 :
224 0 : if (backend != nullptr) {
225 0 : backend->handle_msg(msg);
226 : }
227 : }
228 :
229 : #if HAL_MSP_OPTICALFLOW_ENABLED
230 0 : void AP_OpticalFlow::handle_msp(const MSP::msp_opflow_data_message_t &pkt)
231 : {
232 : // exit immediately if not enabled
233 0 : if (!enabled()) {
234 0 : return;
235 : }
236 :
237 0 : if (backend != nullptr) {
238 0 : backend->handle_msp(pkt);
239 : }
240 : }
241 : #endif //HAL_MSP_OPTICALFLOW_ENABLED
242 :
243 : #if AP_OPTICALFLOW_CALIBRATOR_ENABLED
244 : // start calibration
245 1 : void AP_OpticalFlow::start_calibration()
246 : {
247 1 : if (_calibrator == nullptr) {
248 1 : _calibrator = NEW_NOTHROW AP_OpticalFlow_Calibrator();
249 1 : if (_calibrator == nullptr) {
250 0 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "FlowCal: failed to start");
251 0 : return;
252 : }
253 : }
254 1 : if (_calibrator != nullptr) {
255 1 : _calibrator->start();
256 : }
257 : }
258 :
259 : // stop calibration
260 4 : void AP_OpticalFlow::stop_calibration()
261 : {
262 4 : if (_calibrator != nullptr) {
263 1 : _calibrator->stop();
264 : }
265 4 : }
266 : #endif
267 :
268 10128 : void AP_OpticalFlow::update_state(const OpticalFlow_state &state)
269 : {
270 10128 : _state = state;
271 10128 : _last_update_ms = AP_HAL::millis();
272 :
273 : #if AP_AHRS_ENABLED
274 : // write to log and send to EKF if new data has arrived
275 20256 : AP::ahrs().writeOptFlowMeas(quality(),
276 10128 : _state.flowRate,
277 10128 : _state.bodyRate,
278 : _last_update_ms,
279 : get_pos_offset(),
280 : get_height_override());
281 : #endif
282 : #if HAL_LOGGING_ENABLED
283 10128 : Log_Write_Optflow();
284 : #endif
285 10128 : }
286 :
287 : #if HAL_LOGGING_ENABLED
288 10128 : void AP_OpticalFlow::Log_Write_Optflow()
289 : {
290 10128 : AP_Logger *logger = AP_Logger::get_singleton();
291 10128 : if (logger == nullptr) {
292 17 : return;
293 : }
294 20256 : if (_log_bit != (uint32_t)-1 &&
295 10128 : !logger->should_log(_log_bit)) {
296 17 : return;
297 : }
298 :
299 10111 : struct log_Optflow pkt = {
300 : LOG_PACKET_HEADER_INIT(LOG_OPTFLOW_MSG),
301 10111 : time_us : AP_HAL::micros64(),
302 10111 : surface_quality : _state.surface_quality,
303 10111 : flow_x : _state.flowRate.x,
304 10111 : flow_y : _state.flowRate.y,
305 10111 : body_x : _state.bodyRate.x,
306 10111 : body_y : _state.bodyRate.y
307 10111 : };
308 10111 : logger->WriteBlock(&pkt, sizeof(pkt));
309 : }
310 : #endif // HAL_LOGGING_ENABLED
311 :
312 :
313 : // singleton instance
314 : AP_OpticalFlow *AP_OpticalFlow::_singleton;
315 :
316 : namespace AP {
317 :
318 84466555 : AP_OpticalFlow *opticalflow()
319 : {
320 84466555 : return AP_OpticalFlow::get_singleton();
321 : }
322 :
323 : }
324 :
325 : #endif // AP_OPTICALFLOW_ENABLED
|
