Paparazzi UAS  v5.0.5_stable-7-g4b8bbb7
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
ap_downlink.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2006- Pascal Brisset, Antoine Drouin
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, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  */
21 
34 #ifndef AP_DOWNLINK_H
35 #define AP_DOWNLINK_H
36 
37 #include <inttypes.h>
38 
39 #include "generated/airframe.h"
40 #include "state.h"
41 
42 #ifndef DOWNLINK_DEVICE
43 #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
44 #endif
46 
47 #include "messages.h"
48 #include "generated/periodic_telemetry.h"
49 
50 #if defined DOWNLINK
51 #define Downlink(x) x
52 #else
53 #define Downlink(x) {}
54 #endif
55 
56 #define PERIODIC_SEND_ALIVE(_trans, _dev) DOWNLINK_SEND_ALIVE(_trans, _dev, 16, MD5SUM);
57 
58 #define PERIODIC_SEND_BAT(_trans, _dev) { \
59  int16_t amps = (int16_t) (current/10); \
60  Downlink({ int16_t e = energy; \
61  DOWNLINK_SEND_BAT(_trans, _dev, \
62  &v_ctl_throttle_slewed, \
63  &vsupply, \
64  &amps, \
65  &autopilot_flight_time, \
66  &kill_throttle, \
67  &block_time, \
68  &stage_time, \
69  &e); \
70  }); \
71  }
72 
73 #ifdef MCU_SPI_LINK
74 #define PERIODIC_SEND_DEBUG_MCU_LINK(_trans, _dev) DOWNLINK_SEND_DEBUG_MCU_LINK(_trans, _dev, &link_mcu_nb_err, &link_mcu_fbw_nb_err, &mcu1_ppm_cpt);
75 #else
76 #define PERIODIC_SEND_DEBUG_MCU_LINK(_trans, _dev) {}
77 #endif
78 
79 
80 #define PERIODIC_SEND_DOWNLINK(_trans, _dev) { \
81  static uint16_t last; \
82  uint16_t rate = (downlink_nb_bytes - last) / PERIOD_DOWNLINK_Ap_0; \
83  last = downlink_nb_bytes; \
84  DOWNLINK_SEND_DOWNLINK(_trans, _dev, &downlink_nb_ovrn, &rate, &downlink_nb_msgs); \
85 }
86 
87 
88 #define PERIODIC_SEND_ATTITUDE(_trans, _dev) Downlink({ \
89  struct FloatEulers* att = stateGetNedToBodyEulers_f(); \
90  DOWNLINK_SEND_ATTITUDE(_trans, _dev, &(att->phi), &(att->psi), &(att->theta)); \
91 })
92 
93 
94 #ifndef USE_AIRSPEED
95 #define PERIODIC_SEND_DESIRED(_trans, _dev) { float v_ctl_auto_airspeed_setpoint = NOMINAL_AIRSPEED; DOWNLINK_SEND_DESIRED(_trans, _dev, &h_ctl_roll_setpoint, &h_ctl_pitch_loop_setpoint, &h_ctl_course_setpoint, &desired_x, &desired_y, &v_ctl_altitude_setpoint, &v_ctl_climb_setpoint, &v_ctl_auto_airspeed_setpoint);}
96 #else
97 #define PERIODIC_SEND_DESIRED(_trans, _dev) DOWNLINK_SEND_DESIRED(_trans, _dev, &h_ctl_roll_setpoint, &h_ctl_pitch_loop_setpoint, &h_ctl_course_setpoint, &desired_x, &desired_y, &v_ctl_altitude_setpoint, &v_ctl_climb_setpoint, &v_ctl_auto_airspeed_setpoint);
98 #endif
99 
100 
101 #if USE_INFRARED
102 #define PERIODIC_SEND_STATE_FILTER_STATUS(_trans, _dev) { uint16_t contrast = abs(infrared.roll) + abs(infrared.pitch) + abs(infrared.top); uint8_t mde = 3; if (contrast < 50) mde = 7; DOWNLINK_SEND_STATE_FILTER_STATUS(_trans, _dev, &mde, &contrast); }
103 #elif USE_IMU && USE_AHRS
104 #define PERIODIC_SEND_STATE_FILTER_STATUS(_trans, _dev) { uint8_t mde = 3; if (ahrs.status == AHRS_UNINIT) mde = 2; if (ahrs_timeout_counter > 10) mde = 5; uint16_t val = 0; DOWNLINK_SEND_STATE_FILTER_STATUS(_trans, _dev, &mde, &val); }
105 #else
106 #define PERIODIC_SEND_STATE_FILTER_STATUS(_trans, _dev) {}
107 #endif
108 
109 #define PERIODIC_SEND_NAVIGATION_REF(_trans, _dev) DOWNLINK_SEND_NAVIGATION_REF(_trans, _dev, &nav_utm_east0, &nav_utm_north0, &nav_utm_zone0);
110 
111 
112 #define DownlinkSendWp(_trans, _dev, i) { \
113  float x = nav_utm_east0 + waypoints[i].x; \
114  float y = nav_utm_north0 + waypoints[i].y; \
115  DOWNLINK_SEND_WP_MOVED(_trans, _dev, &i, &x, &y, &(waypoints[i].a),&nav_utm_zone0); \
116 }
117 
118 
119 #define PERIODIC_SEND_WP_MOVED(_trans, _dev) { \
120  static uint8_t i; \
121  i++; if (i >= nb_waypoint) i = 0; \
122  DownlinkSendWp(_trans, _dev, i); \
123 }
124 
125 #if defined RADIO_CALIB && defined RADIO_CONTROL_SETTINGS
126 #include "rc_settings.h"
127 #define PERIODIC_SEND_PPRZ_MODE(_trans, _dev) DOWNLINK_SEND_PPRZ_MODE(_trans, _dev, &pprz_mode, &v_ctl_mode, &lateral_mode, &horizontal_mode, &rc_settings_mode, &mcu1_status);
128 #define PERIODIC_SEND_SETTINGS(_trans, _dev) if (!RcSettingsOff()) DOWNLINK_SEND_SETTINGS(_trans, _dev, &slider_1_val, &slider_2_val);
129 #else
130 #define PERIODIC_SEND_PPRZ_MODE(_trans, _dev) { \
131  uint8_t rc_settings_mode_none = 0; \
132  DOWNLINK_SEND_PPRZ_MODE(_trans, _dev, &pprz_mode, &v_ctl_mode, &lateral_mode, &horizontal_mode, &rc_settings_mode_none, &mcu1_status); \
133  }
134 #define PERIODIC_SEND_SETTINGS(_trans, _dev) {}
135 #endif
136 
137 #if USE_INFRARED || USE_INFRARED_TELEMETRY
139 #define PERIODIC_SEND_IR_SENSORS(_trans, _dev) DOWNLINK_SEND_IR_SENSORS(_trans, _dev, &infrared.value.ir1, &infrared.value.ir2, &infrared.pitch, &infrared.roll, &infrared.top);
140 #else
141 #define PERIODIC_SEND_IR_SENSORS(_trans, _dev) ;
142 #endif
143 
144 #define PERIODIC_SEND_ADC(_trans, _dev) {}
145 
146 
147 #define PERIODIC_SEND_CALIBRATION(_trans, _dev) DOWNLINK_SEND_CALIBRATION(_trans, _dev, &v_ctl_auto_throttle_sum_err, &v_ctl_auto_throttle_submode)
148 
149 #define PERIODIC_SEND_CIRCLE(_trans, _dev) if (nav_in_circle) { DOWNLINK_SEND_CIRCLE(_trans, _dev, &nav_circle_x, &nav_circle_y, &nav_circle_radius); }
150 
151 #define PERIODIC_SEND_SEGMENT(_trans, _dev) if (nav_in_segment) { DOWNLINK_SEND_SEGMENT(_trans, _dev, &nav_segment_x_1, &nav_segment_y_1, &nav_segment_x_2, &nav_segment_y_2); }
152 
153 #ifdef IMU_TYPE_H
154 # ifdef INS_MODULE_H
155 # include "modules/ins/ins_module.h"
156 # define PERIODIC_SEND_IMU_ACCEL_RAW(_trans, _dev) {}
157 # define PERIODIC_SEND_IMU_GYRO_RAW(_trans, _dev) {}
158 # define PERIODIC_SEND_IMU_MAG_RAW(_trans, _dev) {}
159 # define PERIODIC_SEND_IMU_GYRO(_trans, _dev) { DOWNLINK_SEND_IMU_GYRO(_trans, _dev, &ins_p, &ins_q, &ins_r)}
160 # define PERIODIC_SEND_IMU_ACCEL(_trans, _dev) { DOWNLINK_SEND_IMU_ACCEL(_trans, _dev, &ins_ax, &ins_ay, &ins_az)}
161 # define PERIODIC_SEND_IMU_MAG(_trans, _dev) { DOWNLINK_SEND_IMU_MAG(_trans, _dev, &ins_mx, &ins_my, &ins_mz)}
162 # else
163 # include "subsystems/imu.h"
164 # define PERIODIC_SEND_IMU_ACCEL_RAW(_trans, _dev) { DOWNLINK_SEND_IMU_ACCEL_RAW(_trans, _dev, &imu.accel_unscaled.x, &imu.accel_unscaled.y, &imu.accel_unscaled.z)}
165 # define PERIODIC_SEND_IMU_GYRO_RAW(_trans, _dev) { DOWNLINK_SEND_IMU_GYRO_RAW(_trans, _dev, &imu.gyro_unscaled.p, &imu.gyro_unscaled.q, &imu.gyro_unscaled.r)}
166 # define PERIODIC_SEND_IMU_MAG_RAW(_trans, _dev) { DOWNLINK_SEND_IMU_MAG_RAW(_trans, _dev, &imu.mag_unscaled.x, &imu.mag_unscaled.y, &imu.mag_unscaled.z)}
167 # define PERIODIC_SEND_IMU_ACCEL(_trans, _dev) { struct FloatVect3 accel_float; ACCELS_FLOAT_OF_BFP(accel_float, imu.accel); DOWNLINK_SEND_IMU_ACCEL(_trans, _dev, &accel_float.x, &accel_float.y, &accel_float.z)}
168 # define PERIODIC_SEND_IMU_GYRO(_trans, _dev) { struct FloatRates gyro_float; RATES_FLOAT_OF_BFP(gyro_float, imu.gyro); DOWNLINK_SEND_IMU_GYRO(_trans, _dev, &gyro_float.p, &gyro_float.q, &gyro_float.r)}
169 # ifdef USE_MAGNETOMETER
170 # define PERIODIC_SEND_IMU_MAG(_trans, _dev) { struct FloatVect3 mag_float; MAGS_FLOAT_OF_BFP(mag_float, imu.mag); DOWNLINK_SEND_IMU_MAG(_trans, _dev, &mag_float.x, &mag_float.y, &mag_float.z)}
171 # else
172 # define PERIODIC_SEND_IMU_MAG(_trans, _dev) {}
173 # endif
174 # endif
175 #else
176 # define PERIODIC_SEND_IMU_ACCEL_RAW(_trans, _dev) {}
177 # define PERIODIC_SEND_IMU_GYRO_RAW(_trans, _dev) {}
178 # define PERIODIC_SEND_IMU_MAG_RAW(_trans, _dev) {}
179 # define PERIODIC_SEND_IMU_ACCEL(_trans, _dev) {}
180 # define PERIODIC_SEND_IMU_GYRO(_trans, _dev) {}
181 # define PERIODIC_SEND_IMU_MAG(_trans, _dev) {}
182 #endif
183 
184 #ifdef IMU_ANALOG
185 #define PERIODIC_SEND_IMU(_trans, _dev) { int16_t dummy = 42; DOWNLINK_SEND_IMU(_trans, _dev, &(from_fbw.euler_dot[0]), &(from_fbw.euler_dot[1]), &(from_fbw.euler_dot[2]), &dummy, &dummy, &dummy); }
186 #else
187 #define PERIODIC_SEND_IMU(_trans, _dev) {}
188 #endif
189 
190 #define PERIODIC_SEND_ESTIMATOR(_trans, _dev) DOWNLINK_SEND_ESTIMATOR(_trans, _dev, &(stateGetPositionUtm_f()->alt), &(stateGetSpeedEnu_f()->z))
191 
192 #define SEND_NAVIGATION(_trans, _dev) Downlink({ \
193  uint8_t _circle_count = NavCircleCount(); \
194  struct EnuCoor_f* pos = stateGetPositionEnu_f(); \
195  DOWNLINK_SEND_NAVIGATION(_trans, _dev, &nav_block, &nav_stage, &(pos->x), &(pos->y), &dist2_to_wp, &dist2_to_home, &_circle_count, &nav_oval_count); \
196  })
197 
198 #define PERIODIC_SEND_NAVIGATION(_trans, _dev) SEND_NAVIGATION(_trans, _dev)
199 
200 
201 #if defined CAM || defined MOBILE_CAM
202 #define SEND_CAM(_trans, _dev) Downlink({ int16_t x = cam_target_x; int16_t y = cam_target_y; int16_t phi = DegOfRad(cam_phi_c); int16_t theta = DegOfRad(cam_theta_c); DOWNLINK_SEND_CAM(_trans, _dev, &phi, &theta, &x, &y);})
203 #define PERIODIC_SEND_CAM_POINT(_trans, _dev) DOWNLINK_SEND_CAM_POINT(_trans, _dev, &cam_point_distance_from_home, &cam_point_lat, &cam_point_lon)
204 #else
205 #define SEND_CAM(_trans, _dev) {}
206 #define PERIODIC_SEND_CAM_POINT(_trans, _dev) {}
207 #endif
208 
209 #define PERIODIC_SEND_DL_VALUE(_trans, _dev) PeriodicSendDlValue(_trans, _dev)
211 #define PERIODIC_SEND_SURVEY(_trans, _dev) { \
212  if (nav_survey_active) \
213  DOWNLINK_SEND_SURVEY(_trans, _dev, &nav_survey_east, &nav_survey_north, &nav_survey_west, &nav_survey_south); \
214  }
215 
216 #define PERIODIC_SEND_RANGEFINDER(_trans, _dev) DOWNLINK_SEND_RANGEFINDER(_trans, _dev, &rangemeter, &ctl_grz_z_dot, &ctl_grz_z_dot_sum_err, &ctl_grz_z_dot_setpoint, &ctl_grz_z_sum_err, &ctl_grz_z_setpoint, &flying)
217 
218 #define PERIODIC_SEND_TUNE_ROLL(_trans, _dev) DOWNLINK_SEND_TUNE_ROLL(_trans, _dev, &(stateGetBodyRates_f()->p), &(stateGetNedToBodyEulers_f()->phi), &h_ctl_roll_setpoint);
219 
220 #if USE_GPS || defined SITL
221 #define PERIODIC_SEND_GPS_SOL(_trans, _dev) DOWNLINK_SEND_GPS_SOL(_trans, _dev, &gps.pacc, &gps.sacc, &gps.pdop, &gps.num_sv)
222 #else
223 #define PERIODIC_SEND_GPS_SOL(_trans, _dev) {}
224 #endif
225 
226 #define PERIODIC_SEND_GPS(_trans, _dev) { \
227  static uint8_t i; \
228  int16_t climb = -gps.ned_vel.z; \
229  int16_t course = (DegOfRad(gps.course)/((int32_t)1e6)); \
230  DOWNLINK_SEND_GPS(_trans, _dev, &gps.fix, &gps.utm_pos.east, &gps.utm_pos.north, &course, &gps.hmsl, &gps.gspeed, &climb, &gps.week, &gps.tow, &gps.utm_pos.zone, &i); \
231  if ((gps.fix != GPS_FIX_3D) && (i >= gps.nb_channels)) i = 0; \
232  if (i >= gps.nb_channels * 2) i = 0; \
233  if (i < gps.nb_channels && ((gps.fix != GPS_FIX_3D) || (gps.svinfos[i].cno > 0))) { \
234  DOWNLINK_SEND_SVINFO(_trans, _dev, &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); \
235  } \
236  i++; \
237 }
238 
239 #if USE_GPS
240 #define PERIODIC_SEND_GPS_INT(_trans, _dev) { \
241  DOWNLINK_SEND_GPS_INT( _trans, _dev, \
242  &gps.ecef_pos.x, \
243  &gps.ecef_pos.y, \
244  &gps.ecef_pos.z, \
245  &gps.lla_pos.lat, \
246  &gps.lla_pos.lon, \
247  &gps.lla_pos.alt, \
248  &gps.hmsl, \
249  &gps.ecef_vel.x, \
250  &gps.ecef_vel.y, \
251  &gps.ecef_vel.z, \
252  &gps.pacc, \
253  &gps.sacc, \
254  &gps.tow, \
255  &gps.pdop, \
256  &gps.num_sv, \
257  &gps.fix); \
258  }
259 
260 #define PERIODIC_SEND_GPS_LLA(_trans, _dev) { \
261  uint8_t err = 0; \
262  int16_t climb = -gps.ned_vel.z; \
263  int16_t course = (DegOfRad(gps.course)/((int32_t)1e6)); \
264  DOWNLINK_SEND_GPS_LLA( _trans, _dev, \
265  &gps.lla_pos.lat, \
266  &gps.lla_pos.lon, \
267  &gps.lla_pos.alt, \
268  &course, \
269  &gps.gspeed, \
270  &climb, \
271  &gps.week, \
272  &gps.tow, \
273  &gps.fix, \
274  &err); \
275  }
276 #endif
277 
278 #if USE_BARO_MS5534A
279 //#include "baro_MS5534A.h"
280 #define PERIODIC_SEND_BARO_MS5534A(_trans, _dev) DOWNLINK_SEND_BARO_MS5534A(_trans, _dev, &baro_MS5534A_pressure, &baro_MS5534A_temp, &baro_MS5534A_z)
281 #else
282 #define PERIODIC_SEND_BARO_MS5534A(_trans, _dev) {}
283 #endif
284 
285 #if USE_BARO_SCP
286 #include "baro_scp.h"
287 #define PERIODIC_SEND_SCP_STATUS(_trans, _dev) DOWNLINK_SEND_SCP_STATUS(_trans, _dev, &baro_scp_pressure, &baro_scp_temperature)
288 #else
289 #define PERIODIC_SEND_SCP_STATUS(_trans, _dev) {}
290 #endif
291 
292 #if USE_BAROMETER
293 #include "subsystems/sensors/baro.h"
294 #define PERIODIC_SEND_BARO_RAW(_trans, _dev) { \
295  DOWNLINK_SEND_BARO_RAW(_trans, _dev, \
296  &baro.absolute, \
297  &baro.differential); \
298  }
299 #else
300 #define PERIODIC_SEND_BARO_RAW(_trans, _dev) {}
301 #endif
302 
303 #ifdef MEASURE_AIRSPEED
304 #define PERIODIC_SEND_AIRSPEED(_trans, _dev) { \
305  float* airspeed = stateGetAirspeed_f(); \
306  DOWNLINK_SEND_AIRSPEED (_trans, _dev, airspeed, airspeed, airspeed, airspeed); \
307 }
308 #elif USE_AIRSPEED
309 #define PERIODIC_SEND_AIRSPEED(_trans, _dev) DOWNLINK_SEND_AIRSPEED (_trans, _dev, stateGetAirspeed_f(), &v_ctl_auto_airspeed_setpoint, &v_ctl_auto_airspeed_controlled, &v_ctl_auto_groundspeed_setpoint)
310 #else
311 #define PERIODIC_SEND_AIRSPEED(_trans, _dev) {}
312 #endif
313 
314 #define PERIODIC_SEND_ENERGY(_trans, _dev) Downlink({ const int16_t e = energy; const float vsup = ((float)vsupply) / 10.0f; const float curs = ((float) current)/1000.0f; const float power = vsup * curs; DOWNLINK_SEND_ENERGY(_trans, _dev, &vsup, &curs, &e, &power); })
315 
316 
318 #define PERIODIC_SEND_H_CTL_A(_trans, _dev) DOWNLINK_SEND_H_CTL_A(_trans, _dev, &h_ctl_roll_sum_err, &h_ctl_ref_roll_angle, &h_ctl_pitch_sum_err, &h_ctl_ref_pitch_angle)
319 
320 
321 #endif /* AP_DOWNLINK_H */
Common barometric sensor implementation.
Variable setting though the radio control.
Inertial Measurement Unit interface.
API to get/set the generic vehicle states.
Device independent INS code.
Fixed wing horizontal adaptive control.