Paparazzi UAS  v5.15_devel-230-gc96ce27
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
dragspeed.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) Tom van Dijk
3  *
4  * This file is part of paparazzi
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, see
18  * <http://www.gnu.org/licenses/>.
19  */
40 
41 #include "subsystems/abi.h"
42 #include "subsystems/abi_common.h"
44 
45 #include <stdio.h>
46 
47 #ifndef DRAGSPEED_SEND_ABI_MESSAGE
48 #define DRAGSPEED_SEND_ABI_MESSAGE TRUE
49 #endif
50 
51 #ifndef DRAGSPEED_ACCEL_ID
52 #define DRAGSPEED_ACCEL_ID ABI_BROADCAST
53 #endif
54 
55 #ifndef DRAGSPEED_COEFF_X
56 #define DRAGSPEED_COEFF_X 1.0
57 #endif
58 
59 #ifndef DRAGSPEED_COEFF_Y
60 #define DRAGSPEED_COEFF_Y 1.0
61 #endif
62 
63 #ifndef DRAGSPEED_R
64 #define DRAGSPEED_R 0.25
65 #endif
66 
67 #ifndef DRAGSPEED_FILTER
68 #define DRAGSPEED_FILTER 0.8
69 #endif
70 
72 
74 static void accel_cb(uint8_t sender_id, uint32_t stamp,
75  struct Int32Vect3 *accel);
76 
77 static void send_dragspeed(struct transport_tx *trans, struct link_device *dev);
78 
79 static void calibrate_coeff(struct Int32Vect3 *accel);
80 static void calibrate_zero(struct Int32Vect3 *accel);
81 
82 void dragspeed_init(void)
83 {
84  // Set initial values
88 #ifdef DRAGSPEED_ZERO_X
89  dragspeed.zero.x = DRAGSPEED_ZERO_X;
90 #endif
91 #ifdef DRAGSPEED_ZERO_Y
92  dragspeed.zero.y = DRAGSPEED_ZERO_Y;
93 #endif
94 #if defined(DRAGSPEED_ZERO_X) && defined(DRAGSPEED_ZERO_Y)
96 #else
98 #endif
99  // Register callbacks
100  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_DRAGSPEED,
102  AbiBindMsgIMU_ACCEL_INT32(DRAGSPEED_ACCEL_ID, &accel_ev, accel_cb);
103 }
104 
106 {
108  return FALSE;
109 }
110 
112 {
114  return FALSE;
115 }
116 
118 {
120 }
121 
122 static void accel_cb(uint8_t sender_id __attribute__((unused)), uint32_t stamp,
123  struct Int32Vect3 *accel)
124 {
125  // Estimate current velocity
126  float vx = -(ACCEL_FLOAT_OF_BFP(accel->x) - dragspeed.zero.x)
127  / dragspeed.coeff.x;
128  float vy = -(ACCEL_FLOAT_OF_BFP(accel->y) - dragspeed.zero.y)
129  / dragspeed.coeff.y;
130  // Simple low-pass filter
131  dragspeed.vel.x += (1 - dragspeed.filter) * (vx - dragspeed.vel.x);
132  dragspeed.vel.y += (1 - dragspeed.filter) * (vy - dragspeed.vel.y);
133  // Send as ABI VELOCITY_ESTIMATE message
134 #if DRAGSPEED_SEND_ABI_MESSAGE
136  AbiSendMsgVELOCITY_ESTIMATE(VEL_DRAGSPEED_ID, stamp, dragspeed.vel.x,
137  dragspeed.vel.y, 0.f, DRAGSPEED_R, DRAGSPEED_R, -1.f);
138  }
139 #endif
140  // Perform calibration if required
141  calibrate_coeff(accel);
142  calibrate_zero(accel);
143 }
144 
154 static void calibrate_coeff(struct Int32Vect3 *accel)
155 {
156  // Reset when new calibration is started
157  static bool calibrate_prev = FALSE;
158  static struct FloatVect2 coeff;
159  static int num_samples_x = 0;
160  static int num_samples_y = 0;
161  if (dragspeed.calibrate_coeff && !calibrate_prev) {
162  coeff.x = 0.0f;
163  coeff.y = 0.0f;
164  num_samples_x = 0;
165  num_samples_y = 0;
166  }
167  calibrate_prev = dragspeed.calibrate_coeff;
168  // Return when calibration is not active
169  if (!dragspeed.calibrate_coeff) {
170  return;
171  }
172  // Also return if zero calibration has not been performed yet
173  if (!dragspeed.zero_calibrated) {
174 #ifdef __linux__
175  fprintf(stderr,
176  "[dragspeed] Error: zero measurement should be calibrated before drag coefficient!\n");
177 #endif
179  return;
180  }
181 
182  // Calculate INS velocity in body frame
183  struct FloatEulers *att = stateGetNedToBodyEulers_f();
184  struct EnuCoor_f *vel_ins = stateGetSpeedEnu_f();
185  struct FloatVect2 vel_ins_body = { cosf(att->psi) * vel_ins->y
186  + sinf(att->psi) * vel_ins->x, -sinf(att->psi) * vel_ins->y
187  + cosf(att->psi) * vel_ins->x };
188  // Calibrate coefficient when velocity is sufficiently high
189  if (fabsf(vel_ins_body.x) > 0.5) {
190  float this_coeff = -(ACCEL_FLOAT_OF_BFP(accel->x) - dragspeed.zero.x)
191  / vel_ins_body.x;
192  coeff.x = (coeff.x * num_samples_x + this_coeff) / (num_samples_x + 1);
193  num_samples_x++;
194  }
195  if (fabsf(vel_ins_body.y) > 0.5) {
196  float this_coeff = -(ACCEL_FLOAT_OF_BFP(accel->y) - dragspeed.zero.y)
197  / vel_ins_body.y;
198  coeff.y = (coeff.y * num_samples_y + this_coeff) / (num_samples_y + 1);
199  num_samples_y++;
200  }
201  // End calibration when enough samples are averaged
202  if (num_samples_x > 1000 && num_samples_y > 1000 && coeff.x != 0
203  && coeff.y != 0) {
204  dragspeed.coeff = coeff;
206  }
207 }
208 
216 static void calibrate_zero(struct Int32Vect3 *accel)
217 {
218  // Reset when new calibration is started
219  static bool calibrate_prev = FALSE;
220  static struct FloatVect2 zero;
221  static int num_samples = 0;
222  if (dragspeed.calibrate_zero && !calibrate_prev) {
223  zero.x = 0.0f;
224  zero.y = 0.0f;
225  num_samples = 0;
226  }
227  calibrate_prev = dragspeed.calibrate_zero;
228  // Return when calibration is not active
229  if (!dragspeed.calibrate_zero) {
230  return;
231  }
232 
233  // Average accelerometer readings when velocity is sufficiently low
234  struct EnuCoor_f *vel_ins = stateGetSpeedEnu_f();
235  float ins_speed = sqrtf(vel_ins->x * vel_ins->x + vel_ins->y * vel_ins->y);
236  if (ins_speed < 0.1) {
237  zero.x = (zero.x * num_samples + ACCEL_FLOAT_OF_BFP(accel->x))
238  / (num_samples + 1);
239  zero.y = (zero.y * num_samples + ACCEL_FLOAT_OF_BFP(accel->y))
240  / (num_samples + 1);
241  num_samples++;
242  // End calibration when enough samples are averaged
243  if (num_samples > 1000) {
244  dragspeed.zero = zero;
247  }
248  }
249 }
250 
251 static void send_dragspeed(struct transport_tx *trans, struct link_device *dev)
252 {
253  // Calculate INS velocity in body frame
254  struct FloatEulers *att = stateGetNedToBodyEulers_f();
255  struct EnuCoor_f *vel_ins = stateGetSpeedEnu_f();
256  struct FloatVect2 vel_ins_body = { cosf(att->psi) * vel_ins->y
257  + sinf(att->psi) * vel_ins->x, -sinf(att->psi) * vel_ins->y
258  + cosf(att->psi) * vel_ins->x };
259  // Send telemetry message
260  pprz_msg_send_DRAGSPEED(trans, dev, AC_ID, &dragspeed.vel.x, &dragspeed.vel.y,
261  &vel_ins_body.x, &vel_ins_body.y);
262 }
263 
Event structure to store callbacks in a linked list.
Definition: abi_common.h:65
static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel)
Definition: dragspeed.c:122
struct FloatVect2 zero
Definition: dragspeed.h:41
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
Periodic telemetry system header (includes downlink utility and generated code).
Common tools for ABI middelware.
vector in East North Up coordinates Units: meters
#define VEL_DRAGSPEED_ID
float filter
Definition: dragspeed.h:36
Main include for ABI (AirBorneInterface).
static void calibrate_zero(struct Int32Vect3 *accel)
Calibrate zero velocity by measuring the accelerations while the drone hovers in-place.
Definition: dragspeed.c:216
bool dragspeed_is_calibrating(void)
Definition: dragspeed.c:117
float psi
in radians
bool zero_calibrated
Definition: dragspeed.h:43
#define FALSE
Definition: std.h:5
euler angles
#define TRUE
Definition: std.h:4
bool dragspeed_calibrate_zero(void)
Definition: dragspeed.c:111
float x
in meters
#define DRAGSPEED_COEFF_Y
Definition: dragspeed.c:60
struct FloatVect2 vel
Definition: dragspeed.h:34
#define ACCEL_FLOAT_OF_BFP(_ai)
unsigned long uint32_t
Definition: types.h:18
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
bool calibrate_coeff
Definition: dragspeed.h:39
static void calibrate_coeff(struct Int32Vect3 *accel)
Calibrate drag coefficient by comparing accelerometer measurements to INS velocities.
Definition: dragspeed.c:154
bool dragspeed_calibrate_coeff(void)
Definition: dragspeed.c:105
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:73
#define DRAGSPEED_FILTER
Definition: dragspeed.c:68
#define DRAGSPEED_COEFF_X
Definition: dragspeed.c:56
#define DRAGSPEED_R
Definition: dragspeed.c:64
struct dragspeed_t dragspeed
Definition: dragspeed.c:71
unsigned char uint8_t
Definition: types.h:14
static struct EnuCoor_f * stateGetSpeedEnu_f(void)
Get ground speed in local ENU coordinates (float).
Definition: state.h:917
static abi_event accel_ev
Definition: dragspeed.c:73
struct FloatVect2 coeff
Definition: dragspeed.h:38
#define DRAGSPEED_ACCEL_ID
Definition: dragspeed.c:52
void dragspeed_init(void)
Definition: dragspeed.c:82
float y
in meters
bool calibrate_zero
Definition: dragspeed.h:42
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46
static void send_dragspeed(struct transport_tx *trans, struct link_device *dev)
Definition: dragspeed.c:251