Paparazzi UAS  v4.2.2_stable-4-gcc32f65
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
ap_downlink.h
Go to the documentation of this file.
1 /*
2  * Paparazzi $Id$
3  *
4  * Copyright (C) 2006- Pascal Brisset, Antoine Drouin
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 
35 #ifndef AP_DOWNLINK_H
36 #define AP_DOWNLINK_H
37 
38 #include <inttypes.h>
39 
40 #include "generated/airframe.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  &estimator_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  DOWNLINK_SEND_ATTITUDE(_trans, _dev, &estimator_phi, &estimator_psi, &estimator_theta); \
90 })
91 
92 
93 #ifndef USE_AIRSPEED
94 #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);}
95 #else
96 #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);
97 #endif
98 
99 
100 #if USE_INFRARED
101 #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); }
102 #elif USE_IMU && USE_AHRS
103 #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); }
104 #else
105 #define PERIODIC_SEND_STATE_FILTER_STATUS(_trans, _dev) {}
106 #endif
107 
108 #define PERIODIC_SEND_NAVIGATION_REF(_trans, _dev) DOWNLINK_SEND_NAVIGATION_REF(_trans, _dev, &nav_utm_east0, &nav_utm_north0, &nav_utm_zone0);
109 
110 
111 #define DownlinkSendWp(_trans, _dev, i) { \
112  float x = nav_utm_east0 + waypoints[i].x; \
113  float y = nav_utm_north0 + waypoints[i].y; \
114  DOWNLINK_SEND_WP_MOVED(_trans, _dev, &i, &x, &y, &(waypoints[i].a),&nav_utm_zone0); \
115 }
116 
117 
118 #define PERIODIC_SEND_WP_MOVED(_trans, _dev) { \
119  static uint8_t i; \
120  i++; if (i >= nb_waypoint) i = 0; \
121  DownlinkSendWp(_trans, _dev, i); \
122 }
123 
124 #if defined RADIO_CALIB && defined RADIO_CONTROL_SETTINGS
125 #include "rc_settings.h"
126 #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);
127 #define PERIODIC_SEND_SETTINGS(_trans, _dev) if (!RcSettingsOff()) DOWNLINK_SEND_SETTINGS(_trans, _dev, &slider_1_val, &slider_2_val);
128 #else
129 #define PERIODIC_SEND_PPRZ_MODE(_trans, _dev) { \
130  uint8_t rc_settings_mode_none = 0; \
131  DOWNLINK_SEND_PPRZ_MODE(_trans, _dev, &pprz_mode, &v_ctl_mode, &lateral_mode, &horizontal_mode, &rc_settings_mode_none, &mcu1_status); \
132  }
133 #define PERIODIC_SEND_SETTINGS(_trans, _dev) {}
134 #endif
135 
136 #if USE_INFRARED || USE_INFRARED_TELEMETRY
138 #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);
139 #else
140 #define PERIODIC_SEND_IR_SENSORS(_trans, _dev) ;
141 #endif
142 
143 #define PERIODIC_SEND_ADC(_trans, _dev) {}
144 
145 
146 #define PERIODIC_SEND_CALIBRATION(_trans, _dev) DOWNLINK_SEND_CALIBRATION(_trans, _dev, &v_ctl_auto_throttle_sum_err, &v_ctl_auto_throttle_submode)
147 
148 #define PERIODIC_SEND_CIRCLE(_trans, _dev) if (nav_in_circle) { DOWNLINK_SEND_CIRCLE(_trans, _dev, &nav_circle_x, &nav_circle_y, &nav_circle_radius); }
149 
150 #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); }
151 
152 #ifdef IMU_TYPE_H
153 # ifdef INS_MODULE_H
154 # include "modules/ins/ins_module.h"
155 # define PERIODIC_SEND_IMU_ACCEL_RAW(_trans, _dev) {}
156 # define PERIODIC_SEND_IMU_GYRO_RAW(_trans, _dev) {}
157 # define PERIODIC_SEND_IMU_MAG_RAW(_trans, _dev) {}
158 # define PERIODIC_SEND_IMU_GYRO(_trans, _dev) { DOWNLINK_SEND_IMU_GYRO(_trans, _dev, &ins_p, &ins_q, &ins_r)}
159 # define PERIODIC_SEND_IMU_ACCEL(_trans, _dev) { DOWNLINK_SEND_IMU_ACCEL(_trans, _dev, &ins_ax, &ins_ay, &ins_az)}
160 # define PERIODIC_SEND_IMU_MAG(_trans, _dev) { DOWNLINK_SEND_IMU_MAG(_trans, _dev, &ins_mx, &ins_my, &ins_mz)}
161 # else
162 # include "subsystems/imu.h"
163 # 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)}
164 # 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)}
165 # 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)}
166 # 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)}
167 # 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)}
168 # ifdef USE_MAGNETOMETER
169 # 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)}
170 # else
171 # define PERIODIC_SEND_IMU_MAG(_trans, _dev) {}
172 # endif
173 # endif
174 #else
175 # define PERIODIC_SEND_IMU_ACCEL_RAW(_trans, _dev) {}
176 # define PERIODIC_SEND_IMU_GYRO_RAW(_trans, _dev) {}
177 # define PERIODIC_SEND_IMU_MAG_RAW(_trans, _dev) {}
178 # define PERIODIC_SEND_IMU_ACCEL(_trans, _dev) {}
179 # define PERIODIC_SEND_IMU_GYRO(_trans, _dev) {}
180 # define PERIODIC_SEND_IMU_MAG(_trans, _dev) {}
181 #endif
182 
183 #ifdef IMU_ANALOG
184 #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); }
185 #else
186 #define PERIODIC_SEND_IMU(_trans, _dev) {}
187 #endif
188 
189 #define PERIODIC_SEND_ESTIMATOR(_trans, _dev) DOWNLINK_SEND_ESTIMATOR(_trans, _dev, &estimator_z, &estimator_z_dot)
190 
191 #define SEND_NAVIGATION(_trans, _dev) Downlink({ uint8_t _circle_count = NavCircleCount(); DOWNLINK_SEND_NAVIGATION(_trans, _dev, &nav_block, &nav_stage, &estimator_x, &estimator_y, &dist2_to_wp, &dist2_to_home, &_circle_count, &nav_oval_count);})
192 
193 #define PERIODIC_SEND_NAVIGATION(_trans, _dev) SEND_NAVIGATION(_trans, _dev)
194 
195 
196 #if defined CAM || defined MOBILE_CAM
197 #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);})
198 #define PERIODIC_SEND_CAM_POINT(_trans, _dev) DOWNLINK_SEND_CAM_POINT(_trans, _dev, &cam_point_distance_from_home, &cam_point_lat, &cam_point_lon)
199 #else
200 #define SEND_CAM(_trans, _dev) {}
201 #define PERIODIC_SEND_CAM_POINT(_trans, _dev) {}
202 #endif
203 
204 #define PERIODIC_SEND_DL_VALUE(_trans, _dev) PeriodicSendDlValue(_trans, _dev)
206 #define PERIODIC_SEND_SURVEY(_trans, _dev) { \
207  if (nav_survey_active) \
208  DOWNLINK_SEND_SURVEY(_trans, _dev, &nav_survey_east, &nav_survey_north, &nav_survey_west, &nav_survey_south); \
209  }
210 
211 #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)
212 
213 #define PERIODIC_SEND_TUNE_ROLL(_trans, _dev) DOWNLINK_SEND_TUNE_ROLL(_trans, _dev, &estimator_p,&estimator_phi, &h_ctl_roll_setpoint);
214 
215 #if USE_GPS || USE_GPS_XSENS || defined SITL
216 #define PERIODIC_SEND_GPS_SOL(_trans, _dev) DOWNLINK_SEND_GPS_SOL(_trans, _dev, &gps.pacc, &gps.sacc, &gps.pdop, &gps.num_sv)
217 #endif
218 
219 #define PERIODIC_SEND_GPS(_trans, _dev) { \
220  static uint8_t i; \
221  int16_t climb = -gps.ned_vel.z; \
222  int16_t course = (DegOfRad(gps.course)/((int32_t)1e6)); \
223  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); \
224  if ((gps.fix != GPS_FIX_3D) && (i >= gps.nb_channels)) i = 0; \
225  if (i >= gps.nb_channels * 2) i = 0; \
226  if (i < gps.nb_channels && gps.svinfos[i].cno > 0) { \
227  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); \
228  } \
229  i++; \
230 }
231 
232 #if USE_GPS
233 #define PERIODIC_SEND_GPS_INT(_trans, _dev) { \
234  DOWNLINK_SEND_GPS_INT( _trans, _dev, \
235  &gps.ecef_pos.x, \
236  &gps.ecef_pos.y, \
237  &gps.ecef_pos.z, \
238  &gps.lla_pos.lat, \
239  &gps.lla_pos.lon, \
240  &gps.lla_pos.alt, \
241  &gps.hmsl, \
242  &gps.ecef_vel.x, \
243  &gps.ecef_vel.y, \
244  &gps.ecef_vel.z, \
245  &gps.pacc, \
246  &gps.sacc, \
247  &gps.tow, \
248  &gps.pdop, \
249  &gps.num_sv, \
250  &gps.fix); \
251  }
252 
253 #define PERIODIC_SEND_GPS_LLA(_trans, _dev) { \
254  uint8_t err = 0; \
255  int16_t climb = -gps.ned_vel.z; \
256  int16_t course = (DegOfRad(gps.course)/((int32_t)1e6)); \
257  DOWNLINK_SEND_GPS_LLA( _trans, _dev, \
258  &gps.lla_pos.lat, \
259  &gps.lla_pos.lon, \
260  &gps.lla_pos.alt, \
261  &course, \
262  &gps.gspeed, \
263  &climb, \
264  &gps.week, \
265  &gps.tow, \
266  &gps.fix, \
267  &err); \
268  }
269 #endif
270 
271 #if USE_BARO_MS5534A
272 //#include "baro_MS5534A.h"
273 #define PERIODIC_SEND_BARO_MS5534A(_trans, _dev) DOWNLINK_SEND_BARO_MS5534A(_trans, _dev, &baro_MS5534A_pressure, &baro_MS5534A_temp, &baro_MS5534A_z)
274 #else
275 #define PERIODIC_SEND_BARO_MS5534A(_trans, _dev) {}
276 #endif
277 
278 #if USE_BARO_SCP
279 #include "baro_scp.h"
280 #define PERIODIC_SEND_SCP_STATUS(_trans, _dev) DOWNLINK_SEND_SCP_STATUS(_trans, _dev, &baro_scp_pressure, &baro_scp_temperature)
281 #else
282 #define PERIODIC_SEND_SCP_STATUS(_trans, _dev) {}
283 #endif
284 
285 //FIXME: we need a better switch here...
286 #if BOARD_HAS_BARO && USE_BARO_AS_ALTIMETER
287 #include "subsystems/sensors/baro.h"
288 #define PERIODIC_SEND_BARO_RAW(_trans, _dev) { \
289  DOWNLINK_SEND_BARO_RAW(_trans, _dev, \
290  &baro.absolute, \
291  &baro.differential); \
292  }
293 #else
294 #define PERIODIC_SEND_BARO_RAW(_trans, _dev) {}
295 #endif
296 
297 #ifdef MEASURE_AIRSPEED
298 #define PERIODIC_SEND_AIRSPEED(_trans, _dev) DOWNLINK_SEND_AIRSPEED (_trans, _dev, &estimator_airspeed, &estimator_airspeed, &estimator_airspeed, &estimator_airspeed)
299 #elif USE_AIRSPEED
300 #define PERIODIC_SEND_AIRSPEED(_trans, _dev) DOWNLINK_SEND_AIRSPEED (_trans, _dev, &estimator_airspeed, &v_ctl_auto_airspeed_setpoint, &v_ctl_auto_airspeed_controlled, &v_ctl_auto_groundspeed_setpoint)
301 #else
302 #define PERIODIC_SEND_AIRSPEED(_trans, _dev) {}
303 #endif
304 
305 #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); })
306 
307 
309 #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)
310 
311 
312 #endif /* AP_DOWNLINK_H */
Variable setting though the radio control.
Inertial Measurement Unit interface.