APM:Libraries
AP_OpticalFlow_Onboard.cpp
Go to the documentation of this file.
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 #include "AP_OpticalFlow_Onboard.h"
16 
17 #include <AP_HAL/AP_HAL.h>
18 
19 #include "OpticalFlow.h"
20 
21 #if CONFIG_HAL_BOARD == HAL_BOARD_LINUX &&\
22  (CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP ||\
23  CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_MINLURE)
24 
25 #ifndef OPTICALFLOW_ONBOARD_DEBUG
26 #define OPTICALFLOW_ONBOARD_DEBUG 0
27 #endif
28 
29 #define OPTICALFLOW_ONBOARD_ID 1
30 extern const AP_HAL::HAL& hal;
31 
33  OpticalFlow_backend(_frontend)
34 {}
35 
37 {
38  /* register callback to get gyro data */
39  hal.opticalflow->init();
40 }
41 
43 {
45  // read at maximum 10Hz
46  uint32_t now = AP_HAL::millis();
47  if (now - _last_read_ms < 100) {
48  return;
49  }
50  _last_read_ms = now;
51 
52  if (!hal.opticalflow->read(data_frame)) {
53  return;
54  }
55 
56  struct OpticalFlow::OpticalFlow_state state;
58  state.surface_quality = data_frame.quality;
59  if (data_frame.delta_time > 0) {
60  const Vector2f flowScaler = _flowScaler();
61  float flowScaleFactorX = 1.0f + 0.001f * flowScaler.x;
62  float flowScaleFactorY = 1.0f + 0.001f * flowScaler.y;
63 
64  // delta_time is in microseconds and flow is in milliradians
65  // per second, so multiply by 1000
66  state.flowRate.x = flowScaleFactorX * 1000.0f /
67  float(data_frame.delta_time) *
68  data_frame.pixel_flow_x_integral;
69 
70  state.flowRate.y = flowScaleFactorY * 1000.0f /
71  float(data_frame.delta_time) *
72  data_frame.pixel_flow_y_integral;
73 
74  // delta_time is in microseconds so multiply to get back to seconds
75  state.bodyRate.x = 1000000.0f / float(data_frame.delta_time) *
76  data_frame.gyro_x_integral;
77 
78  state.bodyRate.y = 1000000.0f / float(data_frame.delta_time) *
79  data_frame.gyro_y_integral;
80 
81  _applyYaw(state.flowRate);
82  } else {
83  state.flowRate.zero();
84  state.bodyRate.zero();
85  }
86 
87  // copy results to front end
88  _update_frontend(state);
89 
90 #if OPTICALFLOW_ONBOARD_DEBUG
91  hal.console->printf("FLOW_ONBOARD qual:%u FlowRateX:%4.2f Y:%4.2f"
92  "BodyRateX:%4.2f Y:%4.2f, delta_time = %u\n",
93  (unsigned)state.surface_quality,
94  (double)state.flowRate.x,
95  (double)state.flowRate.y,
96  (double)state.bodyRate.x,
97  (double)state.bodyRate.y,
98  data_frame.delta_time);
99 #endif
100 }
101 
102 #endif
void _applyYaw(Vector2f &v)
Vector2f _flowScaler(void) const
AP_HAL::UARTDriver * console
Definition: HAL.h:110
T y
Definition: vector2.h:37
#define OPTICALFLOW_ONBOARD_ID
virtual void printf(const char *,...) FMT_PRINTF(2
Definition: BetterStream.cpp:5
AP_HAL::OpticalFlow * opticalflow
Definition: HAL.h:116
void _update_frontend(const struct OpticalFlow::OpticalFlow_state &state)
uint32_t millis()
Definition: system.cpp:157
const AP_HAL::HAL & hal
-*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
Definition: AC_PID_test.cpp:14
virtual void init()=0
T x
Definition: vector2.h:37
virtual bool read(Data_Frame &frame)=0
void zero()
Definition: vector2.h:125
AP_OpticalFlow_Onboard(OpticalFlow &_frontend)