diff --git a/sw/airborne/ap_downlink.h b/sw/airborne/ap_downlink.h new file mode 100644 index 0000000000..469b936a66 --- /dev/null +++ b/sw/airborne/ap_downlink.h @@ -0,0 +1,117 @@ +/* + * Paparazzi $Id$ + * + * Copyright (C) 2006- Pascal Brisset, Antoine Drouin + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + * + */ + +#ifndef AP_DOWNLINK_H +#define AP_DOWNLINK_H + +#include +#include "messages.h" +#include "airframe.h" + +#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE +#include "downlink.h" + +#ifdef SITL +#include "sitl_messages.h" +#else +#include "modem.h" +#include "messages.h" +#endif + +#if DOWNLINK +#define Downlink(x) x +#else +#define Downlink(x) {} +#endif + + +#define PERIODIC_SEND_IDENT() DOWNLINK_SEND_IDENT(&ac_ident); + +#define PERIODIC_SEND_BAT() Downlink({ int16_t e = energy; DOWNLINK_SEND_BAT(&desired_gaz, &vsupply, &estimator_flight_time, &low_battery, &block_time, &stage_time, &e); }) + +#ifdef MCU_SPI_LINK +#define PERIODIC_SEND_DEBUG_MCU_LINK() DOWNLINK_SEND_DEBUG_MCU_LINK(&link_fbw_nb_err, &link_fbw_fbw_nb_err, &mcu1_ppm_cpt); +#else +#define PERIODIC_SEND_DEBUG_MCU_LINK() {} +#endif + +#define PERIODIC_SEND_DEBUG_MODEM() DOWNLINK_SEND_DEBUG_MODEM(&modem_nb_ovrn) +#define PERIODIC_SEND_ATTITUDE() Downlink({ \ + int8_t phi = DegOfRad(estimator_phi); \ + int8_t psi = DegOfRad(estimator_psi); \ + int8_t theta = DegOfRad(estimator_theta); \ + DOWNLINK_SEND_ATTITUDE(&phi, &psi, &theta); \ +}) + +#define PERIODIC_SEND_PPRZ_MODE() DOWNLINK_SEND_PPRZ_MODE(&pprz_mode, &vertical_mode, &lateral_mode, &horizontal_mode, &inflight_calib_mode, &mcu1_status, &ir_estim_mode); +#define PERIODIC_SEND_DESIRED() DOWNLINK_SEND_DESIRED(&desired_roll, &desired_pitch, &desired_x, &desired_y, &desired_altitude, &desired_climb); + +#define PERIODIC_SEND_NAVIGATION_REF() DOWNLINK_SEND_NAVIGATION_REF(&nav_utm_east0, &nav_utm_north0, &nav_utm_zone0); + +#ifdef DATALINK +#define PERIODIC_SEND_ACINFO() { \ + struct ac_info_ *s=get_ac_info(3); \ + DOWNLINK_SEND_ACINFO(&s->east, &s->north, &s->course, &s->alt, &s->gspeed); \ +} +#else +#define PERIODIC_SEND_ACINFO() {} +#endif + +#ifdef COMMAND_CALIB +#define PERIODIC_SEND_SETTINGS() if (inflight_calib_mode != IF_CALIB_MODE_NONE) DOWNLINK_SEND_SETTINGS(&slider_1_val, &slider_2_val); +#else +#define PERIODIC_SEND_SETTINGS() {} +#endif + +#ifdef INFRARED +#define PERIODIC_SEND_ADC() DOWNLINK_SEND_ADC(&ir_roll, &ir_pitch); +#define SEND_RAD_OF_IR() Downlink({ int16_t rad = DeciDegOfRad(estimator_rad); DOWNLINK_SEND_RAD_OF_IR(&ir_roll, &rad, &estimator_rad_of_ir);}) +#define PERIODIC_SEND_CALIB_START() if (!estimator_flight_time && calib_status == WAITING_CALIB_CONTRAST) { DOWNLINK_SEND_CALIB_START(); } +#define PERIODIC_SEND_CALIB_CONTRAST() if (!estimator_flight_time && calib_status == CALIB_DONE) { DOWNLINK_SEND_CALIB_CONTRAST(&ir_contrast); } +#else +#define PERIODIC_SEND_ADC() {} +#define SEND_RAD_OF_IR() {} +#define PERIODIC_SEND_CALIB_START() {} +#define PERIODIC_SEND_CALIB_CONTRAST() {} +#endif + +#define PERIODIC_SEND_CALIBRATION() DOWNLINK_SEND_CALIBRATION(&climb_sum_err, &climb_pgain, &course_pgain) + +#define PERIODIC_SEND_CIRCLE() if (in_circle) { DOWNLINK_SEND_CIRCLE(&circle_x, &circle_y, &circle_radius); } + +#define PERIODIC_SEND_SEGMENT() if (in_segment) { DOWNLINK_SEND_SEGMENT(&segment_x_1, &segment_y_1, &segment_x_2, &segment_y_2); } + +#ifdef IMU_ANALOG +#define PERIODIC_SEND_IMU() { int16_t dummy = 42; DOWNLINK_SEND_IMU(&(from_fbw.euler_dot[0]), &(from_fbw.euler_dot[1]), &(from_fbw.euler_dot[2]), &dummy, &dummy, &dummy); } +#else +#define PERIODIC_SEND_IMU() {} +#endif + +#define SEND_NAVIGATION() Downlink({ int16_t pos_x = estimator_x; int16_t pos_y = estimator_y; int16_t d_course = DeciDegOfRad(desired_course); DOWNLINK_SEND_NAVIGATION(&nav_block, &nav_stage, &pos_x, &pos_y, &d_course, &dist2_to_wp, &dist2_to_home);}) + +#define SEND_CAM() Downlink({ int16_t x = target_x; int16_t y = target_y; int8_t phi = DegOfRad(phi_c); int8_t theta = DegOfRad(theta_c); DOWNLINK_SEND_CAM(&phi, &theta, &x, &y);}) + +#define PERIODIC_SEND_DL_VALUE() PeriodicSendDlValue() /** from flight_plan.h*/ + +#endif /* AP_DOWNLINK_H */ diff --git a/sw/airborne/avr/link_mcu_ap.c b/sw/airborne/avr/link_mcu_ap.c index 40c76d00b6..8de34c91a9 100644 --- a/sw/airborne/avr/link_mcu_ap.c +++ b/sw/airborne/avr/link_mcu_ap.c @@ -23,6 +23,7 @@ */ #include +#include #include //#include #include diff --git a/sw/airborne/avr/modem_hw.c b/sw/airborne/avr/modem_hw.c index 647f40fc3b..86bb805d2f 100644 --- a/sw/airborne/avr/modem_hw.c +++ b/sw/airborne/avr/modem_hw.c @@ -28,6 +28,7 @@ #include #include +#include #include #include #include "modem.h" diff --git a/sw/airborne/avr/spi_ap.c b/sw/airborne/avr/spi_ap.c index 76462a9ea8..9bc6950a6d 100644 --- a/sw/airborne/avr/spi_ap.c +++ b/sw/airborne/avr/spi_ap.c @@ -24,6 +24,7 @@ #include #include +#include #include diff --git a/sw/airborne/downlink.c b/sw/airborne/downlink.c new file mode 100644 index 0000000000..c406a22c5a --- /dev/null +++ b/sw/airborne/downlink.c @@ -0,0 +1,4 @@ +#include "ap_downlink.h" + +uint8_t ck_a, ck_b; +uint8_t downlink_nb_ovrn;