Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
px4flow.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2013 Gautier Hattenberger
3  * 2016 Michal Podhradsky <michal.podhradsky@aggiemail.usu.edu>
4  *
5  * This file is part of paparazzi.
6  *
7  * paparazzi is free software; you can redistribute it and/or modify
8  * it under the terms of the GNU General Public License as published by
9  * the Free Software Foundation; either version 2, or (at your option)
10  * any later version.
11  *
12  * paparazzi is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15  * GNU General Public License for more details.
16  *
17  * You should have received a copy of the GNU General Public License
18  * along with paparazzi; see the file COPYING. If not, write to
19  * the Free Software Foundation, 59 Temple Place - Suite 330,
20  * Boston, MA 02111-1307, USA.
21  *
22  */
23 
32 #include "subsystems/abi.h"
33 
34 // State interface for rotation compensation
35 #include "state.h"
36 
37 // Messages
38 #include "mcu_periph/uart.h"
39 #include "pprzlink/messages.h"
41 
45 
46 // message ID in Mavlink (v1.0)
47 #define MAVLINK_OPTICAL_HEARTBEAT_MSG_ID 0
48 #define MAVLINK_OPTICAL_FLOW_MSG_ID 100
49 #define MAVLINK_OPTICAL_FLOW_RAD_MSG_ID 106
50 
51 /*
52  * acceptable quality of optical flow (0-min,1-max)
53  * higher threshold means fewer measurements will be available
54  * but they will be more precise. Generally it is probably a better
55  * idea to have lower threshold, and propagate the noise (1-quality)
56  * to the INS
57  */
58 #ifndef PX4FLOW_QUALITY_THRESHOLD
59 #define PX4FLOW_QUALITY_THRESHOLD 0.1
60 #endif
61 
62 // request structs for mavlink decoder
66 
67 // enable/disable AGL updates
69 
70 // compensate AGL for body rotation
72 
73 // standard deviation of the measurement
75 
76 
77 // callback function on message reception
78 static void decode_optical_flow_msg(struct mavlink_message *msg __attribute__((unused)))
79 {
80  static float quality = 0;
81  static float noise = 0;
82  uint32_t now_ts = get_sys_time_usec();
83  quality = ((float)optical_flow.quality) / 255.0;
84  noise = px4flow_stddev + (1 - quality) * px4flow_stddev * 10;
85  noise = noise * noise; // square the noise to get variance of the measurement
86 
87  if (quality > PX4FLOW_QUALITY_THRESHOLD) {
88  // flip the axis (if the PX4FLOW is mounted as shown in
89  // https://pixhawk.org/modules/px4flow
90  AbiSendMsgVELOCITY_ESTIMATE(VEL_PX4FLOW_ID,
94  0.0f,
95  noise,
96  noise,
97  -1.f);
98  }
99 
100  // compensate AGL measurement for body rotation
102  float phi = stateGetNedToBodyEulers_f()->phi;
103  float theta = stateGetNedToBodyEulers_f()->theta;
104  float gain = (float)fabs( (double) (cosf(phi) * cosf(theta)));
106  }
107 
108  if (px4flow_update_agl) {
109  // positive distance means it's known/valid
110  if (optical_flow.ground_distance > 0) {
111  AbiSendMsgAGL(AGL_SONAR_PX4FLOW_ID, now_ts, optical_flow.ground_distance);
112  }
113  }
114 }
115 
122 static void decode_optical_flow_rad_msg(struct mavlink_message *msg __attribute__((unused)))
123 {
124  // TODO: do something useful with the data here
125 }
126 
127 static void decode_heartbeat_msg(struct mavlink_message *msg __attribute__((unused)))
128 {
129  //TODO: Check whether we have a right version of firmware etc? i.e. heartbeat.autopilot=MAV_AUTOPILOT_PX4
130 }
131 
135 void px4flow_init(void)
136 {
137  // register mavlink messages
142 
147 
152 
153  px4flow_update_agl = USE_PX4FLOW_AGL;
154  px4flow_compensate_rotation = PX4FLOW_COMPENSATE_ROTATION;
155  px4flow_stddev = PX4FLOW_NOISE_STDDEV;
156 }
157 
158 
163 {
164  static float timestamp = 0;
165  static uint8_t distance_quality = 0;
166  timestamp = ((float)optical_flow.time_usec) * 0.000001;
167  DOWNLINK_SEND_OPTICAL_FLOW(DefaultChannel, DefaultDevice,
168  &timestamp,
176  &distance_quality);
177 }
178 
decode_heartbeat_msg
static void decode_heartbeat_msg(struct mavlink_message *msg)
Definition: px4flow.c:127
abi.h
px4flow_init
void px4flow_init(void)
Initialization function.
Definition: px4flow.c:135
decode_optical_flow_msg
static void decode_optical_flow_msg(struct mavlink_message *msg)
Definition: px4flow.c:78
stateGetNedToBodyEulers_f
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
decode_optical_flow_rad_msg
static void decode_optical_flow_rad_msg(struct mavlink_message *msg)
According to https://pixhawk.org/modules/px4flow the PX4flow module outputs OPTICAL_FLOW_RAD message ...
Definition: px4flow.c:122
uint32_t
unsigned long uint32_t
Definition: types.h:18
mavlink_optical_flow::sensor_id
uint8_t sensor_id
Sensor ID.
Definition: px4flow.h:52
mavlink_optical_flow::ground_distance
float ground_distance
Ground distance in meters. Positive value: distance known. Negative value: Unknown distance.
Definition: px4flow.h:49
FloatEulers::theta
float theta
in radians
Definition: pprz_algebra_float.h:86
msg
uint8_t msg[10]
Buffer used for general comunication over SPI (out buffer)
Definition: high_speed_logger_direct_memory.c:134
get_sys_time_usec
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:68
MAVLINK_OPTICAL_FLOW_MSG_ID
#define MAVLINK_OPTICAL_FLOW_MSG_ID
Definition: px4flow.c:48
mavlink_optical_flow::flow_comp_m_x
float flow_comp_m_x
Flow in meters in x-sensor direction, angular-speed compensated [meters/sec].
Definition: px4flow.h:47
uart.h
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
FloatEulers::phi
float phi
in radians
Definition: pprz_algebra_float.h:85
req_heartbeat
struct mavlink_msg_req req_heartbeat
Definition: px4flow.c:65
optical_flow_rad
struct mavlink_optical_flow_rad optical_flow_rad
Definition: px4flow.c:44
PX4FLOW_QUALITY_THRESHOLD
#define PX4FLOW_QUALITY_THRESHOLD
Definition: px4flow.c:59
mavlink_optical_flow::quality
uint8_t quality
Optical flow quality / confidence. 0: bad, 255: maximum quality.
Definition: px4flow.h:53
px4flow_update_agl
bool px4flow_update_agl
Definition: px4flow.c:68
uint8_t
unsigned char uint8_t
Definition: types.h:14
px4flow.h
driver for the optical flow sensor PX4FLOW
mavlink_optical_flow::flow_comp_m_y
float flow_comp_m_y
Flow in meters in y-sensor direction, angular-speed compensated [meters/sec].
Definition: px4flow.h:48
heartbeat
struct mavlink_heartbeat heartbeat
Definition: px4flow.c:42
AGL_SONAR_PX4FLOW_ID
#define AGL_SONAR_PX4FLOW_ID
Definition: abi_sender_ids.h:141
mavlink_optical_flow::flow_x
int32_t flow_x
Flow in pixels in x-sensor direction.
Definition: px4flow.h:50
MAVLINK_OPTICAL_FLOW_RAD_MSG_ID
#define MAVLINK_OPTICAL_FLOW_RAD_MSG_ID
Definition: px4flow.c:49
px4flow_compensate_rotation
bool px4flow_compensate_rotation
Definition: px4flow.c:71
mavlink_optical_flow::time_usec
uint64_t time_usec
Timestamp (UNIX)
Definition: px4flow.h:46
req_flow_rad
struct mavlink_msg_req req_flow_rad
Definition: px4flow.c:64
px4flow_stddev
float px4flow_stddev
Definition: px4flow.c:74
state.h
mavlink_optical_flow::flow_y
int32_t flow_y
Flow in pixels in y-sensor direction.
Definition: px4flow.h:51
req_flow
struct mavlink_msg_req req_flow
Definition: px4flow.c:63
MAVLINK_OPTICAL_HEARTBEAT_MSG_ID
#define MAVLINK_OPTICAL_HEARTBEAT_MSG_ID
Definition: px4flow.c:47
optical_flow
struct mavlink_optical_flow optical_flow
Definition: px4flow.c:43
px4flow_downlink
void px4flow_downlink(void)
Downlink message for debug.
Definition: px4flow.c:162
VEL_PX4FLOW_ID
#define VEL_PX4FLOW_ID
Definition: abi_sender_ids.h:386