diff --git a/conf/firmwares/subsystems/fixedwing/autopilot.makefile b/conf/firmwares/subsystems/fixedwing/autopilot.makefile index 3fef9171db..b9984a6ff1 100644 --- a/conf/firmwares/subsystems/fixedwing/autopilot.makefile +++ b/conf/firmwares/subsystems/fixedwing/autopilot.makefile @@ -115,6 +115,7 @@ endif # Main # ns_srcs += $(SRC_FIRMWARE)/main.c +ns_srcs += subsystems/datalink/telemetry.c # # LEDs @@ -156,6 +157,7 @@ fbw_srcs += $(SRC_FIRMWARE)/fbw_downlink.c ## ap_CFLAGS += -DAP +ap_CFLAGS += -DDefaultPeriodic='&telemetry_Ap' ap_srcs += $(SRC_FIRMWARE)/main_ap.c ap_srcs += $(SRC_FIRMWARE)/autopilot.c ap_srcs += $(SRC_FIRMWARE)/ap_downlink.c @@ -201,8 +203,9 @@ sim.srcs += $(fbw_srcs) $(ap_srcs) sim.CFLAGS += -DSITL sim.srcs += $(SRC_ARCH)/sim_ap.c -sim.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=IvyTransport -sim.srcs += subsystems/datalink/downlink.c $(SRC_FIRMWARE)/datalink.c $(SRC_ARCH)/sim_gps.c $(SRC_ARCH)/ivy_transport.c $(SRC_ARCH)/sim_adc_generic.c +sim.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=IvyTransport -DDefaultPeriodic='&telemetry_Ap' +sim.srcs += subsystems/datalink/telemetry.c subsystems/datalink/downlink.c $(SRC_FIRMWARE)/datalink.c +sim.srcs += $(SRC_ARCH)/sim_gps.c $(SRC_ARCH)/ivy_transport.c $(SRC_ARCH)/sim_adc_generic.c sim.srcs += subsystems/settings.c sim.srcs += $(SRC_ARCH)/subsystems/settings_arch.c diff --git a/conf/telemetry/default_fixedwing.xml b/conf/telemetry/default_fixedwing.xml index fcfa6320f3..65281893a3 100644 --- a/conf/telemetry/default_fixedwing.xml +++ b/conf/telemetry/default_fixedwing.xml @@ -14,8 +14,6 @@ - - diff --git a/conf/telemetry/default_fixedwing_imu.xml b/conf/telemetry/default_fixedwing_imu.xml index f7f832e34d..f295e16397 100644 --- a/conf/telemetry/default_fixedwing_imu.xml +++ b/conf/telemetry/default_fixedwing_imu.xml @@ -14,8 +14,6 @@ - - diff --git a/conf/telemetry/default_fixedwing_imu_9k6.xml b/conf/telemetry/default_fixedwing_imu_9k6.xml index e97b9046a9..a11857ca06 100644 --- a/conf/telemetry/default_fixedwing_imu_9k6.xml +++ b/conf/telemetry/default_fixedwing_imu_9k6.xml @@ -14,8 +14,6 @@ - - diff --git a/sw/airborne/firmwares/fixedwing/ap_downlink.h b/sw/airborne/firmwares/fixedwing/ap_downlink.h index c22d64d773..2bc4ad1ffe 100644 --- a/sw/airborne/firmwares/fixedwing/ap_downlink.h +++ b/sw/airborne/firmwares/fixedwing/ap_downlink.h @@ -1,5 +1,5 @@ /* - * Copyright (C) 2006- Pascal Brisset, Antoine Drouin + * Copyright (C) 2006-2013 Pascal Brisset, Antoine Drouin, Gautier Hattenberger * * This file is part of paparazzi. * @@ -22,300 +22,20 @@ /** * @file firmwares/fixedwing/ap_downlink.h * - * Set of macros defining the periodic telemetry messages of AP process. - * - * The PeriodicSendAp() macro is generated from the telemetry description - * (named in conf.xml, usually in conf/telemetry directory). This macro - * is a sequence of calls to PERIODIC_SEND_message() which have to be defined - * in the present file. + * The periodic function is generated from the telemetry xml file. + * Each subsystem of the autopilot can register function that + * will be called if needed. * */ #ifndef AP_DOWNLINK_H #define AP_DOWNLINK_H -#include - -#include "generated/airframe.h" -#include "state.h" - #ifndef DOWNLINK_DEVICE #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE #endif #include "subsystems/datalink/downlink.h" -#include "messages.h" #include "generated/periodic_telemetry.h" -#if defined DOWNLINK -#define Downlink(x) x -#else -#define Downlink(x) {} -#endif - -#define PERIODIC_SEND_ALIVE(_trans, _dev) DOWNLINK_SEND_ALIVE(_trans, _dev, 16, MD5SUM); - -#define PERIODIC_SEND_BAT(_trans, _dev) { \ - int16_t amps = (int16_t) (current/10); \ - Downlink({ int16_t e = energy; \ - DOWNLINK_SEND_BAT(_trans, _dev, \ - &v_ctl_throttle_slewed, \ - &vsupply, \ - &s, \ - &autopilot_flight_time, \ - &kill_throttle, \ - &block_time, \ - &stage_time, \ - &e); \ - }); \ - } - -#ifdef MCU_SPI_LINK -#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); -#else -#define PERIODIC_SEND_DEBUG_MCU_LINK(_trans, _dev) {} -#endif - - -#define PERIODIC_SEND_DOWNLINK(_trans, _dev) { \ - static uint16_t last; \ - uint16_t rate = (downlink_nb_bytes - last) / PERIOD_DOWNLINK_Ap_0; \ - last = downlink_nb_bytes; \ - DOWNLINK_SEND_DOWNLINK(_trans, _dev, &downlink_nb_ovrn, &rate, &downlink_nb_msgs); \ -} - - -#define PERIODIC_SEND_ATTITUDE(_trans, _dev) Downlink({ \ - struct FloatEulers* att = stateGetNedToBodyEulers_f(); \ - DOWNLINK_SEND_ATTITUDE(_trans, _dev, &(att->phi), &(att->psi), &(att->theta)); \ -}) - - -#ifndef USE_AIRSPEED -#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);} -#else -#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); -#endif - - -#if USE_INFRARED -#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); } -#elif USE_IMU && USE_AHRS -#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); } -#else -#define PERIODIC_SEND_STATE_FILTER_STATUS(_trans, _dev) {} -#endif - -#define PERIODIC_SEND_NAVIGATION_REF(_trans, _dev) DOWNLINK_SEND_NAVIGATION_REF(_trans, _dev, &nav_utm_east0, &nav_utm_north0, &nav_utm_zone0); - - -#define DownlinkSendWp(_trans, _dev, i) { \ - float x = nav_utm_east0 + waypoints[i].x; \ - float y = nav_utm_north0 + waypoints[i].y; \ - DOWNLINK_SEND_WP_MOVED(_trans, _dev, &i, &x, &y, &(waypoints[i].a),&nav_utm_zone0); \ -} - - -#define PERIODIC_SEND_WP_MOVED(_trans, _dev) { \ - static uint8_t i; \ - i++; if (i >= nb_waypoint) i = 0; \ - DownlinkSendWp(_trans, _dev, i); \ -} - -#if defined RADIO_CALIB && defined RADIO_CONTROL_SETTINGS -#include "rc_settings.h" -#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); -#define PERIODIC_SEND_SETTINGS(_trans, _dev) if (!RcSettingsOff()) DOWNLINK_SEND_SETTINGS(_trans, _dev, &slider_1_val, &slider_2_val); -#else -#define PERIODIC_SEND_PPRZ_MODE(_trans, _dev) { \ - uint8_t rc_settings_mode_none = 0; \ - DOWNLINK_SEND_PPRZ_MODE(_trans, _dev, &pprz_mode, &v_ctl_mode, &lateral_mode, &horizontal_mode, &rc_settings_mode_none, &mcu1_status); \ - } -#define PERIODIC_SEND_SETTINGS(_trans, _dev) {} -#endif - -#if USE_INFRARED || USE_INFRARED_TELEMETRY -#include "subsystems/sensors/infrared.h" -#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); -#else -#define PERIODIC_SEND_IR_SENSORS(_trans, _dev) ; -#endif - -#define PERIODIC_SEND_ADC(_trans, _dev) {} - - -#define PERIODIC_SEND_CALIBRATION(_trans, _dev) DOWNLINK_SEND_CALIBRATION(_trans, _dev, &v_ctl_auto_throttle_sum_err, &v_ctl_auto_throttle_submode) - -#define PERIODIC_SEND_CIRCLE(_trans, _dev) if (nav_in_circle) { DOWNLINK_SEND_CIRCLE(_trans, _dev, &nav_circle_x, &nav_circle_y, &nav_circle_radius); } - -#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); } - -#ifdef IMU_TYPE_H -# ifdef INS_MODULE_H -# include "modules/ins/ins_module.h" -# define PERIODIC_SEND_IMU_ACCEL_RAW(_trans, _dev) {} -# define PERIODIC_SEND_IMU_GYRO_RAW(_trans, _dev) {} -# define PERIODIC_SEND_IMU_MAG_RAW(_trans, _dev) {} -# define PERIODIC_SEND_IMU_GYRO(_trans, _dev) { DOWNLINK_SEND_IMU_GYRO(_trans, _dev, &ins_p, &ins_q, &ins_r)} -# define PERIODIC_SEND_IMU_ACCEL(_trans, _dev) { DOWNLINK_SEND_IMU_ACCEL(_trans, _dev, &ins_ax, &ins_ay, &ins_az)} -# define PERIODIC_SEND_IMU_MAG(_trans, _dev) { DOWNLINK_SEND_IMU_MAG(_trans, _dev, &ins_mx, &ins_my, &ins_mz)} -# else -# include "subsystems/imu.h" -# 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)} -# 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)} -# 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)} -# 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)} -# 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)} -# ifdef USE_MAGNETOMETER -# 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)} -# else -# define PERIODIC_SEND_IMU_MAG(_trans, _dev) {} -# endif -# endif -#else -# define PERIODIC_SEND_IMU_ACCEL_RAW(_trans, _dev) {} -# define PERIODIC_SEND_IMU_GYRO_RAW(_trans, _dev) {} -# define PERIODIC_SEND_IMU_MAG_RAW(_trans, _dev) {} -# define PERIODIC_SEND_IMU_ACCEL(_trans, _dev) {} -# define PERIODIC_SEND_IMU_GYRO(_trans, _dev) {} -# define PERIODIC_SEND_IMU_MAG(_trans, _dev) {} -#endif - -#ifdef IMU_ANALOG -#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); } -#else -#define PERIODIC_SEND_IMU(_trans, _dev) {} -#endif - -#define PERIODIC_SEND_ESTIMATOR(_trans, _dev) DOWNLINK_SEND_ESTIMATOR(_trans, _dev, &(stateGetPositionUtm_f()->alt), &(stateGetSpeedEnu_f()->z)) - -#define SEND_NAVIGATION(_trans, _dev) Downlink({ \ - uint8_t _circle_count = NavCircleCount(); \ - struct EnuCoor_f* pos = stateGetPositionEnu_f(); \ - DOWNLINK_SEND_NAVIGATION(_trans, _dev, &nav_block, &nav_stage, &(pos->x), &(pos->y), &dist2_to_wp, &dist2_to_home, &_circle_count, &nav_oval_count); \ - }) - -#define PERIODIC_SEND_NAVIGATION(_trans, _dev) SEND_NAVIGATION(_trans, _dev) - - -#if defined CAM || defined MOBILE_CAM -#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);}) -#define PERIODIC_SEND_CAM_POINT(_trans, _dev) DOWNLINK_SEND_CAM_POINT(_trans, _dev, &cam_point_distance_from_home, &cam_point_lat, &cam_point_lon) -#else -#define SEND_CAM(_trans, _dev) {} -#define PERIODIC_SEND_CAM_POINT(_trans, _dev) {} -#endif - -#define PERIODIC_SEND_DL_VALUE(_trans, _dev) PeriodicSendDlValue(_trans, _dev) /** generated from the xml settings config in conf/settings */ - -#define PERIODIC_SEND_SURVEY(_trans, _dev) { \ - if (nav_survey_active) \ - DOWNLINK_SEND_SURVEY(_trans, _dev, &nav_survey_east, &nav_survey_north, &nav_survey_west, &nav_survey_south); \ - } - -#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) - -#define PERIODIC_SEND_TUNE_ROLL(_trans, _dev) DOWNLINK_SEND_TUNE_ROLL(_trans, _dev, &(stateGetBodyRates_f()->p), &(stateGetNedToBodyEulers_f()->phi), &h_ctl_roll_setpoint); - -#if USE_GPS || defined SITL -#define PERIODIC_SEND_GPS_SOL(_trans, _dev) DOWNLINK_SEND_GPS_SOL(_trans, _dev, &gps.pacc, &gps.sacc, &gps.pdop, &gps.num_sv) -#else -#define PERIODIC_SEND_GPS_SOL(_trans, _dev) {} -#endif - -#define PERIODIC_SEND_GPS(_trans, _dev) { \ - static uint8_t i; \ - int16_t climb = -gps.ned_vel.z; \ - int16_t course = (DegOfRad(gps.course)/((int32_t)1e6)); \ - 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); \ - if ((gps.fix != GPS_FIX_3D) && (i >= gps.nb_channels)) i = 0; \ - if (i >= gps.nb_channels * 2) i = 0; \ - if (i < gps.nb_channels && ((gps.fix != GPS_FIX_3D) || (gps.svinfos[i].cno > 0))) { \ - 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); \ - } \ - i++; \ -} - -#if USE_GPS -#define PERIODIC_SEND_GPS_INT(_trans, _dev) { \ - DOWNLINK_SEND_GPS_INT( _trans, _dev, \ - &gps.ecef_pos.x, \ - &gps.ecef_pos.y, \ - &gps.ecef_pos.z, \ - &gps.lla_pos.lat, \ - &gps.lla_pos.lon, \ - &gps.lla_pos.alt, \ - &gps.hmsl, \ - &gps.ecef_vel.x, \ - &gps.ecef_vel.y, \ - &gps.ecef_vel.z, \ - &gps.pacc, \ - &gps.sacc, \ - &gps.tow, \ - &gps.pdop, \ - &gps.num_sv, \ - &gps.fix); \ - } - -#define PERIODIC_SEND_GPS_LLA(_trans, _dev) { \ - uint8_t err = 0; \ - int16_t climb = -gps.ned_vel.z; \ - int16_t course = (DegOfRad(gps.course)/((int32_t)1e6)); \ - DOWNLINK_SEND_GPS_LLA( _trans, _dev, \ - &gps.lla_pos.lat, \ - &gps.lla_pos.lon, \ - &gps.lla_pos.alt, \ - &course, \ - &gps.gspeed, \ - &climb, \ - &gps.week, \ - &gps.tow, \ - &gps.fix, \ - &err); \ - } -#endif - -#if USE_BARO_MS5534A -//#include "baro_MS5534A.h" -#define PERIODIC_SEND_BARO_MS5534A(_trans, _dev) DOWNLINK_SEND_BARO_MS5534A(_trans, _dev, &baro_MS5534A_pressure, &baro_MS5534A_temp, &baro_MS5534A_z) -#else -#define PERIODIC_SEND_BARO_MS5534A(_trans, _dev) {} -#endif - -#if USE_BARO_SCP -#include "baro_scp.h" -#define PERIODIC_SEND_SCP_STATUS(_trans, _dev) DOWNLINK_SEND_SCP_STATUS(_trans, _dev, &baro_scp_pressure, &baro_scp_temperature) -#else -#define PERIODIC_SEND_SCP_STATUS(_trans, _dev) {} -#endif - -#if USE_BAROMETER -#include "subsystems/sensors/baro.h" -#define PERIODIC_SEND_BARO_RAW(_trans, _dev) { \ - DOWNLINK_SEND_BARO_RAW(_trans, _dev, \ - &baro.absolute, \ - &baro.differential); \ - } -#else -#define PERIODIC_SEND_BARO_RAW(_trans, _dev) {} -#endif - -#ifdef MEASURE_AIRSPEED -#define PERIODIC_SEND_AIRSPEED(_trans, _dev) { \ - float* airspeed = stateGetAirspeed_f(); \ - DOWNLINK_SEND_AIRSPEED (_trans, _dev, airspeed, airspeed, airspeed, airspeed); \ -} -#elif USE_AIRSPEED -#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) -#else -#define PERIODIC_SEND_AIRSPEED(_trans, _dev) {} -#endif - -#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); }) - - -#include "firmwares/fixedwing/stabilization/stabilization_adaptive.h" -#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) - - #endif /* AP_DOWNLINK_H */ diff --git a/sw/airborne/firmwares/fixedwing/autopilot.c b/sw/airborne/firmwares/fixedwing/autopilot.c index eabae9d709..6c455381e4 100644 --- a/sw/airborne/firmwares/fixedwing/autopilot.c +++ b/sw/airborne/firmwares/fixedwing/autopilot.c @@ -28,8 +28,14 @@ #include "firmwares/fixedwing/autopilot.h" +#include "state.h" +#include "firmwares/fixedwing/ap_downlink.h" +#include "subsystems/nav.h" +#include "generated/settings.h" + uint8_t pprz_mode; bool_t kill_throttle; +uint8_t mcu1_status; bool_t launch; @@ -39,12 +45,105 @@ uint16_t autopilot_flight_time; uint8_t lateral_mode; uint16_t vsupply; +int32_t current; float energy; bool_t gps_lost; bool_t power_switch; +static void send_alive(void) { + DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM); +} + +#if defined RADIO_CALIB && defined RADIO_CONTROL_SETTINGS +#include "rc_settings.h" +static void send_rc_settings(void) { + if (!RcSettingsOff()) + DOWNLINK_SEND_SETTINGS(DefaultChannel, DefaultDevice, &slider_1_val, &slider_2_val); +} +#else +uint8_t rc_settings_mode = 0; +#endif + +static void send_mode(void) { + DOWNLINK_SEND_PPRZ_MODE(DefaultChannel, DefaultDevice, + &pprz_mode, &v_ctl_mode, &lateral_mode, &horizontal_mode, &rc_settings_mode, &mcu1_status); +} + +static void send_attitude(void) { + struct FloatEulers* att = stateGetNedToBodyEulers_f(); + DOWNLINK_SEND_ATTITUDE(DefaultChannel, DefaultDevice, + &(att->phi), &(att->psi), &(att->theta)); +}; + +static void send_estimator(void) { + DOWNLINK_SEND_ESTIMATOR(DefaultChannel, DefaultDevice, + &(stateGetPositionUtm_f()->alt), &(stateGetSpeedEnu_f()->z)); +} + +static void send_bat(void) { + int16_t amps = (int16_t) (current/10); + int16_t e = energy; + DOWNLINK_SEND_BAT(DefaultChannel, DefaultDevice, + &v_ctl_throttle_slewed, &vsupply, &s, + &autopilot_flight_time, &kill_throttle, + &block_time, &stage_time, &e); +} + +static void send_energy(void) { + 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(DefaultChannel, DefaultDevice, &vsup, &curs, &e, &power); +} + +static void send_dl_value(void) { + PeriodicSendDlValue(DefaultChannel, DefaultDevice); +} + +// FIXME not the best place +#include "firmwares/fixedwing/stabilization/stabilization_attitude.h" +#include CTRL_TYPE_H +static void send_desired(void) { +#ifndef USE_AIRSPEED + float v_ctl_auto_airspeed_setpoint = NOMINAL_AIRSPEED; +#endif + DOWNLINK_SEND_DESIRED(DefaultChannel, DefaultDevice, + &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); +} + +static void send_airspeed(void) { +#ifdef MEASURE_AIRSPEED + float* airspeed = stateGetAirspeed_f(); + DOWNLINK_SEND_AIRSPEED(DefaultChannel, DefaultDevice, airspeed, airspeed, airspeed, airspeed); +#elif USE_AIRSPEED + DOWNLINK_SEND_AIRSPEED(DefaultChannel, DefaultDevice, + stateGetAirspeed_f(), &v_ctl_auto_airspeed_setpoint, + &v_ctl_auto_airspeed_controlled, &v_ctl_auto_groundspeed_setpoint); +#endif +} + +static void send_downlink(void) { + static uint16_t last; + uint16_t rate = (downlink_nb_bytes - last) / PERIOD_DOWNLINK_Ap_0; + last = downlink_nb_bytes; + DOWNLINK_SEND_DOWNLINK(DefaultChannel, DefaultDevice, + &downlink_nb_ovrn, &rate, &downlink_nb_msgs); +} + +#if USE_BAROMETER +// FIXME we really need a baro subsystem +#include "subsystems/sensors/baro.h" +static void send_baro_raw(void) { + DOWNLINK_SEND_BARO_RAW(DefaultChannel, DefaultDevice, + &baro.absolute, &baro.differential); +} +#endif + void autopilot_init(void) { pprz_mode = PPRZ_MODE_AUTO2; kill_throttle = FALSE; @@ -57,5 +156,22 @@ void autopilot_init(void) { power_switch = FALSE; + /* register some periodic message */ + register_periodic_telemetry(DefaultPeriodic, "ALIVE", send_alive); + register_periodic_telemetry(DefaultPeriodic, "PPRZ_MODE", send_mode); + register_periodic_telemetry(DefaultPeriodic, "DOWNLINK", send_downlink); + register_periodic_telemetry(DefaultPeriodic, "ATTITUDE", send_attitude); + register_periodic_telemetry(DefaultPeriodic, "ESTIMATOR", send_estimator); + register_periodic_telemetry(DefaultPeriodic, "AIRSPEED", send_airspeed); + register_periodic_telemetry(DefaultPeriodic, "BAT", send_bat); + register_periodic_telemetry(DefaultPeriodic, "ENERGY", send_energy); + register_periodic_telemetry(DefaultPeriodic, "DL_VALUE", send_dl_value); + register_periodic_telemetry(DefaultPeriodic, "DESIRED", send_desired); +#if USE_BAROMETER + register_periodic_telemetry(DefaultPeriodic, "BARO_RAW", send_baro_raw); +#endif +#if defined RADIO_CALIB && defined RADIO_CONTROL_SETTINGS + register_periodic_telemetry(DefaultPeriodic, "RC_SETTINGS", send_rc_settings); +#endif } diff --git a/sw/airborne/firmwares/fixedwing/autopilot.h b/sw/airborne/firmwares/fixedwing/autopilot.h index 7e81247790..0e0ec52fb1 100644 --- a/sw/airborne/firmwares/fixedwing/autopilot.h +++ b/sw/airborne/firmwares/fixedwing/autopilot.h @@ -61,6 +61,7 @@ extern void autopilot_init(void); extern uint8_t pprz_mode; extern bool_t kill_throttle; +extern uint8_t mcu1_status; /** flight time in seconds. */ extern uint16_t autopilot_flight_time; @@ -88,6 +89,11 @@ extern uint8_t lateral_mode; */ extern uint16_t vsupply; +/** Supply current in milliAmpere. + * This the ap copy of the measurement from fbw + */ +extern int32_t current; // milliAmpere + /** Fuel consumption (mAh) * TODO: move to electrical subsystem */ diff --git a/sw/airborne/firmwares/fixedwing/datalink.c b/sw/airborne/firmwares/fixedwing/datalink.c index cd76c2aa39..0b627b3c11 100644 --- a/sw/airborne/firmwares/fixedwing/datalink.c +++ b/sw/airborne/firmwares/fixedwing/datalink.c @@ -140,7 +140,8 @@ void dl_parse_msg(void) { DOWNLINK_SEND_WP_MOVED(DefaultChannel, DefaultDevice, &wp_id, &utm.east, &utm.north, &a, &nav_utm_zone0); } else if (msg_id == DL_BLOCK && DL_BLOCK_ac_id(dl_buffer) == AC_ID) { nav_goto_block(DL_BLOCK_block_id(dl_buffer)); - SEND_NAVIGATION(DefaultChannel, DefaultDevice); + // FIXME + //SEND_NAVIGATION(DefaultChannel, DefaultDevice); } else #endif /** NAV */ #ifdef WIND_INFO diff --git a/sw/airborne/firmwares/fixedwing/fbw_downlink.h b/sw/airborne/firmwares/fixedwing/fbw_downlink.h index 061a7334ed..043d2af4a9 100644 --- a/sw/airborne/firmwares/fixedwing/fbw_downlink.h +++ b/sw/airborne/firmwares/fixedwing/fbw_downlink.h @@ -34,67 +34,14 @@ #ifndef FBW_DOWNLINK_H #define FBW_DOWNLINK_H -#include -#include "messages.h" -#include "generated/periodic_telemetry.h" -#include "generated/airframe.h" -#include "subsystems/commands.h" -#include "subsystems/actuators.h" - -#include "mcu_periph/uart.h" -#include "firmwares/fixedwing/main_fbw.h" -#include "subsystems/radio_control.h" -#include "subsystems/electrical.h" -#include "inter_mcu.h" - #ifndef DOWNLINK_DEVICE #define DOWNLINK_DEVICE DOWNLINK_FBW_DEVICE #endif #include "subsystems/datalink/downlink.h" - -#define PERIODIC_SEND_COMMANDS(_trans, _dev) DOWNLINK_SEND_COMMANDS(_trans, _dev, COMMANDS_NB, commands) - -#ifdef RADIO_CONTROL -#define PERIODIC_SEND_FBW_STATUS(_trans, _dev) DOWNLINK_SEND_FBW_STATUS(_trans, _dev, &(radio_control.status), &(radio_control.frame_rate), &fbw_mode, &electrical.vsupply, &electrical.current) -#ifdef RADIO_CONTROL_TYPE_PPM -#define PERIODIC_SEND_PPM(_trans, _dev) { \ - uint16_t ppm_pulses_usec[RADIO_CONTROL_NB_CHANNEL]; \ - for (int i=0;i 10) mde = 5; + uint16_t val = 0; + DOWNLINK_SEND_STATE_FILTER_STATUS(DefaultChannel, DefaultDevice, &mde, &val); +} + #endif // USE_AHRS && USE_IMU #if USE_GPS @@ -130,17 +139,10 @@ static inline void on_baro_dif_event( void ); // what version is this ???? static const uint16_t version = 1; -static uint8_t mcu1_status; - #if defined RADIO_CONTROL || defined RADIO_CONTROL_AUTO1 static uint8_t mcu1_ppm_cpt; #endif -/** Supply current in milliAmpere. - * This the ap copy of the measurement from fbw - */ -static int32_t current; // milliAmpere - tid_t modules_tid; ///< id for modules_periodic_task() timer tid_t telemetry_tid; ///< id for telemetry_periodic() timer @@ -172,6 +174,10 @@ void init_ap( void ) { ahrs_init(); #endif +#if USE_AHRS && USE_IMU + register_periodic_telemetry(DefaultPeriodic, "STATE_FILTER_STATUS", send_fliter_status); +#endif + #if USE_BAROMETER baro_init(); #endif @@ -228,6 +234,7 @@ void init_ap( void ) { #ifdef TRAFFIC_INFO traffic_info_init(); #endif + } @@ -352,8 +359,9 @@ static inline void telecommand_task( void ) { #endif } mode_changed |= mcu1_status_update(); - if ( mode_changed ) - PERIODIC_SEND_PPRZ_MODE(DefaultChannel, DefaultDevice); + // FIXME + //if ( mode_changed ) + // PERIODIC_SEND_PPRZ_MODE(DefaultChannel, DefaultDevice); #if defined RADIO_CONTROL || defined RADIO_CONTROL_AUTO1 /** In AUTO1 mode, compute roll setpoint and pitch setpoint from @@ -407,7 +415,8 @@ void reporting_task( void ) { } /** then report periodicly */ else { - PeriodicSendAp(DefaultChannel, DefaultDevice); + //PeriodicSendAp(DefaultChannel, DefaultDevice); + periodic_telemetry_send_Ap(); } } @@ -431,7 +440,8 @@ void navigation_task( void ) { if (pprz_mode == PPRZ_MODE_AUTO2 || pprz_mode == PPRZ_MODE_HOME) { last_pprz_mode = pprz_mode; pprz_mode = PPRZ_MODE_GPS_OUT_OF_ORDER; - PERIODIC_SEND_PPRZ_MODE(DefaultChannel, DefaultDevice); + // FIXME + //PERIODIC_SEND_PPRZ_MODE(DefaultChannel, DefaultDevice); gps_lost = TRUE; } } else if (gps_lost) { /* GPS is ok */ @@ -439,7 +449,8 @@ void navigation_task( void ) { pprz_mode = last_pprz_mode; gps_lost = FALSE; - PERIODIC_SEND_PPRZ_MODE(DefaultChannel, DefaultDevice); + // FIXME + //PERIODIC_SEND_PPRZ_MODE(DefaultChannel, DefaultDevice); } } #endif /* GPS && FAILSAFE_DELAY_WITHOUT_GPS */ @@ -456,11 +467,11 @@ void navigation_task( void ) { CallTCAS(); #endif -#ifndef PERIOD_NAVIGATION_0 // If not sent periodically (in default 0 mode) - SEND_NAVIGATION(DefaultChannel, DefaultDevice); -#endif +//#ifndef PERIOD_NAVIGATION_0 // If not sent periodically (in default 0 mode) +// SEND_NAVIGATION(DefaultChannel, DefaultDevice); +//#endif - SEND_CAM(DefaultChannel, DefaultDevice); + //SEND_CAM(DefaultChannel, DefaultDevice); /* The nav task computes only nav_altitude. However, we are interested by desired_altitude (= nav_alt+alt_shift) in any case. diff --git a/sw/airborne/firmwares/fixedwing/main_fbw.c b/sw/airborne/firmwares/fixedwing/main_fbw.c index ea95ecd386..aed2198fba 100644 --- a/sw/airborne/firmwares/fixedwing/main_fbw.c +++ b/sw/airborne/firmwares/fixedwing/main_fbw.c @@ -78,6 +78,35 @@ volatile uint8_t fbw_new_actuators = 0; tid_t fbw_periodic_tid; ///< id for periodic_task_fbw() timer tid_t electrical_tid; ///< id for electrical_periodic() timer +/********** PERIODIC MESSAGES ************************************************/ +static void send_commands(void) { + DOWNLINK_SEND_COMMANDS(DefaultChannel, DefaultDevice, COMMANDS_NB, commands); +} + +#ifdef RADIO_CONTROL +static void send_fbw_status(void) { + DOWNLINK_SEND_FBW_STATUS(DefaultChannel, DefaultDevice, + &(radio_control.status), &(radio_control.frame_rate), &fbw_mode, &electrical.vsupply, &electrical.current); +} + +static void send_rc(void) { + DOWNLINK_SEND_RC(DefaultChannel, DefaultDevice, RADIO_CONTROL_NB_CHANNEL, radio_control.values); +} + +#else +static void send_fbw_status(void) { + uint8_t dummy = 0; + DOWNLINK_SEND_FBW_STATUS(DefaultChannel, DefaultDevice, + &dummy, &dummy, &fbw_mode, &electrical.vsupply, &electrical.current); +} +#endif + +#ifdef ACTUATORS +static void send_actuators(void) { + DOWNLINK_SEND_ACTUATORS(DefaultChannel, DefaultDevice , ACTUATORS_NB, actuators); +} +#endif + /********** INIT *************************************************************/ void init_fbw( void ) { @@ -87,14 +116,19 @@ void init_fbw( void ) { electrical_init(); #endif + register_periodic_telemetry(&telemetry_Fbw, "FBW_STATUS", send_fbw_status); + register_periodic_telemetry(&telemetry_Fbw, "COMMANDS", send_commands); + #ifdef ACTUATORS actuators_init(); /* Load the failsafe defaults */ SetCommands(commands_failsafe); fbw_new_actuators = 1; + register_periodic_telemetry(&telemetry_Fbw, "ACTUATORS", send_actuators); #endif #ifdef RADIO_CONTROL radio_control_init(); + register_periodic_telemetry(&telemetry_Fbw, "RC", send_rc); #endif #ifdef INTER_MCU inter_mcu_init(); diff --git a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c index 65ac4683cd..c1ec18744a 100644 --- a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c +++ b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c @@ -36,6 +36,11 @@ #include CTRL_TYPE_H #include "firmwares/fixedwing/autopilot.h" +#ifndef DOWNLINK_DEVICE +#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE +#endif +#include "subsystems/datalink/downlink.h" +#include "generated/periodic_telemetry.h" /* outer loop parameters */ float h_ctl_course_setpoint; /* rad, CW/north */ @@ -144,6 +149,21 @@ inline static void h_ctl_pitch_loop( void ); #define H_CTL_PITCH_KFFD 0. #endif +static void send_calibration(void) { + DOWNLINK_SEND_CALIBRATION(DefaultChannel, DefaultDevice, &v_ctl_auto_throttle_sum_err, &v_ctl_auto_throttle_submode); +} + +static void send_tune_roll(void) { + DOWNLINK_SEND_TUNE_ROLL(DefaultChannel, DefaultDevice, + &(stateGetBodyRates_f()->p), &(stateGetNedToBodyEulers_f()->phi), &h_ctl_roll_setpoint); +} + +static void send_ctl_a(void) { + DOWNLINK_SEND_H_CTL_A(DefaultChannel, DefaultDevice, + &h_ctl_roll_sum_err, &h_ctl_ref_roll_angle, &h_ctl_pitch_sum_err, &h_ctl_ref_pitch_angle) +} + + void h_ctl_init( void ) { h_ctl_course_setpoint = 0.; h_ctl_course_pre_bank = 0.; @@ -191,6 +211,9 @@ void h_ctl_init( void ) { v_ctl_pitch_dash_trim = 0.; #endif + register_periodic_telemetry(DefaultPeriodic, "CALIBRATION", send_calibration); + register_periodic_telemetry(DefaultPeriodic, "TUNE_ROLL", send_tune_roll); + register_periodic_telemetry(DefaultPeriodic, "H_CTL_A", send_ctl_a); } /** diff --git a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c index 7d5a70f6ce..7fc2b91452 100644 --- a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c +++ b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c @@ -35,6 +35,12 @@ #include CTRL_TYPE_H #include "firmwares/fixedwing/autopilot.h" +#ifndef DOWNLINK_DEVICE +#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE +#endif +#include "subsystems/datalink/downlink.h" +#include "generated/periodic_telemetry.h" + /* outer loop parameters */ float h_ctl_course_setpoint; /* rad, CW/north */ float h_ctl_course_pre_bank; @@ -112,6 +118,10 @@ float h_ctl_roll_rate_gain; static float nav_ratio; #endif +static void send_calibration(void) { + DOWNLINK_SEND_CALIBRATION(DefaultChannel, DefaultDevice, &v_ctl_auto_throttle_sum_err, &v_ctl_auto_throttle_submode); +} + void h_ctl_init( void ) { h_ctl_course_setpoint = 0.; h_ctl_course_pre_bank = 0.; @@ -167,6 +177,8 @@ void h_ctl_init( void ) { #ifdef AGR_CLIMB nav_ratio=0; #endif + + register_periodic_telemetry(DefaultPeriodic, "CALIBRATION", send_calibration); } /** diff --git a/sw/airborne/link_mcu_spi.c b/sw/airborne/link_mcu_spi.c index 2c90e83444..e9b7284ab2 100644 --- a/sw/airborne/link_mcu_spi.c +++ b/sw/airborne/link_mcu_spi.c @@ -100,9 +100,21 @@ void link_mcu_event_task( void ) { #define LINK_MCU_SLAVE_IDX SPI_SLAVE0 #endif +#ifndef DOWNLINK_DEVICE +#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE +#endif +#include "subsystems/datalink/downlink.h" +#include "generated/periodic_telemetry.h" + uint8_t link_mcu_nb_err; uint8_t link_mcu_fbw_nb_err; +static void send_debug_link(void) { + uint8_t mcu1_ppm_cpt_foo = 0; //FIXME + DOWNLINK_SEND_DEBUG_MCU_LINK(DefaultChannel, DefaultDevice, + &link_mcu_nb_err, &link_mcu_fbw_nb_err, &mcu1_ppm_cpt_foo); +} + void link_mcu_init(void) { link_mcu_nb_err = 0; @@ -115,6 +127,8 @@ void link_mcu_init(void) { link_mcu_trans.output_buf = (uint8_t*)&link_mcu_from_ap_msg; link_mcu_trans.input_length = LINK_MCU_FRAME_LENGTH; link_mcu_trans.output_length = LINK_MCU_FRAME_LENGTH; + + register_periodic_telemetry(DefaultPeriodic, "DEBUG_MCU_LINK", send_debug_link); } void link_mcu_send(void) { diff --git a/sw/airborne/modules/cam_control/cam.c b/sw/airborne/modules/cam_control/cam.c index fafcc5d89a..4278ca5cca 100644 --- a/sw/airborne/modules/cam_control/cam.c +++ b/sw/airborne/modules/cam_control/cam.c @@ -35,6 +35,12 @@ #include "point.h" #endif // POINT_CAM +#ifndef DOWNLINK_DEVICE +#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE +#endif +#include "subsystems/datalink/downlink.h" +#include "generated/periodic_telemetry.h" + #ifdef TEST_CAM float test_cam_estimator_x; float test_cam_estimator_y; @@ -98,8 +104,21 @@ void cam_target(void); void cam_waypoint_target(void); void cam_ac_target(void); +static void send_cam(void) { + SEND_CAM(DefaultChannel, DefaultDevice); +} + +static void send_cam_point(void) { + DOWNLINK_SEND_CAM_POINT(DefaultChannel, DefaultDevice, + &cam_point_distance_from_home, &cam_point_lat, &cam_point_lon); +} + + void cam_init( void ) { cam_mode = CAM_MODE0; + + register_periodic_telemetry(DefaultPeriodic, "CAM", send_cam); + register_periodic_telemetry(DefaultPeriodic, "CAM_POINT", send_cam_point); } void cam_periodic( void ) { diff --git a/sw/airborne/modules/cam_control/cam.h b/sw/airborne/modules/cam_control/cam.h index 9517d0740a..ce75971d6d 100644 --- a/sw/airborne/modules/cam_control/cam.h +++ b/sw/airborne/modules/cam_control/cam.h @@ -96,4 +96,12 @@ extern bool_t video_tx_state; #endif +#define SEND_CAM(_trans, _dev) { \ + 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); \ +} + #endif // CAM_H diff --git a/sw/airborne/modules/sensors/baro_MS5534A.c b/sw/airborne/modules/sensors/baro_MS5534A.c index 6b605df2d3..c67ea4ec53 100644 --- a/sw/airborne/modules/sensors/baro_MS5534A.c +++ b/sw/airborne/modules/sensors/baro_MS5534A.c @@ -259,6 +259,9 @@ void baro_MS5534A_event( void ) { if (baro_MS5534A_available) { // baro_MS5534A_available = FALSE; // Checked by INS baro_MS5534A_z = ground_alt +((float)baro_MS5534A_ground_pressure - baro_MS5534A_pressure)*0.084; +#if SENSO_SYNC_SEND + DOWNLINK_SEND_BARO_MS5534A(DefaultChannel, DefaultDevice, &baro_MS5534A_pressure, &baro_MS5534A_temp, &baro_MS5534A_z); +#endif // if (alt_baro_enabled) { // EstimatorSetAlt(baro_MS5534A_z); // Updated by INS // } diff --git a/sw/airborne/subsystems/ahrs/ahrs_infrared.c b/sw/airborne/subsystems/ahrs/ahrs_infrared.c index 40128ed7fc..8212ba58ad 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_infrared.c +++ b/sw/airborne/subsystems/ahrs/ahrs_infrared.c @@ -37,12 +37,33 @@ #include "state.h" +#ifndef DOWNLINK_DEVICE +#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE +#endif +#include "subsystems/datalink/downlink.h" +#include "generated/periodic_telemetry.h" + float heading; +static void send_infrared(void) { + DOWNLINK_SEND_IR_SENSORS(DefaultChannel, DefaultDevice, + &infrared.value.ir1, &infrared.value.ir2, &infrared.pitch, &infrared.roll, &infrared.top); +} + +static void send_status(void) { + 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(DefaultChannel, DefaultDevice, &mde, &contrast); +} + void ahrs_init(void) { ahrs.status = AHRS_UNINIT; heading = 0.; + + register_periodic_telemetry(DefaultPeriodic, "IR_SENSORS", send_infrared); + register_periodic_telemetry(DefaultPeriodic, "STATE_FILTER_STATUS", send_status); } void ahrs_align(void) { diff --git a/sw/airborne/subsystems/datalink/telemetry.c b/sw/airborne/subsystems/datalink/telemetry.c new file mode 100644 index 0000000000..a275c25cc1 --- /dev/null +++ b/sw/airborne/subsystems/datalink/telemetry.c @@ -0,0 +1,47 @@ +/* + * Copyright (C) 2013 Gautier Hattenberger + * + * 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. + * + */ + +#include "subsystems/datalink/telemetry_common.h" + +//struct pprz_telemetry telemetry[PERIODIC_TELEMETRY_NB]; + +//void periodic_telemetry_init(void) { +// telemetry = PERIODIC_TELEMETRY_MESSAGES; +//} + +bool_t register_periodic_telemetry(struct pprz_telemetry * _pt, char * _msg, telemetry_cb _cb) { + // look for message name + uint8_t i; + for (i = 0; i < _pt->nb; i++) { + if (str_equal(_pt->msgs[i].msg, _msg)) { + // register callback if not already done + if (_pt->msgs[i].cb == NULL) { + _pt->msgs[i].cb = _cb; + return TRUE; + } + else { return FALSE; } + } + } + // message name is not in telemetry file + return FALSE; +} + diff --git a/sw/airborne/subsystems/datalink/telemetry_common.h b/sw/airborne/subsystems/datalink/telemetry_common.h new file mode 100644 index 0000000000..8fa7fccc8e --- /dev/null +++ b/sw/airborne/subsystems/datalink/telemetry_common.h @@ -0,0 +1,64 @@ +/* + * Copyright (C) 2013 Gautier Hattenberger + * + * 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. + * + */ + +/** @file subsystems/datalink/telemetry_common.h + * + * Common tools for periodic telemetry interface + * Allows subsystem to register callback functions + */ + +#ifndef TELEMETRY_COMMON_H +#define TELEMETRY_COMMON_H + +#include +#include "std.h" + +/** Telemetry callback definition */ +typedef void (*telemetry_cb)(void); + +/** Telemetry header + */ +struct telemetry_msg { + char msg[64]; ///< name in telemetry xml file + telemetry_cb cb; ///< callback funtion +}; + +struct pprz_telemetry { + uint8_t nb; ///< number of messages + struct telemetry_msg* msgs; ///< the list of (msg name, callbacks) +}; + +/** Telemetry init function + */ +//void periodic_telemetry_init(void); + +/** Register function + * @param _pt periodic telemetry structure to register + * @param _msg message name (string) as defined in telemetry xml file + * @param _cb callback function, called according to telemetry mode and specified period + * @return TRUE if message registered with success, FALSE otherwise + */ +bool_t register_periodic_telemetry(struct pprz_telemetry * _pt, char * _msg, telemetry_cb _cb); + + +#endif /* TELEMETRY_COMMON_H */ + diff --git a/sw/airborne/subsystems/gps.c b/sw/airborne/subsystems/gps.c index 67b98072b0..4d02ce83cc 100644 --- a/sw/airborne/subsystems/gps.c +++ b/sw/airborne/subsystems/gps.c @@ -26,6 +26,12 @@ #include "subsystems/gps.h" +#ifndef DOWNLINK_DEVICE +#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE +#endif +#include "subsystems/datalink/downlink.h" +#include "generated/periodic_telemetry.h" + #include "led.h" #define MSEC_PER_WEEK (1000*60*60*24*7) @@ -34,6 +40,56 @@ struct GpsState gps; struct GpsTimeSync gps_time_sync; +static void send_gps(void) { + static uint8_t i; + int16_t climb = -gps.ned_vel.z; + int16_t course = (DegOfRad(gps.course)/((int32_t)1e6)); + DOWNLINK_SEND_GPS(DefaultChannel, DefaultDevice, &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); + if ((gps.fix != GPS_FIX_3D) && (i >= gps.nb_channels)) i = 0; + if (i >= gps.nb_channels * 2) i = 0; + if (i < gps.nb_channels && ((gps.fix != GPS_FIX_3D) || (gps.svinfos[i].cno > 0))) { + 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); + } + i++; +} + +static void send_gps_int(void) { + DOWNLINK_SEND_GPS_INT(DefaultChannel, DefaultDevice, + &gps.ecef_pos.x, &gps.ecef_pos.y, &gps.ecef_pos.z, + &gps.lla_pos.lat, &gps.lla_pos.lon, &gps.lla_pos.alt, + &gps.hmsl, + &gps.ecef_vel.x, &gps.ecef_vel.y, &gps.ecef_vel.z, + &gps.pacc, &gps.sacc, + &gps.tow, + &gps.pdop, + &gps.num_sv, + &gps.fix); +} + +static void send_gps_lla(void) { + uint8_t err = 0; + int16_t climb = -gps.ned_vel.z; + int16_t course = (DegOfRad(gps.course)/((int32_t)1e6)); + DOWNLINK_SEND_GPS_LLA(DefaultChannel, DefaultDevice, + &gps.lla_pos.lat, &gps.lla_pos.lon, &gps.lla_pos.alt, + &course, &gps.gspeed, &climb, + &gps.week, &gps.tow, + &gps.fix, &err); +} + +static void send_gps_sol(void) { + DOWNLINK_SEND_GPS_SOL(DefaultChannel, DefaultDevice, &gps.pacc, &gps.sacc, &gps.pdop, &gps.num_sv); +} + void gps_init(void) { gps.fix = GPS_FIX_NONE; gps.cacc = 0; @@ -43,6 +99,11 @@ void gps_init(void) { #ifdef GPS_TYPE_H gps_impl_init(); #endif + + register_periodic_telemetry(DefaultPeriodic, "GPS", send_gps); + register_periodic_telemetry(DefaultPeriodic, "GPS_INT", send_gps_int); + register_periodic_telemetry(DefaultPeriodic, "GPS_LLA", send_gps_lla); + register_periodic_telemetry(DefaultPeriodic, "GPS_SOL", send_gps_sol); } uint32_t gps_tow_from_sys_ticks(uint32_t sys_ticks) diff --git a/sw/airborne/subsystems/imu.c b/sw/airborne/subsystems/imu.c index 74432019cb..fd65d967ae 100644 --- a/sw/airborne/subsystems/imu.c +++ b/sw/airborne/subsystems/imu.c @@ -26,6 +26,52 @@ #include "subsystems/imu.h" +#ifndef DOWNLINK_DEVICE +#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE +#endif +#include "subsystems/datalink/downlink.h" +#include "generated/periodic_telemetry.h" + +static void send_accel_raw(void) { + DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel, DefaultDevice, + &imu.accel_unscaled.x, &imu.accel_unscaled.y, &imu.accel_unscaled.z); +} + +static void send_gyro_raw(void) { + DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel, DefaultDevice, + &imu.gyro_unscaled.p, &imu.gyro_unscaled.q, &imu.gyro_unscaled.r); +} + +#ifdef USE_MAGNETOMETER +static void send_mag_raw(void) { + DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice, + &imu.mag_unscaled.x, &imu.mag_unscaled.y, &imu.mag_unscaled.z); +} +#endif + +static void send_accel(void) { + struct FloatVect3 accel_float; + ACCELS_FLOAT_OF_BFP(accel_float, imu.accel); + DOWNLINK_SEND_IMU_ACCEL(DefaultChannel, DefaultDevice, + &accel_float.x, &accel_float.y, &accel_float.z); +} + +static void send_gyro(void) { + struct FloatRates gyro_float; + RATES_FLOAT_OF_BFP(gyro_float, imu.gyro); + DOWNLINK_SEND_IMU_GYRO(DefaultChannel, DefaultDevice, + &gyro_float.p, &gyro_float.q, &gyro_float.r); +} + +#ifdef USE_MAGNETOMETER +static void send_mag(void) { + struct FloatVect3 mag_float; + MAGS_FLOAT_OF_BFP(mag_float, imu.mag); + DOWNLINK_SEND_IMU_MAG(DefaultChannel, DefaultDevice, + &mag_float.x, &mag_float.y, &mag_float.z); +} +#endif + struct Imu imu; void imu_init(void) { @@ -56,6 +102,17 @@ INFO("Magnetometer neutrals are set to zero, you should calibrate!") INT32_QUAT_NORMALIZE(imu.body_to_imu_quat); INT32_RMAT_OF_EULERS(imu.body_to_imu_rmat, body_to_imu_eulers); + register_periodic_telemetry(DefaultPeriodic, "IMU_ACCEL_RAW", send_accel_raw); + register_periodic_telemetry(DefaultPeriodic, "IMU_GYRO_RAW", send_gyro_raw); +#ifdef USE_MAGNETOMETER + register_periodic_telemetry(DefaultPeriodic, "IMU_MAG_RAW", send_mag_raw); +#endif + register_periodic_telemetry(DefaultPeriodic, "IMU_ACCEL", send_accel); + register_periodic_telemetry(DefaultPeriodic, "IMU_GYRO", send_gyro); +#ifdef USE_MAGNETOMETER + register_periodic_telemetry(DefaultPeriodic, "IMU_MAG", send_mag); +#endif + imu_impl_init(); } diff --git a/sw/airborne/subsystems/nav.c b/sw/airborne/subsystems/nav.c index bbd4021782..b3d07a2815 100644 --- a/sw/airborne/subsystems/nav.c +++ b/sw/airborne/subsystems/nav.c @@ -36,6 +36,12 @@ #include "inter_mcu.h" #include "subsystems/navigation/traffic_info.h" +#ifndef DOWNLINK_DEVICE +#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE +#endif +#include "subsystems/datalink/downlink.h" +#include "generated/periodic_telemetry.h" + #define RCLost() bit_is_set(fbw_state->status, RADIO_REALLY_LOST) enum oval_status oval_status; @@ -133,7 +139,7 @@ void nav_circle_XY(float x, float y, float radius) { (dist2_center > Square(abs_radius + dist_carrot) || dist2_center < Square(abs_radius - dist_carrot)) ? 0 : - atan((*stateGetHorizontalSpeedNorm_f())*(*stateGetHorizontalSpeedNorm_f()) / (G*radius)); + atan((*stateGetHorizontalSpeedNorm_f())*(*stateGetHorizontalSpeedNorm_f()) / (GRAVITY*radius)); float carrot_angle = dist_carrot / abs_radius; carrot_angle = Min(carrot_angle, M_PI/4); @@ -431,6 +437,44 @@ void nav_periodic_task(void) { nav_set_altitude(); } +/** + * \brief Periodic telemetry + */ +static void send_nav_ref(void) { + DOWNLINK_SEND_NAVIGATION_REF(DefaultChannel, DefaultDevice, + &nav_utm_east0, &nav_utm_north0, &nav_utm_zone0); +} + +static void send_nav(void) { + SEND_NAVIGATION(DefaultChannel, DefaultDevice); +} + +static void send_wp_moved(void) { + static uint8_t i; + i++; if (i >= nb_waypoint) i = 0; + DownlinkSendWp(DefaultChannel, DefaultDevice, i); +} + +static void send_circle(void) { + if (nav_in_circle) { + DOWNLINK_SEND_CIRCLE(DefaultChannel, DefaultDevice, + &nav_circle_x, &nav_circle_y, &nav_circle_radius); + } +} + +static void send_segment(void) { + if (nav_in_segment) { + DOWNLINK_SEND_SEGMENT(DefaultChannel, DefaultDevice, + &nav_segment_x_1, &nav_segment_y_1, &nav_segment_x_2, &nav_segment_y_2); + } +} + +static void send_survey(void) { + if (nav_survey_active) { + DOWNLINK_SEND_SURVEY(DefaultChannel, DefaultDevice, + &nav_survey_east, &nav_survey_north, &nav_survey_west, &nav_survey_south); + } +} /** * \brief Navigation Initialisation @@ -448,6 +492,13 @@ void nav_init(void) { nav_ground_speed_pgain = ABS(NAV_GROUND_SPEED_PGAIN); nav_ground_speed_setpoint = NOMINAL_AIRSPEED; #endif + + register_periodic_telemetry(DefaultPeriodic, "NAVIGATION_REF", send_nav_ref); + register_periodic_telemetry(DefaultPeriodic, "NAVIGATION", send_nav); + register_periodic_telemetry(DefaultPeriodic, "WP_MOVED", send_wp_moved); + register_periodic_telemetry(DefaultPeriodic, "CIRCLE", send_circle); + register_periodic_telemetry(DefaultPeriodic, "SEGMENT", send_segment); + register_periodic_telemetry(DefaultPeriodic, "SURVEY", send_survey); } /** diff --git a/sw/airborne/subsystems/nav.h b/sw/airborne/subsystems/nav.h index 8c80b68a31..14dd78a2ff 100644 --- a/sw/airborne/subsystems/nav.h +++ b/sw/airborne/subsystems/nav.h @@ -41,7 +41,7 @@ #include "subsystems/navigation/common_flight_plan.h" #include "subsystems/navigation/common_nav.h" -#define G 9.806 +#define GRAVITY 9.806 #define Square(_x) ((_x)*(_x)) #define DistanceSquare(p1_x, p1_y, p2_x, p2_y) (Square(p1_x-p2_x)+Square(p1_y-p2_y)) @@ -200,4 +200,16 @@ bool_t nav_approaching_xy(float x, float y, float from_x, float from_y, float ap #define GetPosY() (stateGetPositionUtm_f()->east) #define GetPosAlt() (stateGetPositionUtm_f()->alt) +#define SEND_NAVIGATION(_trans, _dev) { \ + uint8_t _circle_count = NavCircleCount(); \ + struct EnuCoor_f* pos = stateGetPositionEnu_f(); \ + DOWNLINK_SEND_NAVIGATION(_trans, _dev, &nav_block, &nav_stage, &(pos->x), &(pos->y), &dist2_to_wp, &dist2_to_home, &_circle_count, &nav_oval_count); \ +} + +#define DownlinkSendWp(_trans, _dev, i) { \ + float x = nav_utm_east0 + waypoints[i].x; \ + float y = nav_utm_north0 + waypoints[i].y; \ + DOWNLINK_SEND_WP_MOVED(_trans, _dev, &i, &x, &y, &(waypoints[i].a),&nav_utm_zone0); \ +} + #endif /* NAV_H */ diff --git a/sw/airborne/subsystems/radio_control/ppm.c b/sw/airborne/subsystems/radio_control/ppm.c index b21648e4d2..58d622b133 100644 --- a/sw/airborne/subsystems/radio_control/ppm.c +++ b/sw/airborne/subsystems/radio_control/ppm.c @@ -22,10 +22,32 @@ #include "subsystems/radio_control.h" #include "subsystems/radio_control/ppm.h" +#ifndef DOWNLINK_DEVICE +#define DOWNLINK_DEVICE DOWNLINK_FBW_DEVICE +#endif +#include "subsystems/datalink/downlink.h" + +#ifdef FBW +#define DOWNLINK_TELEMETRY &telemetry_Fbw +#else +#define DOWNLINK_TELEMETRY DefaultPeriodic +#endif +#include "generated/periodic_telemetry.h" + uint16_t ppm_pulses[ PPM_NB_CHANNEL ]; volatile bool_t ppm_frame_available; +static void send_ppm(void) { + uint16_t ppm_pulses_usec[RADIO_CONTROL_NB_CHANNEL]; + for (int i=0;i List.iter (fun mode -> let mode_name = ExtXml.attrib mode "name" in - lprintf out_h "if (telemetry_mode_%s == TELEMETRY_MODE_%s_%s) {\\\n" process_name process_name mode_name; + lprintf out_h "if (telemetry_mode_%s == TELEMETRY_MODE_%s_%s) {\n" process_name process_name mode_name; right (); (** Filter message list to remove messages linked to unloaded modules *) @@ -62,7 +62,7 @@ let output_modes = fun out_h process_name modes freq modules -> List.iter (fun m -> let v = sprintf "i%d" m in let _type = if m >= 256 then "uint16_t" else "uint8_t" in - lprintf out_h "static %s %s = 0; %s++; if (%s>=%d) %s=0;\\\n" _type v v v m v; + lprintf out_h "static %s %s = 0; %s++; if (%s>=%d) %s=0;\n" _type v v v m v; ) modulos; (** For each message in this mode *) @@ -78,17 +78,23 @@ let output_modes = fun out_h process_name modes freq modules -> let message_phase = try int_of_float (float_of_string (ExtXml.attrib message "phase")*.float_of_int freq) with _ -> !i in phase := message_phase; let else_ = if List.mem_assoc p !l && not (List.mem (p, !phase) !l) then "else " else "" in - lprintf out_h "%sif (i%d == %d) {\\\n" else_ p !phase; + lprintf out_h "%sif (i%d == %d) {\n" else_ p !phase; l := (p, !phase) :: !l; i := !i + freq/10; right (); - lprintf out_h "PERIODIC_SEND_%s(_trans, _dev);\\\n" message_name; + lprintf out_h "if (telemetry_%s.msgs[TELEMETRY_%s_MSG_%s_ID].cb != NULL)\n" process_name (String.uppercase process_name) message_name; + right (); + lprintf out_h "telemetry_%s.msgs[TELEMETRY_%s_MSG_%s_ID].cb();\n" process_name (String.uppercase process_name) message_name; left (); - lprintf out_h "} \\\n" + fprintf out_h "#if USE_PERIODIC_TELEMETRY_REPORT\n"; + lprintf out_h "else peridodic_telemetry_err_report(TELEMETRY_%s_MSG_%s_ID);\n" (String.uppercase process_name) message_name; + fprintf out_h "#endif\n"; + left (); + lprintf out_h "}\n" ) messages; left (); - lprintf out_h "}\\\n") + lprintf out_h "}\n") modes let write_settings = fun xml_file out_set telemetry_xml -> @@ -141,24 +147,18 @@ let _ = fprintf out_h "#ifndef _VAR_PERIODIC_H_\n"; fprintf out_h "#define _VAR_PERIODIC_H_\n\n"; fprintf out_h "#include \"std.h\"\n"; - fprintf out_h "#include \"generated/airframe.h\"\n\n"; + fprintf out_h "#include \"generated/airframe.h\"\n"; + fprintf out_h "#include \"subsystems/datalink/telemetry_common.h\"\n\n"; (** For each process *) List.iter (fun process -> let process_name = ExtXml.attrib process "name" in - fprintf out_h "\n/* Macros for %s process */\n" process_name; - fprintf out_h "#ifdef PERIODIC_C_%s\n" (String.uppercase process_name); - fprintf out_h "#ifndef TELEMETRY_MODE_%s\n" (String.uppercase process_name); - fprintf out_h "#define TELEMETRY_MODE_%s 0\n" (String.uppercase process_name); - fprintf out_h "#endif\n"; - fprintf out_h "uint8_t telemetry_mode_%s = TELEMETRY_MODE_%s;\n" process_name (String.uppercase process_name); - fprintf out_h "#else /* PERIODIC_C_%s not defined (general header) */\n" (String.uppercase process_name); - fprintf out_h "extern uint8_t telemetry_mode_%s;\n" process_name; - fprintf out_h "#endif /* PERIODIC_C_%s */\n" (String.uppercase process_name); - let modes = Xml.children process in + let messages = Hashtbl.create 5 in + + fprintf out_h "\n/* Periodic telemetry: %s process */\n" process_name; let i = ref 0 in (** For each mode of this process *) @@ -170,12 +170,41 @@ let _ = (fun x -> let p = ExtXml.attrib x "period" and n = ExtXml.attrib x "name" in - Xml2h.define (sprintf "PERIOD_%s_%s_%d" n process_name !i) (sprintf "(%s)" p)) + (* add message to the list if it exists *) + if not (Hashtbl.mem messages n) then Hashtbl.add messages n (); + Xml2h.define (sprintf "PERIOD_%s_%s_%d" n process_name !i) (sprintf "(%s)" p)) (* FIXME really needed ? *) (Xml.children mode); incr i) modes; - lprintf out_h "#define PeriodicSend%s(_trans, _dev) { /* %dHz */ \\\n" process_name freq; + let i = ref 0 in + (* Print message ID and total number *) + Hashtbl.iter (fun n _ -> + Xml2h.define (sprintf "TELEMETRY_%s_MSG_%s_ID" (String.uppercase process_name) n) (sprintf "%d" !i); + incr i; + ) messages; + Xml2h.define (sprintf "TELEMETRY_%s_NB_MSG" (String.uppercase process_name)) (sprintf "%d" !i); + + (* Structure initialization *) + fprintf out_h "#define TELEMETRY_%s_STRUCT { \\\n" (String.uppercase process_name); + Hashtbl.iter (fun n _ -> fprintf out_h " { \"%s\", NULL }, \\\n" n) messages; + fprintf out_h "};\n"; + + fprintf out_h "\n/* Functions for %s process */\n" process_name; + fprintf out_h "#ifdef PERIODIC_C_%s\n" (String.uppercase process_name); + fprintf out_h "#ifndef TELEMETRY_MODE_%s\n" (String.uppercase process_name); + fprintf out_h "#define TELEMETRY_MODE_%s 0\n" (String.uppercase process_name); + fprintf out_h "#endif\n"; + fprintf out_h "uint8_t telemetry_mode_%s = TELEMETRY_MODE_%s;\n" process_name (String.uppercase process_name); + fprintf out_h "struct telemetry_msg telemetry_msg_%s[TELEMETRY_%s_NB_MSG] = TELEMETRY_%s_STRUCT;\n" process_name (String.uppercase process_name) (String.uppercase process_name); + fprintf out_h "struct pprz_telemetry telemetry_%s = { TELEMETRY_%s_NB_MSG, telemetry_msg_%s };\n" process_name (String.uppercase process_name) process_name; + fprintf out_h "#else /* PERIODIC_C_%s not defined (general header) */\n" (String.uppercase process_name); + fprintf out_h "extern uint8_t telemetry_mode_%s;\n" process_name; + fprintf out_h "extern struct telemetry_msg telemetry_msg_%s[TELEMETRY_%s_NB_MSG];\n" process_name (String.uppercase process_name); + fprintf out_h "extern struct pprz_telemetry telemetry_%s;\n" process_name; + fprintf out_h "#endif /* PERIODIC_C_%s */\n" (String.uppercase process_name); + + lprintf out_h "static inline void periodic_telemetry_send_%s(void) { /* %dHz */\n" process_name freq; (*TODO pass transport+device *) right (); output_modes out_h process_name modes freq modules_name; left ();