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