Paparazzi UAS  v5.15_devel-81-gd13dafb
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
jevois_mavlink.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) MAVLab
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  */
27 
28 
29 #define DEBUG_PRINT printf
30 #include <stdio.h>
31 
32 
33 /*
34  * MavLink protocol
35  */
36 
37 #include <mavlink/mavlink_types.h>
38 #include "mavlink/paparazzi/mavlink.h"
39 
40 #include "subsystems/imu.h"
41 #include "autopilot.h"
42 #include "generated/modules.h"
43 
44 #include "subsystems/abi.h"
46 
47 #include "std.h"
49 
50 
51 mavlink_system_t mavlink_system;
52 
53 #ifndef MAVLINK_SYSID
54 #define MAVLINK_SYSID 1
55 #endif
56 
57 
58 
59 static void mavlink_send_heartbeat(void);
60 static void mavlink_send_attitude(void);
61 static void mavlink_send_highres_imu(void);
62 static void mavlink_send_set_mode(void);
63 
64 
65 /*
66  * Exported data
67  */
68 
69 
71  int received;
72  int count;
73  int x;
74  int y;
75  int w;
76  int h;
77  int quality;
78  int source;
79 } jevois_visual_target = {false, 0, 0, 0, 0, 0, 0};
80 
82  int received;
83  int cnt;
84  float x;
85  float y;
86  float z;
87 } jevois_vision_position = {false, 0, 0.0f, 0.0f, 0.0f};
88 
89 /*
90  * Paparazzi Module functions : filter functions
91  */
92 
94 
98 
100 {
104 }
105 
106 void jevois_mavlink_filter_init(void);
108 {
109  init_butterworth_2_low_pass_int(&ax_filtered, 20.0, 1.0 / ((float)MODULES_FREQUENCY), imu.accel_unscaled.x);
110  init_butterworth_2_low_pass_int(&ay_filtered, 20.0, 1.0 / ((float)MODULES_FREQUENCY), imu.accel_unscaled.y);
111  init_butterworth_2_low_pass_int(&az_filtered, 20.0, 1.0 / ((float)MODULES_FREQUENCY), imu.accel_unscaled.z);
112 }
113 
114 /*
115  * Paparazzi Module functions : forward to telemetry
116  */
117 
118 // Send Manual Setpoint over telemetry using ROTORCRAFT_RADIO_CONTROL message
119 
120 static void send_jevois_mavlink_visual_target(struct transport_tx *trans, struct link_device *dev)
121 {
130  pprz_msg_send_VISUALTARGET(trans, dev, AC_ID, &cnt,
131  &tx, &ty, &w, &h, &s);
132  }
133 }
134 
135 static void send_jevois_mavlink_visual_position(struct transport_tx *trans, struct link_device *dev)
136 {
140  float foo = 0;
141  pprz_msg_send_VISION_POSITION_ESTIMATE(trans, dev, AC_ID,
142  &cnt,
146  &foo, &foo );
147  }
148 }
149 
150 /*
151  * Paparazzi Module functions : communications
152  */
153 
155 {
156  // Initialize Mavlink parser
157  mavlink_system.sysid = MAVLINK_SYSID; // System ID, 1-255
158  mavlink_system.compid = 0; // Component/Subsystem ID, 1-255
159 
160  // Initialize Anti-Aliasing Sensor Filter
162 
163  // Send telemetry
166 }
167 
169 {
170  RunOnceEvery(100, mavlink_send_heartbeat());
171  RunOnceEvery(2, mavlink_send_attitude());
172  RunOnceEvery(1, mavlink_send_highres_imu());
173  RunOnceEvery(1, mavlink_send_set_mode());
174 }
175 
176 #ifndef JEVOIS_MAVLINK_ABI_ID
177 #define JEVOIS_MAVLINK_ABI_ID 34
178 #endif
179 
180 
181 
183 {
184  mavlink_message_t msg;
185  mavlink_status_t status;
186 
187 
188  while (MAVLinkChAvailable()) {
189  uint8_t c = MAVLinkGetch();
190  if (mavlink_parse_char(MAVLINK_COMM_1, c, &msg, &status)) {
191  //printf("msg.msgid is %d\n",msg.msgid);
192  switch (msg.msgid) {
193  case MAVLINK_MSG_ID_HEARTBEAT: {
194  mavlink_heartbeat_t heartbeat;
195  mavlink_msg_heartbeat_decode(&msg, &heartbeat);
196  // do something with heartbeat variable
197  DEBUG_PRINT("[jevois mavlink] heartbeat\n");
198  }
199  break;
200 
201  case MAVLINK_MSG_ID_DEBUG: {
202  mavlink_debug_t jevois_mavlink_debug;
203  mavlink_msg_debug_decode(&msg, &jevois_mavlink_debug);
204 
205  DEBUG_PRINT("[jevois mavlink] debug value is %f\n", jevois_mavlink_debug.value);
206  DEBUG_PRINT("[jevois mavlink] debug ind is %d\n", jevois_mavlink_debug.ind);
207  }
208  break;
209  case MAVLINK_MSG_ID_HIGHRES_IMU: {
210  mavlink_highres_imu_t jevois_mavlink_visual_target;
211  mavlink_msg_highres_imu_decode(&msg, &jevois_mavlink_visual_target);
212 
215  jevois_visual_target.x = jevois_mavlink_visual_target.xacc;
216  jevois_visual_target.y = jevois_mavlink_visual_target.yacc;
217  jevois_visual_target.w = jevois_mavlink_visual_target.xmag;
218  jevois_visual_target.h = jevois_mavlink_visual_target.ymag;
219  jevois_visual_target.quality = jevois_mavlink_visual_target.abs_pressure;
220  jevois_visual_target.source = jevois_mavlink_visual_target.diff_pressure;
221 
222 
223  AbiSendMsgVISUAL_DETECTION(JEVOIS_MAVLINK_ABI_ID,
224  jevois_visual_target.x, // pixel-x
225  jevois_visual_target.y, // pixel-y
226  jevois_visual_target.w, // size
227  jevois_visual_target.h, // size
228  jevois_visual_target.quality, // quality
230 
231  DEBUG_PRINT("[jevois mavlink] VISUAL_DETECTION %f,%f\n", jevois_mavlink_visual_target.xacc,
232  jevois_mavlink_visual_target.yacc);
233 
234  }
235  break;
236 
237  case MAVLINK_MSG_ID_MANUAL_SETPOINT: {
238  mavlink_manual_setpoint_t jevois_mavlink_manual_setpoint;
239  mavlink_msg_manual_setpoint_decode(&msg, &jevois_mavlink_manual_setpoint);
240 
241  AbiSendMsgMANUAL_SETPOINT(JEVOIS_MAVLINK_ABI_ID, jevois_mavlink_manual_setpoint.thrust,
242  jevois_mavlink_manual_setpoint.roll,
243  jevois_mavlink_manual_setpoint.pitch,
244  jevois_mavlink_manual_setpoint.yaw);
245 
246  DEBUG_PRINT("[jevois mavlink] phi_cmd = %f\n", DegOfRad(jevois_mavlink_manual_setpoint.roll));
247  DEBUG_PRINT("[jevois mavlink] theta_cmd = %f\n", DegOfRad(jevois_mavlink_manual_setpoint.pitch));
248  DEBUG_PRINT("[jevois mavlink] psi_cmd = %f\n", DegOfRad(jevois_mavlink_manual_setpoint.yaw));
249  DEBUG_PRINT("[jevois mavlink] alt_cmd = %f\n", jevois_mavlink_manual_setpoint.thrust);
250 
251  }
252  break;
253 
254 
255  case MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE: {
256  mavlink_vision_position_estimate_t jevois_mavlink_vision_position_estimate;
257  mavlink_msg_vision_position_estimate_decode(&msg, &jevois_mavlink_vision_position_estimate);
258 
261  jevois_vision_position.x = jevois_mavlink_vision_position_estimate.x;
262  jevois_vision_position.y = jevois_mavlink_vision_position_estimate.y;
263  jevois_vision_position.z = jevois_mavlink_vision_position_estimate.z;
264 
265  AbiSendMsgRELATIVE_LOCALIZATION(JEVOIS_MAVLINK_ABI_ID,
270  0.0f,
271  0.0f,
272  0.0f);
273 
274  DEBUG_PRINT("[jevois mavlink] VISION_POSITION_ESTIMATE %f,%f,%f \n", jevois_vision_position.x,
276 
277  }
278  break;
279 
280  }
281  }
282  }
283 }
284 
285 
287 
288 
289 #include "state.h"
290 #include "mcu_periph/sys_time.h"
291 
292 
293 
294 static void mavlink_send_attitude(void)
295 {
296  mavlink_msg_attitude_send(MAVLINK_COMM_0,
298  stateGetNedToBodyEulers_f()->phi, // Phi
299  stateGetNedToBodyEulers_f()->theta, // Theta
300  stateGetNedToBodyEulers_f()->psi, // Psi
301  stateGetBodyRates_f()->p, // p
302  stateGetBodyRates_f()->q, // q
303  stateGetBodyRates_f()->r); // r
305 }
306 
307 
308 static void mavlink_send_set_mode(void)
309 {
310  mavlink_msg_set_mode_send(MAVLINK_COMM_0,
312  0, //autopilotMode.currentMode,
313  0
314  );
316 }
317 
318 static void mavlink_send_heartbeat(void)
319 {
320  mavlink_msg_heartbeat_send(MAVLINK_COMM_0,
321  MAV_TYPE_FIXED_WING,
322  MAV_AUTOPILOT_PPZ,
323  MAV_MODE_FLAG_AUTO_ENABLED,
324  0, // custom_mode
325  MAV_STATE_CALIBRATING);
327 }
328 
329 
330 static void mavlink_send_highres_imu(void)
331 {
332  mavlink_msg_highres_imu_send(MAVLINK_COMM_0,
334  get_butterworth_2_low_pass_int(&ax_filtered) / 1024.0,
335  get_butterworth_2_low_pass_int(&ay_filtered) / 1024.0,
336  get_butterworth_2_low_pass_int(&az_filtered) / 1024.0,
338  stateGetBodyRates_f()->q,
339  stateGetBodyRates_f()->r,
341  stateGetNedToBodyEulers_f()->theta,
343  0,
344  0,
345  0,
346  0,
347  0);
349 }
350 
unsigned short uint16_t
Definition: types.h:16
static uint32_t s
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).
uint32_t get_sys_time_msec(void)
Get the time in milliseconds since startup.
Definition: sys_time_arch.c:78
Simple first order low pass filter with bilinear transform.
Main include for ABI (AirBorneInterface).
static void init_butterworth_2_low_pass_int(Butterworth2LowPass_int *filter, float cut_off, float sample_time, int32_t value)
Init a second order Butterworth filter.
struct Imu imu
global IMU state
Definition: imu.c:108
static int32_t get_butterworth_2_low_pass_int(Butterworth2LowPass_int *filter)
Get current value of the second order Butterworth low pass filter(fixed point version).
struct Int32Vect3 accel
accelerometer measurements in m/s^2 in BFP with INT32_ACCEL_FRAC
Definition: imu.h:39
Architecture independent timing functions.
uint8_t status
signed short int16_t
Definition: types.h:17
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
uint16_t foo
Definition: main_demo5.c:59
static int32_t update_butterworth_2_low_pass_int(Butterworth2LowPass_int *filter, int32_t value)
Update second order Butterworth low pass filter state with a new value(fixed point version)...
Inertial Measurement Unit interface.
static struct FloatRates * stateGetBodyRates_f(void)
Get vehicle body angular rate (float).
Definition: state.h:1200
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:73
Core autopilot interface common to all firmwares.
static void h(const real32_T x[7], const real32_T q[4], real32_T y[6])
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.
Convenience defines for ABI sender IDs.
struct Int32Vect3 accel_unscaled
unscaled accelerometer measurements
Definition: imu.h:47
static float p[2][2]
struct mavlink_heartbeat heartbeat
Definition: px4flow.c:42
uint8_t msg[10]
Buffer used for general comunication over SPI (out buffer)
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46