Paparazzi UAS  v4.0.4_stable-3-gf39211a
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
telemetry.h
Go to the documentation of this file.
1 /*
2  * $Id$
3  *
4  * Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
5  *
6  * This file is part of paparazzi.
7  *
8  * paparazzi is free software; you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation; either version 2, or (at your option)
11  * any later version.
12  *
13  * paparazzi is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  * GNU General Public License for more details.
17  *
18  * You should have received a copy of the GNU General Public License
19  * along with paparazzi; see the file COPYING. If not, write to
20  * the Free Software Foundation, 59 Temple Place - Suite 330,
21  * Boston, MA 02111-1307, USA.
22  */
23 
24 #ifndef TELEMETRY_H
25 #define TELEMETRY_H
26 
27 #include "std.h"
28 #include "messages.h"
29 #include "mcu_periph/uart.h"
30 
32 #include "generated/periodic_telemetry.h"
33 
34 #ifdef RADIO_CONTROL
36 #endif
37 
40 
42 
43 #include "mcu_periph/sys_time.h"
44 #include "subsystems/electrical.h"
45 #include "subsystems/imu.h"
46 #if USE_GPS
47 #include "subsystems/gps.h"
48 #endif
49 #include "subsystems/ins.h"
50 #include "subsystems/ahrs.h"
51 // I2C Error counters
52 #include "mcu_periph/i2c.h"
53 
54 #define PERIODIC_SEND_ALIVE(_trans, _dev) DOWNLINK_SEND_ALIVE(_trans, _dev, 16, MD5SUM)
55 
56 #if USE_GPS
57 #define PERIODIC_SEND_ROTORCRAFT_STATUS(_trans, _dev) { \
58  uint32_t imu_nb_err = 0; \
59  uint8_t _twi_blmc_nb_err = 0; \
60  uint16_t time_sec = sys_time.nb_sec; \
61  DOWNLINK_SEND_ROTORCRAFT_STATUS(_trans, _dev, \
62  &imu_nb_err, \
63  &_twi_blmc_nb_err, \
64  &radio_control.status, \
65  &radio_control.frame_rate, \
66  &gps.fix, \
67  &autopilot_mode, \
68  &autopilot_in_flight, \
69  &autopilot_motors_on, \
70  &guidance_h_mode, \
71  &guidance_v_mode, \
72  &electrical.vsupply, \
73  &time_sec \
74  ); \
75  }
76 #else /* !USE_GPS */
77 #define PERIODIC_SEND_ROTORCRAFT_STATUS(_trans, _dev) { \
78  uint32_t imu_nb_err = 0; \
79  uint8_t twi_blmc_nb_err = 0; \
80  uint8_t fix = GPS_FIX_NONE; \
81  uint16_t time_sec = sys_time.nb_sec; \
82  DOWNLINK_SEND_ROTORCRAFT_STATUS(_trans, _dev, \
83  &imu_nb_err, \
84  &twi_blmc_nb_err, \
85  &radio_control.status, \
86  &radio_control.frame_rate, \
87  &fix, \
88  &autopilot_mode, \
89  &autopilot_in_flight, \
90  &autopilot_motors_on, \
91  &guidance_h_mode, \
92  &guidance_v_mode, \
93  &electrical.vsupply, \
94  &time_sec \
95  ); \
96  }
97 #endif /* USE_GPS */
98 
99 #ifdef RADIO_CONTROL
100 #define PERIODIC_SEND_RC(_trans, _dev) DOWNLINK_SEND_RC(_trans, _dev, RADIO_CONTROL_NB_CHANNEL, radio_control.values)
101 #if defined RADIO_KILL_SWITCH
102 #define PERIODIC_SEND_ROTORCRAFT_RADIO_CONTROL(_trans, _dev) SEND_ROTORCRAFT_RADIO_CONTROL( _trans, _dev, &radio_control.values[RADIO_KILL_SWITCH])
103 #else /* ! RADIO_KILL_SWITCH */
104 #define PERIODIC_SEND_ROTORCRAFT_RADIO_CONTROL(_trans, _dev) { \
105  int16_t foo = -42; \
106  SEND_ROTORCRAFT_RADIO_CONTROL( _trans, _dev, &foo) \
107 }
108 #endif /* !RADIO_KILL_SWITCH */
109 #define SEND_ROTORCRAFT_RADIO_CONTROL(_trans, _dev, _kill_switch) { \
110  DOWNLINK_SEND_ROTORCRAFT_RADIO_CONTROL(_trans, _dev, \
111  &radio_control.values[RADIO_ROLL], \
112  &radio_control.values[RADIO_PITCH], \
113  &radio_control.values[RADIO_YAW], \
114  &radio_control.values[RADIO_THROTTLE], \
115  &radio_control.values[RADIO_MODE], \
116  _kill_switch, \
117  &radio_control.status);}
118 #else /* ! RADIO_CONTROL */
119 #define PERIODIC_SEND_RC(_trans, _dev) {}
120 #define PERIODIC_SEND_ROTORCRAFT_RADIO_CONTROL(_trans, _dev) {}
121 #endif
122 
123 #ifdef RADIO_CONTROL_TYPE_PPM
124 #define PERIODIC_SEND_PPM(_trans, _dev) { \
125  uint16_t ppm_pulses_usec[RADIO_CONTROL_NB_CHANNEL]; \
126  for (int i=0;i<RADIO_CONTROL_NB_CHANNEL;i++) \
127  ppm_pulses_usec[i] = USEC_OF_RC_PPM_TICKS(ppm_pulses[i]); \
128  DOWNLINK_SEND_PPM(_trans, _dev, \
129  &radio_control.frame_rate, \
130  PPM_NB_CHANNEL, \
131  ppm_pulses_usec); \
132  }
133 #else
134 #define PERIODIC_SEND_PPM(_trans, _dev) {}
135 #endif
136 
137 #define PERIODIC_SEND_IMU_GYRO_SCALED(_trans, _dev) { \
138  DOWNLINK_SEND_IMU_GYRO_SCALED(_trans, _dev, \
139  &imu.gyro.p, \
140  &imu.gyro.q, \
141  &imu.gyro.r); \
142  }
143 
144 #define PERIODIC_SEND_IMU_ACCEL_SCALED(_trans, _dev) { \
145  DOWNLINK_SEND_IMU_ACCEL_SCALED(_trans, _dev, \
146  &imu.accel.x, \
147  &imu.accel.y, \
148  &imu.accel.z); \
149  }
150 
151 #define PERIODIC_SEND_IMU_MAG_SCALED(_trans, _dev) { \
152  DOWNLINK_SEND_IMU_MAG_SCALED(_trans, _dev, \
153  &imu.mag.x, \
154  &imu.mag.y, \
155  &imu.mag.z); \
156  }
157 
158 #define PERIODIC_SEND_IMU_GYRO_RAW(_trans, _dev) { \
159  DOWNLINK_SEND_IMU_GYRO_RAW(_trans, _dev, \
160  &imu.gyro_unscaled.p, \
161  &imu.gyro_unscaled.q, \
162  &imu.gyro_unscaled.r); \
163  }
164 
165 #define PERIODIC_SEND_IMU_ACCEL_RAW(_trans, _dev) { \
166  DOWNLINK_SEND_IMU_ACCEL_RAW(_trans, _dev, \
167  &imu.accel_unscaled.x, \
168  &imu.accel_unscaled.y, \
169  &imu.accel_unscaled.z); \
170  }
171 
172 #define PERIODIC_SEND_IMU_MAG_RAW(_trans, _dev) { \
173  DOWNLINK_SEND_IMU_MAG_RAW(_trans, _dev, \
174  &imu.mag_unscaled.x, \
175  &imu.mag_unscaled.y, \
176  &imu.mag_unscaled.z); \
177  }
178 
179 
180 #include "subsystems/sensors/baro.h"
181 #define PERIODIC_SEND_BARO_RAW(_trans, _dev) { \
182  DOWNLINK_SEND_BARO_RAW(_trans, _dev, \
183  &baro.absolute, \
184  &baro.differential); \
185  }
186 
187 
188 
190 #define PERIODIC_SEND_RATE_LOOP(_trans, _dev) { \
191  DOWNLINK_SEND_RATE_LOOP(_trans, _dev, \
192  &stabilization_rate_sp.p, \
193  &stabilization_rate_sp.q, \
194  &stabilization_rate_sp.r, \
195  &stabilization_rate_ref.p, \
196  &stabilization_rate_ref.q, \
197  &stabilization_rate_ref.r, \
198  &stabilization_rate_refdot.p, \
199  &stabilization_rate_refdot.q, \
200  &stabilization_rate_refdot.r, \
201  &stabilization_rate_sum_err.p, \
202  &stabilization_rate_sum_err.q, \
203  &stabilization_rate_sum_err.r, \
204  &stabilization_rate_ff_cmd.p, \
205  &stabilization_rate_ff_cmd.q, \
206  &stabilization_rate_ff_cmd.r, \
207  &stabilization_rate_fb_cmd.p, \
208  &stabilization_rate_fb_cmd.q, \
209  &stabilization_rate_fb_cmd.r, \
210  &stabilization_cmd[COMMAND_THRUST]); \
211  }
212 
213 #ifdef STABILISATION_ATTITUDE_TYPE_INT
214 #define PERIODIC_SEND_STAB_ATTITUDE(_trans, _dev) { \
215  DOWNLINK_SEND_STAB_ATTITUDE_INT(_trans, _dev, \
216  &ahrs.body_rate.p, \
217  &ahrs.body_rate.q, \
218  &ahrs.body_rate.r, \
219  &ahrs.ltp_to_body_euler.phi, \
220  &ahrs.ltp_to_body_euler.theta, \
221  &ahrs.ltp_to_body_euler.psi, \
222  &stab_att_sp_euler.phi, \
223  &stab_att_sp_euler.theta, \
224  &stab_att_sp_euler.psi, \
225  &stabilization_att_sum_err.phi, \
226  &stabilization_att_sum_err.theta, \
227  &stabilization_att_sum_err.psi, \
228  &stabilization_att_fb_cmd[COMMAND_ROLL], \
229  &stabilization_att_fb_cmd[COMMAND_PITCH], \
230  &stabilization_att_fb_cmd[COMMAND_YAW], \
231  &stabilization_att_ff_cmd[COMMAND_ROLL], \
232  &stabilization_att_ff_cmd[COMMAND_PITCH], \
233  &stabilization_att_ff_cmd[COMMAND_YAW], \
234  &stabilization_cmd[COMMAND_ROLL], \
235  &stabilization_cmd[COMMAND_PITCH], \
236  &stabilization_cmd[COMMAND_YAW]); \
237  }
238 
239 
240 #define PERIODIC_SEND_STAB_ATTITUDE_REF(_trans, _dev) { \
241  DOWNLINK_SEND_STAB_ATTITUDE_REF_INT(_trans, _dev, \
242  &stab_att_sp_euler.phi, \
243  &stab_att_sp_euler.theta, \
244  &stab_att_sp_euler.psi, \
245  &stab_att_ref_euler.phi, \
246  &stab_att_ref_euler.theta, \
247  &stab_att_ref_euler.psi, \
248  &stab_att_ref_rate.p, \
249  &stab_att_ref_rate.q, \
250  &stab_att_ref_rate.r, \
251  &stab_att_ref_accel.p, \
252  &stab_att_ref_accel.q, \
253  &stab_att_ref_accel.r); \
254  }
255 #endif /* STABILISATION_ATTITUDE_TYPE_INT */
256 
257 #ifdef STABILISATION_ATTITUDE_TYPE_FLOAT
258 #define PERIODIC_SEND_STAB_ATTITUDE(_trans, _dev) { \
259  DOWNLINK_SEND_STAB_ATTITUDE_FLOAT(_trans, _dev, \
260  &ahrs_float.body_rate.p, \
261  &ahrs_float.body_rate.q, \
262  &ahrs_float.body_rate.r, \
263  &ahrs_float.ltp_to_body_euler.phi, \
264  &ahrs_float.ltp_to_body_euler.theta, \
265  &ahrs_float.ltp_to_body_euler.psi, \
266  &stab_att_ref_euler.phi, \
267  &stab_att_ref_euler.theta, \
268  &stab_att_ref_euler.psi, \
269  &stabilization_att_sum_err_eulers.phi, \
270  &stabilization_att_sum_err_eulers.theta, \
271  &stabilization_att_sum_err_eulers.psi, \
272  &stabilization_att_fb_cmd[COMMAND_ROLL], \
273  &stabilization_att_fb_cmd[COMMAND_PITCH], \
274  &stabilization_att_fb_cmd[COMMAND_YAW], \
275  &stabilization_att_ff_cmd[COMMAND_ROLL], \
276  &stabilization_att_ff_cmd[COMMAND_PITCH], \
277  &stabilization_att_ff_cmd[COMMAND_YAW], \
278  &stabilization_cmd[COMMAND_ROLL], \
279  &stabilization_cmd[COMMAND_PITCH], \
280  &stabilization_cmd[COMMAND_YAW], \
281  &ahrs_float.body_rate_d.p, \
282  &ahrs_float.body_rate_d.q, \
283  &ahrs_float.body_rate_d.r); \
284  }
285 
286 #define PERIODIC_SEND_STAB_ATTITUDE_REF(_trans, _dev) { \
287  DOWNLINK_SEND_STAB_ATTITUDE_REF_FLOAT(_trans, _dev, \
288  &stab_att_sp_euler.phi, \
289  &stab_att_sp_euler.theta, \
290  &stab_att_sp_euler.psi, \
291  &stab_att_ref_euler.phi, \
292  &stab_att_ref_euler.theta, \
293  &stab_att_ref_euler.psi, \
294  &stab_att_ref_rate.p, \
295  &stab_att_ref_rate.q, \
296  &stab_att_ref_rate.r, \
297  &stab_att_ref_accel.p, \
298  &stab_att_ref_accel.q, \
299  &stab_att_ref_accel.r); \
300  }
301 
302 #endif /* STABILISATION_ATTITUDE_TYPE_FLOAT */
303 
304 
306 #define PERIODIC_SEND_FILTER_ALIGNER(_trans, _dev) { \
307  DOWNLINK_SEND_FILTER_ALIGNER(_trans, _dev, \
308  &ahrs_aligner.lp_gyro.p, \
309  &ahrs_aligner.lp_gyro.q, \
310  &ahrs_aligner.lp_gyro.r, \
311  &imu.gyro.p, \
312  &imu.gyro.q, \
313  &imu.gyro.r, \
314  &ahrs_aligner.noise, \
315  &ahrs_aligner.low_noise_cnt, \
316  &ahrs_aligner.status); \
317  }
318 
319 
320 #define PERIODIC_SEND_ROTORCRAFT_CMD(_trans, _dev) { \
321  DOWNLINK_SEND_ROTORCRAFT_CMD(_trans, _dev, \
322  &stabilization_cmd[COMMAND_ROLL], \
323  &stabilization_cmd[COMMAND_PITCH], \
324  &stabilization_cmd[COMMAND_YAW], \
325  &stabilization_cmd[COMMAND_THRUST]); \
326  }
327 
328 
329 #if USE_AHRS_CMPL_EULER
331 #define PERIODIC_SEND_FILTER(_trans, _dev) { \
332  DOWNLINK_SEND_FILTER(_trans, _dev, \
333  &ahrs.ltp_to_imu_euler.phi, \
334  &ahrs.ltp_to_imu_euler.theta, \
335  &ahrs.ltp_to_imu_euler.psi, \
336  &ahrs_impl.measure.phi, \
337  &ahrs_impl.measure.theta, \
338  &ahrs_impl.measure.psi, \
339  &ahrs_impl.hi_res_euler.phi, \
340  &ahrs_impl.hi_res_euler.theta, \
341  &ahrs_impl.hi_res_euler.psi, \
342  &ahrs_impl.residual.phi, \
343  &ahrs_impl.residual.theta, \
344  &ahrs_impl.residual.psi, \
345  &ahrs_impl.gyro_bias.p, \
346  &ahrs_impl.gyro_bias.q, \
347  &ahrs_impl.gyro_bias.r); \
348  }
349 #else
350 #define PERIODIC_SEND_FILTER(_trans, _dev) {}
351 #endif
352 
353 #if USE_AHRS_LKF
354 #include "subsystems/ahrs.h"
355 #include "ahrs/ahrs_float_lkf.h"
356 #define PERIODIC_SEND_AHRS_LKF(_trans, _dev) { \
357  DOWNLINK_SEND_AHRS_LKF(&bafl_eulers.phi, \
358  _trans, _dev, \
359  &bafl_eulers.theta, \
360  &bafl_eulers.psi, \
361  &bafl_quat.qi, \
362  &bafl_quat.qx, \
363  &bafl_quat.qy, \
364  &bafl_quat.qz, \
365  &bafl_rates.p, \
366  &bafl_rates.q, \
367  &bafl_rates.r, \
368  &bafl_accel_measure.x, \
369  &bafl_accel_measure.y, \
370  &bafl_accel_measure.z, \
371  &bafl_mag.x, \
372  &bafl_mag.y, \
373  &bafl_mag.z); \
374  }
375 #define PERIODIC_SEND_AHRS_LKF_DEBUG(_trans, _dev) { \
376  DOWNLINK_SEND_AHRS_LKF_DEBUG(_trans, _dev, \
377  &bafl_X[0], \
378  &bafl_X[1], \
379  &bafl_X[2], \
380  &bafl_bias.p, \
381  &bafl_bias.q, \
382  &bafl_bias.r, \
383  &bafl_qnorm, \
384  &bafl_phi_accel, \
385  &bafl_theta_accel, \
386  &bafl_P[0][0], \
387  &bafl_P[1][1], \
388  &bafl_P[2][2], \
389  &bafl_P[3][3], \
390  &bafl_P[4][4], \
391  &bafl_P[5][5]); \
392  }
393 #define PERIODIC_SEND_AHRS_LKF_ACC_DBG(_trans, _dev) { \
394  DOWNLINK_SEND_AHRS_LKF_ACC_DBG(_trans, _dev, \
395  &bafl_q_a_err.qi, \
396  &bafl_q_a_err.qx, \
397  &bafl_q_a_err.qy, \
398  &bafl_q_a_err.qz, \
399  &bafl_b_a_err.p, \
400  &bafl_b_a_err.q, \
401  &bafl_b_a_err.r); \
402  }
403 #define PERIODIC_SEND_AHRS_LKF_MAG_DBG(_trans, _dev) { \
404  DOWNLINK_SEND_AHRS_LKF_MAG_DBG(_trans, _dev, \
405  &bafl_q_m_err.qi, \
406  &bafl_q_m_err.qx, \
407  &bafl_q_m_err.qy, \
408  &bafl_q_m_err.qz, \
409  &bafl_b_m_err.p, \
410  &bafl_b_m_err.q, \
411  &bafl_b_m_err.r); \
412  }
413 #else
414 #define PERIODIC_SEND_AHRS_LKF(_trans, _dev) {}
415 #define PERIODIC_SEND_AHRS_LKF_DEBUG(_trans, _dev) {}
416 #define PERIODIC_SEND_AHRS_LKF_MAG_DBG(_trans, _dev) {}
417 #define PERIODIC_SEND_AHRS_LKF_ACC_DBG(_trans, _dev) {}
418 #endif
419 
420 #if defined STABILISATION_ATTITUDE_TYPE_QUAT && defined STABILISATION_ATTITUDE_TYPE_INT
421 #define PERIODIC_SEND_AHRS_REF_QUAT(_trans, _dev) { \
422  DOWNLINK_SEND_AHRS_REF_QUAT(_trans, _dev, \
423  &stab_att_ref_quat.qi, \
424  &stab_att_ref_quat.qx, \
425  &stab_att_ref_quat.qy, \
426  &stab_att_ref_quat.qz, \
427  &ahrs.ltp_to_body_quat.qi, \
428  &ahrs.ltp_to_body_quat.qx, \
429  &ahrs.ltp_to_body_quat.qy, \
430  &ahrs.ltp_to_body_quat.qz); \
431  }
432 #else
433 #define PERIODIC_SEND_AHRS_REF_QUAT(_trans, _dev) {}
434 #endif /* STABILISATION_ATTITUDE_TYPE_QUAT */
435 
436 #define PERIODIC_SEND_AHRS_QUAT_INT(_trans, _dev) { \
437  DOWNLINK_SEND_AHRS_QUAT_INT(_trans, _dev, \
438  &ahrs.ltp_to_imu_quat.qi, \
439  &ahrs.ltp_to_imu_quat.qx, \
440  &ahrs.ltp_to_imu_quat.qy, \
441  &ahrs.ltp_to_imu_quat.qz, \
442  &ahrs.ltp_to_body_quat.qi, \
443  &ahrs.ltp_to_body_quat.qx, \
444  &ahrs.ltp_to_body_quat.qy, \
445  &ahrs.ltp_to_body_quat.qz); \
446  }
447 
448 #define PERIODIC_SEND_AHRS_EULER_INT(_trans, _dev) { \
449  DOWNLINK_SEND_AHRS_EULER_INT(_trans, _dev, \
450  &ahrs.ltp_to_imu_euler.phi, \
451  &ahrs.ltp_to_imu_euler.theta, \
452  &ahrs.ltp_to_imu_euler.psi, \
453  &ahrs.ltp_to_body_euler.phi, \
454  &ahrs.ltp_to_body_euler.theta, \
455  &ahrs.ltp_to_body_euler.psi); \
456  }
457 
458 #define PERIODIC_SEND_AHRS_RMAT_INT(_trans, _dev) { \
459  DOWNLINK_SEND_AHRS_RMAT(_trans, _dev, \
460  &ahrs.ltp_to_imu_rmat.m[0], \
461  &ahrs.ltp_to_imu_rmat.m[1], \
462  &ahrs.ltp_to_imu_rmat.m[2], \
463  &ahrs.ltp_to_imu_rmat.m[3], \
464  &ahrs.ltp_to_imu_rmat.m[4], \
465  &ahrs.ltp_to_imu_rmat.m[5], \
466  &ahrs.ltp_to_imu_rmat.m[6], \
467  &ahrs.ltp_to_imu_rmat.m[7], \
468  &ahrs.ltp_to_imu_rmat.m[8], \
469  &ahrs.ltp_to_body_rmat.m[0], \
470  &ahrs.ltp_to_body_rmat.m[1], \
471  &ahrs.ltp_to_body_rmat.m[2], \
472  &ahrs.ltp_to_body_rmat.m[3], \
473  &ahrs.ltp_to_body_rmat.m[4], \
474  &ahrs.ltp_to_body_rmat.m[5], \
475  &ahrs.ltp_to_body_rmat.m[6], \
476  &ahrs.ltp_to_body_rmat.m[7], \
477  &ahrs.ltp_to_body_rmat.m[8]); \
478  }
479 
480 
481 
482 #if USE_VFF
483 #include "subsystems/ins/vf_float.h"
484 #define PERIODIC_SEND_VFF(_trans, _dev) { \
485  DOWNLINK_SEND_VFF(_trans, _dev, \
486  &vff_z_meas, \
487  &vff_z, \
488  &vff_zdot, \
489  &vff_bias, \
490  & vff_P[0][0], \
491  & vff_P[1][1], \
492  & vff_P[2][2]); \
493  }
494 #else
495 #define PERIODIC_SEND_VFF(_trans, _dev) {}
496 #endif
497 
498 #if USE_HFF
499 #include "subsystems/ins/hf_float.h"
500 #define PERIODIC_SEND_HFF(_trans, _dev) { \
501  DOWNLINK_SEND_HFF(_trans, _dev, \
502  &b2_hff_state.x, \
503  &b2_hff_state.y, \
504  &b2_hff_state.xdot, \
505  &b2_hff_state.ydot, \
506  &b2_hff_state.xdotdot, \
507  &b2_hff_state.ydotdot); \
508  }
509 #define PERIODIC_SEND_HFF_DBG(_trans, _dev) { \
510  DOWNLINK_SEND_HFF_DBG(_trans, _dev, \
511  &b2_hff_x_meas, \
512  &b2_hff_y_meas, \
513  &b2_hff_xd_meas, \
514  &b2_hff_yd_meas, \
515  &b2_hff_state.xP[0][0], \
516  &b2_hff_state.yP[0][0], \
517  &b2_hff_state.xP[1][1], \
518  &b2_hff_state.yP[1][1]); \
519  }
520 #ifdef GPS_LAG
521 #define PERIODIC_SEND_HFF_GPS(_trans, _dev) { \
522  DOWNLINK_SEND_HFF_GPS(_trans, _dev, \
523  &(b2_hff_rb_last->lag_counter), \
524  &lag_counter_err, \
525  &save_counter); \
526  }
527 #else
528 #define PERIODIC_SEND_HFF_GPS(_trans, _dev) {}
529 #endif
530 #else
531 #define PERIODIC_SEND_HFF(_trans, _dev) {}
532 #define PERIODIC_SEND_HFF_DBG(_trans, _dev) {}
533 #define PERIODIC_SEND_HFF_GPS(_trans, _dev) {}
534 #endif
535 
536 #define PERIODIC_SEND_GUIDANCE(_trans, _dev) { \
537  DOWNLINK_SEND_GUIDANCE(_trans, _dev, \
538  &guidance_h_cur_pos.x, \
539  &guidance_h_cur_pos.y, \
540  &guidance_h_held_pos.x, \
541  &guidance_h_held_pos.y); \
542  }
543 
544 #define PERIODIC_SEND_INS_Z(_trans, _dev) { \
545  DOWNLINK_SEND_INS_Z(_trans, _dev, \
546  &ins_baro_alt, \
547  &ins_ltp_pos.z, \
548  &ins_ltp_speed.z, \
549  &ins_ltp_accel.z); \
550  }
551 
552 #define PERIODIC_SEND_INS(_trans, _dev) { \
553  DOWNLINK_SEND_INS(_trans, _dev, \
554  &ins_ltp_pos.x, \
555  &ins_ltp_pos.y, \
556  &ins_ltp_pos.z, \
557  &ins_ltp_speed.x, \
558  &ins_ltp_speed.y, \
559  &ins_ltp_speed.z, \
560  &ins_ltp_accel.x, \
561  &ins_ltp_accel.y, \
562  &ins_ltp_accel.z); \
563  }
564 
565 #define PERIODIC_SEND_INS_REF(_trans, _dev) { \
566  if (ins_ltp_initialised) \
567  DOWNLINK_SEND_INS_REF(_trans, _dev, \
568  &ins_ltp_def.ecef.x, \
569  &ins_ltp_def.ecef.y, \
570  &ins_ltp_def.ecef.z, \
571  &ins_ltp_def.lla.lat, \
572  &ins_ltp_def.lla.lon, \
573  &ins_ltp_def.lla.alt, \
574  &ins_ltp_def.hmsl, \
575  &ins_qfe); \
576  }
577 
578 #define PERIODIC_SEND_VERT_LOOP(_trans, _dev) { \
579  DOWNLINK_SEND_VERT_LOOP(_trans, _dev, \
580  &guidance_v_z_sp, \
581  &guidance_v_zd_sp, \
582  &ins_ltp_pos.z, \
583  &ins_ltp_speed.z, \
584  &ins_ltp_accel.z, \
585  &guidance_v_z_ref, \
586  &guidance_v_zd_ref, \
587  &guidance_v_zdd_ref, \
588  &gv_adapt_X, \
589  &gv_adapt_P, \
590  &gv_adapt_Xmeas, \
591  &guidance_v_z_sum_err, \
592  &guidance_v_ff_cmd, \
593  &guidance_v_fb_cmd, \
594  &guidance_v_delta_t); \
595  }
596 
597 #define PERIODIC_SEND_HOVER_LOOP(_trans, _dev) { \
598  DOWNLINK_SEND_HOVER_LOOP(_trans, _dev, \
599  &guidance_h_pos_sp.x, \
600  &guidance_h_pos_sp.y, \
601  &ins_ltp_pos.x, \
602  &ins_ltp_pos.y, \
603  &ins_ltp_speed.x, \
604  &ins_ltp_speed.y, \
605  &ins_ltp_accel.x, \
606  &ins_ltp_accel.y, \
607  &guidance_h_pos_err.x, \
608  &guidance_h_pos_err.y, \
609  &guidance_h_speed_err.x, \
610  &guidance_h_speed_err.y, \
611  &guidance_h_pos_err_sum.x, \
612  &guidance_h_pos_err_sum.y, \
613  &guidance_h_nav_err.x, \
614  &guidance_h_nav_err.y, \
615  &guidance_h_command_earth.x, \
616  &guidance_h_command_earth.y, \
617  &guidance_h_command_body.phi, \
618  &guidance_h_command_body.theta, \
619  &guidance_h_command_body.psi); \
620  }
621 
622 #define PERIODIC_SEND_GUIDANCE_H_REF(_trans, _dev) { \
623  DOWNLINK_SEND_GUIDANCE_H_REF_INT(_trans, _dev, \
624  &guidance_h_pos_sp.x, \
625  &guidance_h_pos_ref.x, \
626  &guidance_h_speed_ref.x, \
627  &guidance_h_accel_ref.x, \
628  &guidance_h_pos_sp.y, \
629  &guidance_h_pos_ref.y, \
630  &guidance_h_speed_ref.y, \
631  &guidance_h_accel_ref.y); \
632 }
633 
635 #define PERIODIC_SEND_ROTORCRAFT_FP(_trans, _dev) { \
636  int32_t carrot_up = -guidance_v_z_sp; \
637  DOWNLINK_SEND_ROTORCRAFT_FP( _trans, _dev, \
638  &ins_enu_pos.x, \
639  &ins_enu_pos.y, \
640  &ins_enu_pos.z, \
641  &ins_enu_speed.x, \
642  &ins_enu_speed.y, \
643  &ins_enu_speed.z, \
644  &ahrs.ltp_to_body_euler.phi, \
645  &ahrs.ltp_to_body_euler.theta, \
646  &ahrs.ltp_to_body_euler.psi, \
647  &guidance_h_pos_sp.y, \
648  &guidance_h_pos_sp.x, \
649  &carrot_up, \
650  &guidance_h_command_body.psi, \
651  &stabilization_cmd[COMMAND_THRUST], \
652  &autopilot_flight_time); \
653  }
654 
655 #if USE_GPS
656 #define PERIODIC_SEND_GPS_INT(_trans, _dev) { \
657  DOWNLINK_SEND_GPS_INT( _trans, _dev, \
658  &gps.ecef_pos.x, \
659  &gps.ecef_pos.y, \
660  &gps.ecef_pos.z, \
661  &gps.lla_pos.lat, \
662  &gps.lla_pos.lon, \
663  &gps.lla_pos.alt, \
664  &gps.hmsl, \
665  &gps.ecef_vel.x, \
666  &gps.ecef_vel.y, \
667  &gps.ecef_vel.z, \
668  &gps.pacc, \
669  &gps.sacc, \
670  &gps.tow, \
671  &gps.pdop, \
672  &gps.num_sv, \
673  &gps.fix); \
674  static uint8_t i; \
675  static uint8_t last_cnos[GPS_NB_CHANNELS]; \
676  if (i == gps.nb_channels) i = 0; \
677  if (i < gps.nb_channels && gps.svinfos[i].cno > 0 && gps.svinfos[i].cno != last_cnos[i]) { \
678  DOWNLINK_SEND_SVINFO(DefaultChannel, DefaultDevice, &i, &gps.svinfos[i].svid, &gps.svinfos[i].flags, &gps.svinfos[i].qi, &gps.svinfos[i].cno, &gps.svinfos[i].elev, &gps.svinfos[i].azim); \
679  last_cnos[i] = gps.svinfos[i].cno; \
680  } \
681  i++; \
682  }
683 #else
684 #define PERIODIC_SEND_GPS_INT(_trans, _dev) {}
685 #endif
686 
688 #define PERIODIC_SEND_ROTORCRAFT_NAV_STATUS(_trans, _dev) { \
689  DOWNLINK_SEND_ROTORCRAFT_NAV_STATUS(_trans, _dev, \
690  &block_time, \
691  &stage_time, \
692  &nav_block, \
693  &nav_stage, \
694  &horizontal_mode); \
695  if (horizontal_mode == HORIZONTAL_MODE_ROUTE) { \
696  float sx = POS_FLOAT_OF_BFP(waypoints[nav_segment_start].x); \
697  float sy = POS_FLOAT_OF_BFP(waypoints[nav_segment_start].y); \
698  float ex = POS_FLOAT_OF_BFP(waypoints[nav_segment_end].x); \
699  float ey = POS_FLOAT_OF_BFP(waypoints[nav_segment_end].y); \
700  DOWNLINK_SEND_SEGMENT(_trans, _dev, &sx, &sy, &ex, &ey); \
701  } \
702  else if (horizontal_mode == HORIZONTAL_MODE_CIRCLE) { \
703  float cx = POS_FLOAT_OF_BFP(waypoints[nav_circle_centre].x); \
704  float cy = POS_FLOAT_OF_BFP(waypoints[nav_circle_centre].y); \
705  float r = POS_FLOAT_OF_BFP(nav_circle_radius); \
706  DOWNLINK_SEND_CIRCLE(_trans, _dev, &cx, &cy, &r); \
707  } \
708  }
709 
710 #define PERIODIC_SEND_WP_MOVED(_trans, _dev) { \
711  static uint8_t i; \
712  i++; if (i >= nb_waypoint) i = 0; \
713  DOWNLINK_SEND_WP_MOVED_ENU(_trans, _dev, \
714  &i, \
715  &(waypoints[i].x), \
716  &(waypoints[i].y), \
717  &(waypoints[i].z)); \
718  }
719 
720 #if USE_CAM
721 #define PERIODIC_SEND_BOOZ2_CAM(_trans, _dev) DOWNLINK_SEND_BOOZ2_CAM(_trans, _dev,&booz_cam_tilt,&booz_cam_pan);
722 #else
723 #define PERIODIC_SEND_BOOZ2_CAM(_trans, _dev) {}
724 #endif
725 
726 #define PERIODIC_SEND_ROTORCRAFT_TUNE_HOVER(_trans, _dev) { \
727  DOWNLINK_SEND_ROTORCRAFT_TUNE_HOVER(_trans, _dev, \
728  &radio_control.values[RADIO_ROLL], \
729  &radio_control.values[RADIO_PITCH], \
730  &radio_control.values[RADIO_YAW], \
731  &stabilization_cmd[COMMAND_ROLL], \
732  &stabilization_cmd[COMMAND_PITCH], \
733  &stabilization_cmd[COMMAND_YAW], \
734  &stabilization_cmd[COMMAND_THRUST], \
735  &ahrs.ltp_to_imu_euler.phi, \
736  &ahrs.ltp_to_imu_euler.theta, \
737  &ahrs.ltp_to_imu_euler.psi, \
738  &ahrs.ltp_to_body_euler.phi, \
739  &ahrs.ltp_to_body_euler.theta, \
740  &ahrs.ltp_to_body_euler.psi); \
741  }
742 
743 #ifdef USE_I2C0
744 #define PERIODIC_SEND_I2C0_ERRORS(_trans, _dev) { \
745  uint16_t i2c0_ack_fail_cnt = i2c0.errors->ack_fail_cnt; \
746  uint16_t i2c0_miss_start_stop_cnt = i2c0.errors->miss_start_stop_cnt; \
747  uint16_t i2c0_arb_lost_cnt = i2c0.errors->arb_lost_cnt; \
748  uint16_t i2c0_over_under_cnt = i2c0.errors->over_under_cnt; \
749  uint16_t i2c0_pec_recep_cnt = i2c0.errors->pec_recep_cnt; \
750  uint16_t i2c0_timeout_tlow_cnt = i2c0.errors->timeout_tlow_cnt; \
751  uint16_t i2c0_smbus_alert_cnt = i2c0.errors->smbus_alert_cnt; \
752  uint16_t i2c0_unexpected_event_cnt = i2c0.errors->unexpected_event_cnt; \
753  uint32_t i2c0_last_unexpected_event = i2c0.errors->last_unexpected_event; \
754  const uint8_t _bus0 = 0; \
755  DOWNLINK_SEND_I2C_ERRORS(_trans, _dev, \
756  &i2c0_ack_fail_cnt, \
757  &i2c0_miss_start_stop_cnt, \
758  &i2c0_arb_lost_cnt, \
759  &i2c0_over_under_cnt, \
760  &i2c0_pec_recep_cnt, \
761  &i2c0_timeout_tlow_cnt, \
762  &i2c0_smbus_alert_cnt, \
763  &i2c0_unexpected_event_cnt, \
764  &i2c0_last_unexpected_event, \
765  &_bus0); \
766  }
767 #else
768 #define PERIODIC_SEND_I2C0_ERRORS(_trans, _dev) {}
769 #endif
770 
771 #ifdef USE_I2C1
772 #define PERIODIC_SEND_I2C1_ERRORS(_trans, _dev) { \
773  uint16_t i2c1_ack_fail_cnt = i2c1.errors->ack_fail_cnt; \
774  uint16_t i2c1_miss_start_stop_cnt = i2c1.errors->miss_start_stop_cnt; \
775  uint16_t i2c1_arb_lost_cnt = i2c1.errors->arb_lost_cnt; \
776  uint16_t i2c1_over_under_cnt = i2c1.errors->over_under_cnt; \
777  uint16_t i2c1_pec_recep_cnt = i2c1.errors->pec_recep_cnt; \
778  uint16_t i2c1_timeout_tlow_cnt = i2c1.errors->timeout_tlow_cnt; \
779  uint16_t i2c1_smbus_alert_cnt = i2c1.errors->smbus_alert_cnt; \
780  uint16_t i2c1_unexpected_event_cnt = i2c1.errors->unexpected_event_cnt; \
781  uint32_t i2c1_last_unexpected_event = i2c1.errors->last_unexpected_event; \
782  const uint8_t _bus1 = 1; \
783  DOWNLINK_SEND_I2C_ERRORS(_trans, _dev, \
784  &i2c1_ack_fail_cnt, \
785  &i2c1_miss_start_stop_cnt, \
786  &i2c1_arb_lost_cnt, \
787  &i2c1_over_under_cnt, \
788  &i2c1_pec_recep_cnt, \
789  &i2c1_timeout_tlow_cnt, \
790  &i2c1_smbus_alert_cnt, \
791  &i2c1_unexpected_event_cnt, \
792  &i2c1_last_unexpected_event, \
793  &_bus1); \
794  }
795 #else
796 #define PERIODIC_SEND_I2C1_ERRORS(_trans, _dev) {}
797 #endif
798 
799 #ifdef USE_I2C2
800 #define PERIODIC_SEND_I2C2_ERRORS(_trans, _dev) { \
801  uint16_t i2c2_ack_fail_cnt = i2c2.errors->ack_fail_cnt; \
802  uint16_t i2c2_miss_start_stop_cnt = i2c2.errors->miss_start_stop_cnt; \
803  uint16_t i2c2_arb_lost_cnt = i2c2.errors->arb_lost_cnt; \
804  uint16_t i2c2_over_under_cnt = i2c2.errors->over_under_cnt; \
805  uint16_t i2c2_pec_recep_cnt = i2c2.errors->pec_recep_cnt; \
806  uint16_t i2c2_timeout_tlow_cnt = i2c2.errors->timeout_tlow_cnt; \
807  uint16_t i2c2_smbus_alert_cnt = i2c2.errors->smbus_alert_cnt; \
808  uint16_t i2c2_unexpected_event_cnt = i2c2.errors->unexpected_event_cnt; \
809  uint32_t i2c2_last_unexpected_event = i2c2.errors->last_unexpected_event; \
810  const uint8_t _bus2 = 2; \
811  DOWNLINK_SEND_I2C_ERRORS(_trans, _dev, \
812  &i2c2_ack_fail_cnt, \
813  &i2c2_miss_start_stop_cnt, \
814  &i2c2_arb_lost_cnt, \
815  &i2c2_over_under_cnt, \
816  &i2c2_pec_recep_cnt, \
817  &i2c2_timeout_tlow_cnt, \
818  &i2c2_smbus_alert_cnt, \
819  &i2c2_unexpected_event_cnt, \
820  &i2c2_last_unexpected_event, \
821  &_bus2); \
822  }
823 #else
824 #define PERIODIC_SEND_I2C2_ERRORS(_trans, _dev) {}
825 #endif
826 
827 #ifndef SITL
828 #define PERIODIC_SEND_I2C_ERRORS(_trans, _dev) { \
829  PERIODIC_SEND_I2C0_ERRORS(_trans, _dev); \
830  PERIODIC_SEND_I2C1_ERRORS(_trans, _dev); \
831  PERIODIC_SEND_I2C2_ERRORS(_trans, _dev); \
832 }
833 #else
834 #define PERIODIC_SEND_I2C_ERRORS(_trans, _dev) {}
835 #endif
836 
837 // FIXME: still used?? or replace by EXTRA_ADC
838 #define PERIODIC_SEND_BOOZ2_SONAR(_trans, _dev) {}
839 
840 #ifdef BOOZ2_TRACK_CAM
841 #include "cam_track.h"
842 #define PERIODIC_SEND_CAM_TRACK(_trans, _dev) DOWNLINK_SEND_NPS_SPEED_POS(_trans, _dev, \
843  &target_accel_ned.x, \
844  &target_accel_ned.y, \
845  &target_accel_ned.z, \
846  &target_speed_ned.x, \
847  &target_speed_ned.y, \
848  &target_speed_ned.z, \
849  &target_pos_ned.x, \
850  &target_pos_ned.y, \
851  &target_pos_ned.z)
852 #else
853 #define PERIODIC_SEND_CAM_TRACK(_trans, _dev) {}
854 #endif
855 
856 #include "generated/settings.h"
857 #define PERIODIC_SEND_DL_VALUE(_trans, _dev) PeriodicSendDlValue(_trans, _dev)
858 
859 #endif /* TELEMETRY_H */
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
Attitude and Heading Reference System interface.
Device independent INS code.
blob tracking with cmucam
Architecture independent timing functions.
Device independent GPS code (interface)
Inertial Measurement Unit interface.
General stabilization interface for rotorcrafts.