Paparazzi UAS  v5.14.0_stable-0-g3f680d1
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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  quality = ((float)optical_flow.quality) / 255.0;
83  noise = px4flow_stddev + (1 - quality) * px4flow_stddev * 10;
84  noise = noise * noise; // square the noise to get variance of the measurement
85 
86  if (quality > PX4FLOW_QUALITY_THRESHOLD) {
87  // flip the axis (if the PX4FLOW is mounted as shown in
88  // https://pixhawk.org/modules/px4flow
89  AbiSendMsgVELOCITY_ESTIMATE(VEL_PX4FLOW_ID,
93  0.0f,
94  noise,
95  noise,
96  -1.f);
97  }
98 
99  // compensate AGL measurement for body rotation
101  float phi = stateGetNedToBodyEulers_f()->phi;
102  float theta = stateGetNedToBodyEulers_f()->theta;
103  float gain = (float)fabs( (double) (cosf(phi) * cosf(theta)));
105  }
106 
107  if (px4flow_update_agl) {
108  // positive distance means it's known/valid
109  if (optical_flow.ground_distance > 0) {
111  }
112  }
113 }
114 
121 static void decode_optical_flow_rad_msg(struct mavlink_message *msg __attribute__((unused)))
122 {
123  // TODO: do something useful with the data here
124 }
125 
126 static void decode_heartbeat_msg(struct mavlink_message *msg __attribute__((unused)))
127 {
128  //TODO: Check whether we have a right version of firmware etc? i.e. heartbeat.autopilot=MAV_AUTOPILOT_PX4
129 }
130 
134 void px4flow_init(void)
135 {
136  // register mavlink messages
141 
146 
151 
152  px4flow_update_agl = USE_PX4FLOW_AGL;
153  px4flow_compensate_rotation = PX4FLOW_COMPENSATE_ROTATION;
154  px4flow_stddev = PX4FLOW_NOISE_STDDEV;
155 }
156 
157 
162 {
163  static float timestamp = 0;
164  timestamp = ((float)optical_flow.time_usec) * 0.000001;
165  DOWNLINK_SEND_PX4FLOW(DefaultChannel, DefaultDevice,
166  &timestamp,
174 }
175 
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
float phi
in radians
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
struct mavlink_optical_flow optical_flow
Definition: px4flow.c:43
static void decode_optical_flow_msg(struct mavlink_message *msg)
Definition: px4flow.c:78
void px4flow_downlink(void)
Downlink message for debug.
Definition: px4flow.c:161
#define AGL_SONAR_PX4FLOW_ID
static void decode_heartbeat_msg(struct mavlink_message *msg)
Definition: px4flow.c:126
driver for the optical flow sensor PX4FLOW
Main include for ABI (AirBorneInterface).
int16_t flow_x
Flow in pixels in x-sensor direction.
Definition: px4flow.h:50
#define MAVLINK_OPTICAL_FLOW_MSG_ID
Definition: px4flow.c:48
struct mavlink_optical_flow_rad optical_flow_rad
Definition: px4flow.c:44
float flow_comp_m_x
Flow in meters in x-sensor direction, angular-speed compensated [meters/sec].
Definition: px4flow.h:47
float ground_distance
Ground distance in meters. Positive value: distance known. Negative value: Unknown distance...
Definition: px4flow.h:49
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:121
float theta
in radians
#define PX4FLOW_QUALITY_THRESHOLD
Definition: px4flow.c:59
uint8_t sensor_id
Sensor ID.
Definition: px4flow.h:52
struct mavlink_msg_req req_heartbeat
Definition: px4flow.c:65
uint8_t quality
Optical flow quality / confidence. 0: bad, 255: maximum quality.
Definition: px4flow.h:53
bool px4flow_update_agl
Definition: px4flow.c:68
bool px4flow_compensate_rotation
Definition: px4flow.c:71
#define MAVLINK_OPTICAL_FLOW_RAD_MSG_ID
Definition: px4flow.c:49
float flow_comp_m_y
Flow in meters in y-sensor direction, angular-speed compensated [meters/sec].
Definition: px4flow.h:48
#define VEL_PX4FLOW_ID
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.
struct mavlink_msg_req req_flow
Definition: px4flow.c:63
float px4flow_stddev
Definition: px4flow.c:74
uint64_t time_usec
Timestamp (UNIX)
Definition: px4flow.h:46
struct mavlink_heartbeat heartbeat
Definition: px4flow.c:42
uint8_t msg[10]
Buffer used for general comunication over SPI (out buffer)
struct mavlink_msg_req req_flow_rad
Definition: px4flow.c:64
int16_t flow_y
Flow in pixels in y-sensor direction.
Definition: px4flow.h:51
#define MAVLINK_OPTICAL_HEARTBEAT_MSG_ID
Definition: px4flow.c:47
void px4flow_init(void)
Initialization function.
Definition: px4flow.c:134