[telemetry] convert macros in registered callback to functions

This commit is contained in:
Gautier Hattenberger
2014-11-04 23:56:55 +01:00
parent c4b73552cd
commit e5cdfa1ce1
49 changed files with 171 additions and 166 deletions
+4 -4
View File
@@ -121,8 +121,8 @@ static void navdata_write(const uint8_t *buf, size_t count)
#if PERIODIC_TELEMETRY #if PERIODIC_TELEMETRY
#include "subsystems/datalink/telemetry.h" #include "subsystems/datalink/telemetry.h"
static void send_navdata(void) { static void send_navdata(struct transport_tx *trans, struct device *dev) {
DOWNLINK_SEND_ARDRONE_NAVDATA(DefaultChannel, DefaultDevice, pprz_msg_send_ARDRONE_NAVDATA(trans, dev, AC_ID,
&navdata.taille, &navdata.taille,
&navdata.nu_trame, &navdata.nu_trame,
&navdata.ax, &navdata.ax,
@@ -159,7 +159,7 @@ static void send_fliter_status(void) {
if (ahrs.status == AHRS_UNINIT) mde = 2; if (ahrs.status == AHRS_UNINIT) mde = 2;
if (imu_lost) mde = 5; if (imu_lost) mde = 5;
uint16_t val = 0; uint16_t val = 0;
DOWNLINK_SEND_STATE_FILTER_STATUS(DefaultChannel, DefaultDevice, &mde, &val); pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &mde, &val);
} }
#endif #endif
@@ -324,7 +324,7 @@ static void mag_freeze_check(void) {
uint8_t mde = 5; uint8_t mde = 5;
uint16_t val = 0; uint16_t val = 0;
DOWNLINK_SEND_STATE_FILTER_STATUS(DefaultChannel, DefaultDevice, &mde, &val); pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &mde, &val);
// stop acquisition // stop acquisition
uint8_t cmd=0x02; uint8_t cmd=0x02;
+20 -15
View File
@@ -57,39 +57,44 @@ bool_t gps_lost;
bool_t power_switch; bool_t power_switch;
static void send_alive(struct transport_tx *trans, struct device *dev) { static void send_alive(struct transport_tx *trans, struct device *dev) {
DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM); pprz_msg_send_ALIVE(trans, dev, AC_ID, 16, MD5SUM);
} }
#if defined RADIO_CALIB && defined RADIO_CONTROL_SETTINGS #if defined RADIO_CALIB && defined RADIO_CONTROL_SETTINGS
#include "rc_settings.h" #include "rc_settings.h"
static void send_rc_settings(struct transport_tx *trans, struct device *dev) { static void send_rc_settings(struct transport_tx *trans, struct device *dev) {
if (!RcSettingsOff()) if (!RcSettingsOff())
DOWNLINK_SEND_SETTINGS(DefaultChannel, DefaultDevice, &slider_1_val, &slider_2_val); pprz_msg_send_SETTINGS(trans, dev, AC_ID, &slider_1_val, &slider_2_val);
} }
#else #else
uint8_t rc_settings_mode = 0; uint8_t rc_settings_mode = 0;
#endif #endif
void autopilot_send_mode(void) { static void send_mode(struct transport_tx *trans, struct device *dev) {
DOWNLINK_SEND_PPRZ_MODE(DefaultChannel, DefaultDevice, pprz_msg_send_PPRZ_MODE(trans, dev, AC_ID,
&pprz_mode, &v_ctl_mode, &lateral_mode, &horizontal_mode, &rc_settings_mode, &mcu1_status); &pprz_mode, &v_ctl_mode, &lateral_mode, &horizontal_mode, &rc_settings_mode, &mcu1_status);
} }
void autopilot_send_mode(void) {
// use default telemetry here
send_mode(&(DefaultChannel).trans_tx, &(DefaultDevice).device);
}
static void send_attitude(struct transport_tx *trans, struct device *dev) { static void send_attitude(struct transport_tx *trans, struct device *dev) {
struct FloatEulers* att = stateGetNedToBodyEulers_f(); struct FloatEulers* att = stateGetNedToBodyEulers_f();
DOWNLINK_SEND_ATTITUDE(DefaultChannel, DefaultDevice, pprz_msg_send_ATTITUDE(trans, dev, AC_ID,
&(att->phi), &(att->psi), &(att->theta)); &(att->phi), &(att->psi), &(att->theta));
}; };
static void send_estimator(struct transport_tx *trans, struct device *dev) { static void send_estimator(struct transport_tx *trans, struct device *dev) {
DOWNLINK_SEND_ESTIMATOR(DefaultChannel, DefaultDevice, pprz_msg_send_ESTIMATOR(trans, dev, AC_ID,
&(stateGetPositionUtm_f()->alt), &(stateGetSpeedEnu_f()->z)); &(stateGetPositionUtm_f()->alt), &(stateGetSpeedEnu_f()->z));
} }
static void send_bat(struct transport_tx *trans, struct device *dev) { static void send_bat(struct transport_tx *trans, struct device *dev) {
int16_t amps = (int16_t) (current/10); int16_t amps = (int16_t) (current/10);
int16_t e = energy; int16_t e = energy;
DOWNLINK_SEND_BAT(DefaultChannel, DefaultDevice, pprz_msg_send_BAT(trans, dev, AC_ID,
&v_ctl_throttle_slewed, &vsupply, &amps, &v_ctl_throttle_slewed, &vsupply, &amps,
&autopilot_flight_time, (uint8_t*)(&kill_throttle), &autopilot_flight_time, (uint8_t*)(&kill_throttle),
&block_time, &stage_time, &e); &block_time, &stage_time, &e);
@@ -100,11 +105,11 @@ static void send_energy(struct transport_tx *trans, struct device *dev) {
float vsup = ((float)vsupply) / 10.0f; float vsup = ((float)vsupply) / 10.0f;
float curs = ((float)current) / 1000.0f; float curs = ((float)current) / 1000.0f;
float power = vsup * curs; float power = vsup * curs;
DOWNLINK_SEND_ENERGY(DefaultChannel, DefaultDevice, &vsup, &curs, &e, &power); pprz_msg_send_ENERGY(trans, dev, AC_ID, &vsup, &curs, &e, &power);
} }
static void send_dl_value(struct transport_tx *trans, struct device *dev) { static void send_dl_value(struct transport_tx *trans, struct device *dev) {
PeriodicSendDlValue(DefaultChannel, DefaultDevice); PeriodicSendDlValue(trans, dev);
} }
// FIXME not the best place // FIXME not the best place
@@ -114,18 +119,18 @@ static void send_desired(struct transport_tx *trans, struct device *dev) {
#ifndef USE_AIRSPEED #ifndef USE_AIRSPEED
float v_ctl_auto_airspeed_setpoint = NOMINAL_AIRSPEED; float v_ctl_auto_airspeed_setpoint = NOMINAL_AIRSPEED;
#endif #endif
DOWNLINK_SEND_DESIRED(DefaultChannel, DefaultDevice, pprz_msg_send_DESIRED(trans, dev, AC_ID,
&h_ctl_roll_setpoint, &h_ctl_pitch_loop_setpoint, &h_ctl_course_setpoint, &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, &desired_x, &desired_y, &v_ctl_altitude_setpoint, &v_ctl_climb_setpoint,
&v_ctl_auto_airspeed_setpoint); &v_ctl_auto_airspeed_setpoint);
} }
static void send_airspeed(struct transport_tx *trans, struct device *dev) { static void send_airspeed(struct transport_tx *trans __attribute__((unused)), struct device *dev __attribute__((unused))) {
#ifdef MEASURE_AIRSPEED #ifdef MEASURE_AIRSPEED
float* airspeed = stateGetAirspeed_f(); float* airspeed = stateGetAirspeed_f();
DOWNLINK_SEND_AIRSPEED(DefaultChannel, DefaultDevice, airspeed, airspeed, airspeed, airspeed); pprz_msg_send_AIRSPEED(trans, dev, AC_ID, airspeed, airspeed, airspeed, airspeed);
#elif USE_AIRSPEED #elif USE_AIRSPEED
DOWNLINK_SEND_AIRSPEED(DefaultChannel, DefaultDevice, pprz_msg_send_AIRSPEED(trans, dev, AC_ID,
stateGetAirspeed_f(), &v_ctl_auto_airspeed_setpoint, stateGetAirspeed_f(), &v_ctl_auto_airspeed_setpoint,
&v_ctl_auto_airspeed_controlled, &v_ctl_auto_groundspeed_setpoint); &v_ctl_auto_airspeed_controlled, &v_ctl_auto_groundspeed_setpoint);
#endif #endif
@@ -135,7 +140,7 @@ static void send_downlink(struct transport_tx *trans, struct device *dev) {
static uint16_t last; static uint16_t last;
uint16_t rate = (downlink_nb_bytes - last) / PERIOD_DOWNLINK_Ap_0; uint16_t rate = (downlink_nb_bytes - last) / PERIOD_DOWNLINK_Ap_0;
last = downlink_nb_bytes; last = downlink_nb_bytes;
DOWNLINK_SEND_DOWNLINK(DefaultChannel, DefaultDevice, pprz_msg_send_DOWNLINK(trans, dev, AC_ID,
&downlink_nb_ovrn, &rate, &downlink_nb_msgs); &downlink_nb_ovrn, &rate, &downlink_nb_msgs);
} }
@@ -157,7 +162,7 @@ void autopilot_init(void) {
/* register some periodic message */ /* register some periodic message */
register_periodic_telemetry(DefaultPeriodic, "ALIVE", send_alive); register_periodic_telemetry(DefaultPeriodic, "ALIVE", send_alive);
register_periodic_telemetry(DefaultPeriodic, "PPRZ_MODE", autopilot_send_mode); register_periodic_telemetry(DefaultPeriodic, "PPRZ_MODE", send_mode);
register_periodic_telemetry(DefaultPeriodic, "DOWNLINK", send_downlink); register_periodic_telemetry(DefaultPeriodic, "DOWNLINK", send_downlink);
register_periodic_telemetry(DefaultPeriodic, "ATTITUDE", send_attitude); register_periodic_telemetry(DefaultPeriodic, "ATTITUDE", send_attitude);
register_periodic_telemetry(DefaultPeriodic, "ESTIMATOR", send_estimator); register_periodic_telemetry(DefaultPeriodic, "ESTIMATOR", send_estimator);
+2 -2
View File
@@ -148,7 +148,7 @@ static void send_filter_status(struct transport_tx *trans, struct device *dev) {
if (ahrs.status == AHRS_UNINIT) mde = 2; if (ahrs.status == AHRS_UNINIT) mde = 2;
if (ahrs_timeout_counter > 10) mde = 5; if (ahrs_timeout_counter > 10) mde = 5;
uint16_t val = 0; uint16_t val = 0;
DOWNLINK_SEND_STATE_FILTER_STATUS(DefaultChannel, DefaultDevice, &mde, &val); pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &mde, &val);
} }
#endif // USE_AHRS && USE_IMU #endif // USE_AHRS && USE_IMU
@@ -460,7 +460,7 @@ void reporting_task( void ) {
/** then report periodicly */ /** then report periodicly */
else { else {
//PeriodicSendAp(DefaultChannel, DefaultDevice); //PeriodicSendAp(DefaultChannel, DefaultDevice);
periodic_telemetry_send_Ap((void*)(DefaultChannel), (void*)(DefaultDevice)); periodic_telemetry_send_Ap(&(DefaultChannel).trans_tx, &(DefaultDevice).device);
} }
} }
+6 -6
View File
@@ -77,30 +77,30 @@ tid_t electrical_tid; ///< id for electrical_periodic() timer
/********** PERIODIC MESSAGES ************************************************/ /********** PERIODIC MESSAGES ************************************************/
#if PERIODIC_TELEMETRY #if PERIODIC_TELEMETRY
static void send_commands(struct transport_tx *trans, struct device *dev) { static void send_commands(struct transport_tx *trans, struct device *dev) {
DOWNLINK_SEND_COMMANDS(DefaultChannel, DefaultDevice, COMMANDS_NB, commands); pprz_msg_send_COMMANDS(trans, dev, AC_ID, COMMANDS_NB, commands);
} }
#ifdef RADIO_CONTROL #ifdef RADIO_CONTROL
static void send_fbw_status(struct transport_tx *trans, struct device *dev) { static void send_fbw_status(struct transport_tx *trans, struct device *dev) {
DOWNLINK_SEND_FBW_STATUS(DefaultChannel, DefaultDevice, pprz_msg_send_FBW_STATUS(trans, dev, AC_ID,
&(radio_control.status), &(radio_control.frame_rate), &fbw_mode, &electrical.vsupply, &electrical.current); &(radio_control.status), &(radio_control.frame_rate), &fbw_mode, &electrical.vsupply, &electrical.current);
} }
static void send_rc(struct transport_tx *trans, struct device *dev) { static void send_rc(struct transport_tx *trans, struct device *dev) {
DOWNLINK_SEND_RC(DefaultChannel, DefaultDevice, RADIO_CONTROL_NB_CHANNEL, radio_control.values); pprz_msg_send_RC(trans, dev, AC_ID, RADIO_CONTROL_NB_CHANNEL, radio_control.values);
} }
#else #else
static void send_fbw_status(struct transport_tx *trans, struct device *dev) { static void send_fbw_status(struct transport_tx *trans, struct device *dev) {
uint8_t dummy = 0; uint8_t dummy = 0;
DOWNLINK_SEND_FBW_STATUS(DefaultChannel, DefaultDevice, pprz_msg_send_FBW_STATUS(trans, dev, AC_ID,
&dummy, &dummy, &fbw_mode, &electrical.vsupply, &electrical.current); &dummy, &dummy, &fbw_mode, &electrical.vsupply, &electrical.current);
} }
#endif #endif
#ifdef ACTUATORS #ifdef ACTUATORS
static void send_actuators(struct transport_tx *trans, struct device *dev) { static void send_actuators(struct transport_tx *trans, struct device *dev) {
DOWNLINK_SEND_ACTUATORS(DefaultChannel, DefaultDevice , ACTUATORS_NB, actuators); pprz_msg_send_ACTUATORS(trans, dev, AC_ID , ACTUATORS_NB, actuators);
} }
#endif #endif
@@ -335,7 +335,7 @@ void periodic_task_fbw( void ) {
#endif #endif
#if PERIODIC_TELEMETRY #if PERIODIC_TELEMETRY
periodic_telemetry_send_Fbw((void*)(DefaultChannel), (void*)(DefaultDevice)); periodic_telemetry_send_Fbw(&(DefaultChannel).trans_tx, &(DefaultDevice).device);
#endif #endif
} }
+4 -4
View File
@@ -452,7 +452,7 @@ void nav_periodic_task(void) {
#include "subsystems/datalink/telemetry.h" #include "subsystems/datalink/telemetry.h"
static void send_nav_ref(struct transport_tx *trans, struct device *dev) { static void send_nav_ref(struct transport_tx *trans, struct device *dev) {
DOWNLINK_SEND_NAVIGATION_REF(DefaultChannel, DefaultDevice, pprz_msg_send_NAVIGATION_REF(trans, dev, AC_ID,
&nav_utm_east0, &nav_utm_north0, &nav_utm_zone0, &ground_alt); &nav_utm_east0, &nav_utm_north0, &nav_utm_zone0, &ground_alt);
} }
@@ -475,21 +475,21 @@ bool_t DownlinkSendWpNr(uint8_t _wp)
static void send_circle(struct transport_tx *trans, struct device *dev) { static void send_circle(struct transport_tx *trans, struct device *dev) {
if (nav_in_circle) { if (nav_in_circle) {
DOWNLINK_SEND_CIRCLE(DefaultChannel, DefaultDevice, pprz_msg_send_CIRCLE(trans, dev, AC_ID,
&nav_circle_x, &nav_circle_y, &nav_circle_radius); &nav_circle_x, &nav_circle_y, &nav_circle_radius);
} }
} }
static void send_segment(struct transport_tx *trans, struct device *dev) { static void send_segment(struct transport_tx *trans, struct device *dev) {
if (nav_in_segment) { if (nav_in_segment) {
DOWNLINK_SEND_SEGMENT(DefaultChannel, DefaultDevice, pprz_msg_send_SEGMENT(trans, dev, AC_ID,
&nav_segment_x_1, &nav_segment_y_1, &nav_segment_x_2, &nav_segment_y_2); &nav_segment_x_1, &nav_segment_y_1, &nav_segment_x_2, &nav_segment_y_2);
} }
} }
static void send_survey(struct transport_tx *trans, struct device *dev) { static void send_survey(struct transport_tx *trans, struct device *dev) {
if (nav_survey_active) { if (nav_survey_active) {
DOWNLINK_SEND_SURVEY(DefaultChannel, DefaultDevice, pprz_msg_send_SURVEY(trans, dev, AC_ID,
&nav_survey_east, &nav_survey_north, &nav_survey_west, &nav_survey_south); &nav_survey_east, &nav_survey_north, &nav_survey_west, &nav_survey_south);
} }
} }
@@ -193,16 +193,16 @@ inline static void h_ctl_pitch_loop( void );
#include "subsystems/datalink/telemetry.h" #include "subsystems/datalink/telemetry.h"
static void send_calibration(struct transport_tx *trans, struct device *dev) { static void send_calibration(struct transport_tx *trans, struct device *dev) {
DOWNLINK_SEND_CALIBRATION(DefaultChannel, DefaultDevice, &v_ctl_auto_throttle_sum_err, &v_ctl_auto_throttle_submode); pprz_msg_send_CALIBRATION(trans, dev, AC_ID, &v_ctl_auto_throttle_sum_err, &v_ctl_auto_throttle_submode);
} }
static void send_tune_roll(struct transport_tx *trans, struct device *dev) { static void send_tune_roll(struct transport_tx *trans, struct device *dev) {
DOWNLINK_SEND_TUNE_ROLL(DefaultChannel, DefaultDevice, pprz_msg_send_TUNE_ROLL(trans, dev, AC_ID,
&(stateGetBodyRates_f()->p), &(stateGetNedToBodyEulers_f()->phi), &h_ctl_roll_setpoint); &(stateGetBodyRates_f()->p), &(stateGetNedToBodyEulers_f()->phi), &h_ctl_roll_setpoint);
} }
static void send_ctl_a(struct transport_tx *trans, struct device *dev) { static void send_ctl_a(struct transport_tx *trans, struct device *dev) {
DOWNLINK_SEND_H_CTL_A(DefaultChannel, DefaultDevice, pprz_msg_send_H_CTL_A(trans, dev, AC_ID,
&h_ctl_roll_sum_err, &h_ctl_roll_sum_err,
&h_ctl_roll_setpoint, &h_ctl_roll_setpoint,
&h_ctl_ref.roll_angle, &h_ctl_ref.roll_angle,
@@ -116,7 +116,7 @@ static float nav_ratio;
#include "subsystems/datalink/telemetry.h" #include "subsystems/datalink/telemetry.h"
static void send_calibration(struct transport_tx *trans, struct device *dev) { static void send_calibration(struct transport_tx *trans, struct device *dev) {
DOWNLINK_SEND_CALIBRATION(DefaultChannel, DefaultDevice, &v_ctl_auto_throttle_sum_err, &v_ctl_auto_throttle_submode); pprz_msg_send_CALIBRATION(trans, dev, AC_ID, &v_ctl_auto_throttle_sum_err, &v_ctl_auto_throttle_submode);
} }
#endif #endif
+9 -9
View File
@@ -139,7 +139,7 @@ PRINT_CONFIG_MSG("Enabled UNLOCKED_HOME_MODE since MODE_AUTO2 is AP_MODE_HOME")
#endif #endif
static void send_alive(struct transport_tx *trans, struct device *dev) { static void send_alive(struct transport_tx *trans, struct device *dev) {
DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM); pprz_msg_send_ALIVE(trans, dev, AC_ID, 16, MD5SUM);
} }
#if USE_MOTOR_MIXING #if USE_MOTOR_MIXING
@@ -159,7 +159,7 @@ static void send_status(struct transport_tx *trans, struct device *dev) {
uint8_t fix = GPS_FIX_NONE; uint8_t fix = GPS_FIX_NONE;
#endif #endif
uint16_t time_sec = sys_time.nb_sec; uint16_t time_sec = sys_time.nb_sec;
DOWNLINK_SEND_ROTORCRAFT_STATUS(DefaultChannel, DefaultDevice, pprz_msg_send_ROTORCRAFT_STATUS(trans, dev, AC_ID,
&imu_nb_err, &_motor_nb_err, &imu_nb_err, &_motor_nb_err,
&radio_control.status, &radio_control.frame_rate, &radio_control.status, &radio_control.frame_rate,
&fix, &autopilot_mode, &fix, &autopilot_mode,
@@ -173,12 +173,12 @@ static void send_energy(struct transport_tx *trans, struct device *dev) {
float vsup = ((float)electrical.vsupply) / 10.0f; float vsup = ((float)electrical.vsupply) / 10.0f;
float curs = ((float)electrical.current) / 1000.0f; float curs = ((float)electrical.current) / 1000.0f;
float power = vsup * curs; float power = vsup * curs;
DOWNLINK_SEND_ENERGY(DefaultChannel, DefaultDevice, &vsup, &curs, &e, &power); pprz_msg_send_ENERGY(trans, dev, AC_ID, &vsup, &curs, &e, &power);
} }
static void send_fp(struct transport_tx *trans, struct device *dev) { static void send_fp(struct transport_tx *trans, struct device *dev) {
int32_t carrot_up = -guidance_v_z_sp; int32_t carrot_up = -guidance_v_z_sp;
DOWNLINK_SEND_ROTORCRAFT_FP(DefaultChannel, DefaultDevice, pprz_msg_send_ROTORCRAFT_FP(trans, dev, AC_ID,
&(stateGetPositionEnu_i()->x), &(stateGetPositionEnu_i()->x),
&(stateGetPositionEnu_i()->y), &(stateGetPositionEnu_i()->y),
&(stateGetPositionEnu_i()->z), &(stateGetPositionEnu_i()->z),
@@ -198,7 +198,7 @@ static void send_fp(struct transport_tx *trans, struct device *dev) {
#ifdef RADIO_CONTROL #ifdef RADIO_CONTROL
static void send_rc(struct transport_tx *trans, struct device *dev) { static void send_rc(struct transport_tx *trans, struct device *dev) {
DOWNLINK_SEND_RC(DefaultChannel, DefaultDevice, RADIO_CONTROL_NB_CHANNEL, radio_control.values); pprz_msg_send_RC(trans, dev, AC_ID, RADIO_CONTROL_NB_CHANNEL, radio_control.values);
} }
static void send_rotorcraft_rc(struct transport_tx *trans, struct device *dev) { static void send_rotorcraft_rc(struct transport_tx *trans, struct device *dev) {
@@ -207,7 +207,7 @@ static void send_rotorcraft_rc(struct transport_tx *trans, struct device *dev) {
#else #else
int16_t _kill_switch = 42; int16_t _kill_switch = 42;
#endif #endif
DOWNLINK_SEND_ROTORCRAFT_RADIO_CONTROL(DefaultChannel, DefaultDevice, pprz_msg_send_ROTORCRAFT_RADIO_CONTROL(trans, dev, AC_ID,
&radio_control.values[RADIO_ROLL], &radio_control.values[RADIO_ROLL],
&radio_control.values[RADIO_PITCH], &radio_control.values[RADIO_PITCH],
&radio_control.values[RADIO_YAW], &radio_control.values[RADIO_YAW],
@@ -220,16 +220,16 @@ static void send_rotorcraft_rc(struct transport_tx *trans, struct device *dev) {
#ifdef ACTUATORS #ifdef ACTUATORS
static void send_actuators(struct transport_tx *trans, struct device *dev) { static void send_actuators(struct transport_tx *trans, struct device *dev) {
DOWNLINK_SEND_ACTUATORS(DefaultChannel, DefaultDevice , ACTUATORS_NB, actuators); pprz_msg_send_ACTUATORS(trans, dev, AC_ID , ACTUATORS_NB, actuators);
} }
#endif #endif
static void send_dl_value(struct transport_tx *trans, struct device *dev) { static void send_dl_value(struct transport_tx *trans, struct device *dev) {
PeriodicSendDlValue(DefaultChannel, DefaultDevice); PeriodicSendDlValue(trans, dev);
} }
static void send_rotorcraft_cmd(struct transport_tx *trans, struct device *dev) { static void send_rotorcraft_cmd(struct transport_tx *trans, struct device *dev) {
DOWNLINK_SEND_ROTORCRAFT_CMD(DefaultChannel, DefaultDevice, pprz_msg_send_ROTORCRAFT_CMD(trans, dev, AC_ID,
&stabilization_cmd[COMMAND_ROLL], &stabilization_cmd[COMMAND_ROLL],
&stabilization_cmd[COMMAND_PITCH], &stabilization_cmd[COMMAND_PITCH],
&stabilization_cmd[COMMAND_YAW], &stabilization_cmd[COMMAND_YAW],
@@ -111,7 +111,7 @@ static void read_rc_setpoint_speed_i(struct Int32Vect2 *speed_sp, bool_t in_flig
static void send_gh(struct transport_tx *trans, struct device *dev) { static void send_gh(struct transport_tx *trans, struct device *dev) {
struct NedCoor_i* pos = stateGetPositionNed_i(); struct NedCoor_i* pos = stateGetPositionNed_i();
DOWNLINK_SEND_GUIDANCE_H_INT(DefaultChannel, DefaultDevice, pprz_msg_send_GUIDANCE_H_INT(trans, dev, AC_ID,
&guidance_h_pos_sp.x, &guidance_h_pos_sp.y, &guidance_h_pos_sp.x, &guidance_h_pos_sp.y,
&guidance_h_pos_ref.x, &guidance_h_pos_ref.y, &guidance_h_pos_ref.x, &guidance_h_pos_ref.y,
&(pos->x), &(pos->y)); &(pos->x), &(pos->y));
@@ -121,7 +121,7 @@ static void send_hover_loop(struct transport_tx *trans, struct device *dev) {
struct NedCoor_i* pos = stateGetPositionNed_i(); struct NedCoor_i* pos = stateGetPositionNed_i();
struct NedCoor_i* speed = stateGetSpeedNed_i(); struct NedCoor_i* speed = stateGetSpeedNed_i();
struct NedCoor_i* accel = stateGetAccelNed_i(); struct NedCoor_i* accel = stateGetAccelNed_i();
DOWNLINK_SEND_HOVER_LOOP(DefaultChannel, DefaultDevice, pprz_msg_send_HOVER_LOOP(trans, dev, AC_ID,
&guidance_h_pos_sp.x, &guidance_h_pos_sp.x,
&guidance_h_pos_sp.y, &guidance_h_pos_sp.y,
&(pos->x), &(pos->y), &(pos->x), &(pos->y),
@@ -139,7 +139,7 @@ static void send_hover_loop(struct transport_tx *trans, struct device *dev) {
} }
static void send_href(struct transport_tx *trans, struct device *dev) { static void send_href(struct transport_tx *trans, struct device *dev) {
DOWNLINK_SEND_GUIDANCE_H_REF_INT(DefaultChannel, DefaultDevice, pprz_msg_send_GUIDANCE_H_REF_INT(trans, dev, AC_ID,
&guidance_h_pos_sp.x, &guidance_h_pos_ref.x, &guidance_h_pos_sp.x, &guidance_h_pos_ref.x,
&guidance_h_speed_sp.x, &guidance_h_speed_ref.x, &guidance_h_speed_sp.x, &guidance_h_speed_ref.x,
&guidance_h_accel_ref.x, &guidance_h_accel_ref.x,
@@ -149,7 +149,7 @@ static void send_href(struct transport_tx *trans, struct device *dev) {
} }
static void send_tune_hover(struct transport_tx *trans, struct device *dev) { static void send_tune_hover(struct transport_tx *trans, struct device *dev) {
DOWNLINK_SEND_ROTORCRAFT_TUNE_HOVER(DefaultChannel, DefaultDevice, pprz_msg_send_ROTORCRAFT_TUNE_HOVER(trans, dev, AC_ID,
&radio_control.values[RADIO_ROLL], &radio_control.values[RADIO_ROLL],
&radio_control.values[RADIO_PITCH], &radio_control.values[RADIO_PITCH],
&radio_control.values[RADIO_YAW], &radio_control.values[RADIO_YAW],
@@ -143,7 +143,7 @@ static void run_hover_loop(bool_t in_flight);
#include "subsystems/datalink/telemetry.h" #include "subsystems/datalink/telemetry.h"
static void send_vert_loop(struct transport_tx *trans, struct device *dev) { static void send_vert_loop(struct transport_tx *trans, struct device *dev) {
DOWNLINK_SEND_VERT_LOOP(DefaultChannel, DefaultDevice, pprz_msg_send_VERT_LOOP(trans, dev, AC_ID,
&guidance_v_z_sp, &guidance_v_zd_sp, &guidance_v_z_sp, &guidance_v_zd_sp,
&(stateGetPositionNed_i()->z), &(stateGetPositionNed_i()->z),
&(stateGetSpeedNed_i()->z), &(stateGetSpeedNed_i()->z),
@@ -160,7 +160,7 @@ static void send_vert_loop(struct transport_tx *trans, struct device *dev) {
} }
static void send_tune_vert(struct transport_tx *trans, struct device *dev) { static void send_tune_vert(struct transport_tx *trans, struct device *dev) {
DOWNLINK_SEND_TUNE_VERT(DefaultChannel, DefaultDevice, pprz_msg_send_TUNE_VERT(trans, dev, AC_ID,
&guidance_v_z_sp, &guidance_v_z_sp,
&(stateGetPositionNed_i()->z), &(stateGetPositionNed_i()->z),
&guidance_v_z_ref, &guidance_v_z_ref,
+1 -1
View File
@@ -228,7 +228,7 @@ STATIC_INLINE void main_periodic( void ) {
} }
STATIC_INLINE void telemetry_periodic(void) { STATIC_INLINE void telemetry_periodic(void) {
periodic_telemetry_send_Main((void*)(DefaultChannel), (void*)(DefaultDevice)); periodic_telemetry_send_Main(&(DefaultChannel).trans_tx, &(DefaultDevice).device);
} }
/** mode to enter when RC is lost while using a mode with RC input (not AP_MODE_NAV) */ /** mode to enter when RC is lost while using a mode with RC input (not AP_MODE_NAV) */
@@ -109,7 +109,7 @@ static inline void nav_set_altitude( void );
static void send_nav_status(struct transport_tx *trans, struct device *dev) { static void send_nav_status(struct transport_tx *trans, struct device *dev) {
float dist_home = sqrtf(dist2_to_home); float dist_home = sqrtf(dist2_to_home);
float dist_wp = sqrtf(dist2_to_wp); float dist_wp = sqrtf(dist2_to_wp);
DOWNLINK_SEND_ROTORCRAFT_NAV_STATUS(DefaultChannel, DefaultDevice, pprz_msg_send_ROTORCRAFT_NAV_STATUS(trans, dev, AC_ID,
&block_time, &stage_time, &block_time, &stage_time,
&dist_home, &dist_wp, &dist_home, &dist_wp,
&nav_block, &nav_stage, &nav_block, &nav_stage,
@@ -119,20 +119,20 @@ static void send_nav_status(struct transport_tx *trans, struct device *dev) {
float sy = POS_FLOAT_OF_BFP(nav_segment_start.y); float sy = POS_FLOAT_OF_BFP(nav_segment_start.y);
float ex = POS_FLOAT_OF_BFP(nav_segment_end.x); float ex = POS_FLOAT_OF_BFP(nav_segment_end.x);
float ey = POS_FLOAT_OF_BFP(nav_segment_end.y); float ey = POS_FLOAT_OF_BFP(nav_segment_end.y);
DOWNLINK_SEND_SEGMENT(DefaultChannel, DefaultDevice, &sx, &sy, &ex, &ey); pprz_msg_send_SEGMENT(trans, dev, AC_ID, &sx, &sy, &ex, &ey);
} }
else if (horizontal_mode == HORIZONTAL_MODE_CIRCLE) { else if (horizontal_mode == HORIZONTAL_MODE_CIRCLE) {
float cx = POS_FLOAT_OF_BFP(nav_circle_center.x); float cx = POS_FLOAT_OF_BFP(nav_circle_center.x);
float cy = POS_FLOAT_OF_BFP(nav_circle_center.y); float cy = POS_FLOAT_OF_BFP(nav_circle_center.y);
float r = POS_FLOAT_OF_BFP(nav_circle_radius); float r = POS_FLOAT_OF_BFP(nav_circle_radius);
DOWNLINK_SEND_CIRCLE(DefaultChannel, DefaultDevice, &cx, &cy, &r); pprz_msg_send_CIRCLE(trans, dev, AC_ID, &cx, &cy, &r);
} }
} }
static void send_wp_moved(struct transport_tx *trans, struct device *dev) { static void send_wp_moved(struct transport_tx *trans, struct device *dev) {
static uint8_t i; static uint8_t i;
i++; if (i >= nb_waypoint) i = 0; i++; if (i >= nb_waypoint) i = 0;
DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, pprz_msg_send_WP_MOVED_ENU(trans, dev, AC_ID,
&i, &i,
&(waypoints[i].x), &(waypoints[i].x),
&(waypoints[i].y), &(waypoints[i].y),
@@ -48,7 +48,7 @@ static void send_att(struct transport_tx *trans, struct device *dev) {
struct FloatRates* body_rate = stateGetBodyRates_f(); struct FloatRates* body_rate = stateGetBodyRates_f();
struct FloatEulers* att = stateGetNedToBodyEulers_f(); struct FloatEulers* att = stateGetNedToBodyEulers_f();
float foo = 0.0; float foo = 0.0;
DOWNLINK_SEND_STAB_ATTITUDE_FLOAT(DefaultChannel, DefaultDevice, pprz_msg_send_STAB_ATTITUDE_FLOAT(trans, dev, AC_ID,
&(body_rate->p), &(body_rate->q), &(body_rate->r), &(body_rate->p), &(body_rate->q), &(body_rate->r),
&(att->phi), &(att->theta), &(att->psi), &(att->phi), &(att->theta), &(att->psi),
&stab_att_sp_euler.phi, &stab_att_sp_euler.phi,
@@ -70,7 +70,7 @@ static void send_att(struct transport_tx *trans, struct device *dev) {
} }
static void send_att_ref(struct transport_tx *trans, struct device *dev) { static void send_att_ref(struct transport_tx *trans, struct device *dev) {
DOWNLINK_SEND_STAB_ATTITUDE_REF_FLOAT(DefaultChannel, DefaultDevice, pprz_msg_send_STAB_ATTITUDE_REF_FLOAT(trans, dev, AC_ID,
&stab_att_sp_euler.phi, &stab_att_sp_euler.phi,
&stab_att_sp_euler.theta, &stab_att_sp_euler.theta,
&stab_att_sp_euler.psi, &stab_att_sp_euler.psi,
@@ -68,7 +68,7 @@ static inline void reset_psi_ref_from_body(void) {
static void send_att(struct transport_tx *trans, struct device *dev) { static void send_att(struct transport_tx *trans, struct device *dev) {
struct Int32Rates* body_rate = stateGetBodyRates_i(); struct Int32Rates* body_rate = stateGetBodyRates_i();
struct Int32Eulers* att = stateGetNedToBodyEulers_i(); struct Int32Eulers* att = stateGetNedToBodyEulers_i();
DOWNLINK_SEND_STAB_ATTITUDE_INT(DefaultChannel, DefaultDevice, pprz_msg_send_STAB_ATTITUDE_INT(trans, dev, AC_ID,
&(body_rate->p), &(body_rate->q), &(body_rate->r), &(body_rate->p), &(body_rate->q), &(body_rate->r),
&(att->phi), &(att->theta), &(att->psi), &(att->phi), &(att->theta), &(att->psi),
&stab_att_sp_euler.phi, &stab_att_sp_euler.phi,
@@ -89,7 +89,7 @@ static void send_att(struct transport_tx *trans, struct device *dev) {
} }
static void send_att_ref(struct transport_tx *trans, struct device *dev) { static void send_att_ref(struct transport_tx *trans, struct device *dev) {
DOWNLINK_SEND_STAB_ATTITUDE_REF_INT(DefaultChannel, DefaultDevice, pprz_msg_send_STAB_ATTITUDE_REF_INT(trans, dev, AC_ID,
&stab_att_sp_euler.phi, &stab_att_sp_euler.phi,
&stab_att_sp_euler.theta, &stab_att_sp_euler.theta,
&stab_att_sp_euler.psi, &stab_att_sp_euler.psi,
@@ -98,7 +98,7 @@ static const float psi_ddgain_surface[] = STABILIZATION_ATTITUDE_PSI_DDGAIN_SURF
static void send_att(struct transport_tx *trans, struct device *dev) { static void send_att(struct transport_tx *trans, struct device *dev) {
struct FloatRates* body_rate = stateGetBodyRates_f(); struct FloatRates* body_rate = stateGetBodyRates_f();
struct FloatEulers* att = stateGetNedToBodyEulers_f(); struct FloatEulers* att = stateGetNedToBodyEulers_f();
DOWNLINK_SEND_STAB_ATTITUDE_FLOAT(DefaultChannel, DefaultDevice, pprz_msg_send_STAB_ATTITUDE_FLOAT(trans, dev, AC_ID,
&(body_rate->p), &(body_rate->q), &(body_rate->r), &(body_rate->p), &(body_rate->q), &(body_rate->r),
&(att->phi), &(att->theta), &(att->psi), &(att->phi), &(att->theta), &(att->psi),
&stab_att_sp_euler.phi, &stab_att_sp_euler.phi,
@@ -120,7 +120,7 @@ static void send_att(struct transport_tx *trans, struct device *dev) {
} }
static void send_att_ref(struct transport_tx *trans, struct device *dev) { static void send_att_ref(struct transport_tx *trans, struct device *dev) {
DOWNLINK_SEND_STAB_ATTITUDE_REF_FLOAT(DefaultChannel, DefaultDevice, pprz_msg_send_STAB_ATTITUDE_REF_FLOAT(trans, dev, AC_ID,
&stab_att_sp_euler.phi, &stab_att_sp_euler.phi,
&stab_att_sp_euler.theta, &stab_att_sp_euler.theta,
&stab_att_sp_euler.psi, &stab_att_sp_euler.psi,
@@ -72,7 +72,7 @@ int32_t stabilization_att_ff_cmd[COMMANDS_NB];
static void send_att(struct transport_tx *trans, struct device *dev) { //FIXME really use this message here ? static void send_att(struct transport_tx *trans, struct device *dev) { //FIXME really use this message here ?
struct Int32Rates* body_rate = stateGetBodyRates_i(); struct Int32Rates* body_rate = stateGetBodyRates_i();
struct Int32Eulers* att = stateGetNedToBodyEulers_i(); struct Int32Eulers* att = stateGetNedToBodyEulers_i();
DOWNLINK_SEND_STAB_ATTITUDE_INT(DefaultChannel, DefaultDevice, pprz_msg_send_STAB_ATTITUDE_INT(trans, dev, AC_ID,
&(body_rate->p), &(body_rate->q), &(body_rate->r), &(body_rate->p), &(body_rate->q), &(body_rate->r),
&(att->phi), &(att->theta), &(att->psi), &(att->phi), &(att->theta), &(att->psi),
&stab_att_sp_euler.phi, &stab_att_sp_euler.phi,
@@ -93,7 +93,7 @@ static void send_att(struct transport_tx *trans, struct device *dev) { //FIXME r
} }
static void send_att_ref(struct transport_tx *trans, struct device *dev) { static void send_att_ref(struct transport_tx *trans, struct device *dev) {
DOWNLINK_SEND_STAB_ATTITUDE_REF_INT(DefaultChannel, DefaultDevice, pprz_msg_send_STAB_ATTITUDE_REF_INT(trans, dev, AC_ID,
&stab_att_sp_euler.phi, &stab_att_sp_euler.phi,
&stab_att_sp_euler.theta, &stab_att_sp_euler.theta,
&stab_att_sp_euler.psi, &stab_att_sp_euler.psi,
@@ -110,7 +110,7 @@ static void send_att_ref(struct transport_tx *trans, struct device *dev) {
static void send_ahrs_ref_quat(struct transport_tx *trans, struct device *dev) { static void send_ahrs_ref_quat(struct transport_tx *trans, struct device *dev) {
struct Int32Quat* quat = stateGetNedToBodyQuat_i(); struct Int32Quat* quat = stateGetNedToBodyQuat_i();
DOWNLINK_SEND_AHRS_REF_QUAT(DefaultChannel, DefaultDevice, pprz_msg_send_AHRS_REF_QUAT(trans, dev, AC_ID,
&stab_att_ref_quat.qi, &stab_att_ref_quat.qi,
&stab_att_ref_quat.qx, &stab_att_ref_quat.qx,
&stab_att_ref_quat.qy, &stab_att_ref_quat.qy,
@@ -119,7 +119,7 @@ struct Int32Rates stabilization_rate_ff_cmd;
#include "subsystems/datalink/telemetry.h" #include "subsystems/datalink/telemetry.h"
static void send_rate(struct transport_tx *trans, struct device *dev) { static void send_rate(struct transport_tx *trans, struct device *dev) {
DOWNLINK_SEND_RATE_LOOP(DefaultChannel, DefaultDevice, pprz_msg_send_RATE_LOOP(trans, dev, AC_ID,
&stabilization_rate_sp.p, &stabilization_rate_sp.p,
&stabilization_rate_sp.q, &stabilization_rate_sp.q,
&stabilization_rate_sp.r, &stabilization_rate_sp.r,
+2 -2
View File
@@ -186,7 +186,7 @@ struct link_mcu_msg link_mcu_from_fbw_msg;
static void send_commands(struct transport_tx *trans, struct device *dev) static void send_commands(struct transport_tx *trans, struct device *dev)
{ {
DOWNLINK_SEND_COMMANDS(DefaultChannel, DefaultDevice, COMMANDS_NB, ap_state->commands); pprz_msg_send_COMMANDS(trans, dev, AC_ID, COMMANDS_NB, ap_state->commands);
} }
static void send_fbw_status(struct transport_tx *trans, struct device *dev) static void send_fbw_status(struct transport_tx *trans, struct device *dev)
@@ -206,7 +206,7 @@ static void send_fbw_status(struct transport_tx *trans, struct device *dev)
} else { } else {
rc_status = RC_LOST; rc_status = RC_LOST;
} }
DOWNLINK_SEND_FBW_STATUS(DefaultChannel, DefaultDevice, pprz_msg_send_FBW_STATUS(trans, dev, AC_ID,
&(rc_status), &(fbw_state->ppm_cpt), &(fbw_status), &(fbw_state->vsupply), &(fbw_state->current)); &(rc_status), &(fbw_state->ppm_cpt), &(fbw_status), &(fbw_state->vsupply), &(fbw_state->current));
} }
#endif #endif
+1 -1
View File
@@ -108,7 +108,7 @@ uint8_t link_mcu_fbw_nb_err;
static void send_debug_link(struct transport_tx *trans, struct device *dev) { static void send_debug_link(struct transport_tx *trans, struct device *dev) {
uint8_t mcu1_ppm_cpt_foo = 0; //FIXME uint8_t mcu1_ppm_cpt_foo = 0; //FIXME
DOWNLINK_SEND_DEBUG_MCU_LINK(DefaultChannel, DefaultDevice, pprz_msg_send_DEBUG_MCU_LINK(trans, dev, AC_ID,
&link_mcu_nb_err, &link_mcu_fbw_nb_err, &mcu1_ppm_cpt_foo); &link_mcu_nb_err, &link_mcu_fbw_nb_err, &mcu1_ppm_cpt_foo);
} }
#endif #endif
+2 -2
View File
@@ -255,7 +255,7 @@ inline void parse_mavpilot_msg( void );
static void send_commands(struct transport_tx *trans, struct device *dev) { static void send_commands(struct transport_tx *trans, struct device *dev) {
DOWNLINK_SEND_COMMANDS(DefaultChannel, DefaultDevice, COMMANDS_NB, ap_state->commands); pprz_msg_send_COMMANDS(trans, dev, AC_ID, COMMANDS_NB, ap_state->commands);
} }
@@ -272,7 +272,7 @@ static void send_fbw_status(struct transport_tx *trans, struct device *dev) {
rc_status = RC_OK; rc_status = RC_OK;
else else
rc_status = RC_LOST; rc_status = RC_LOST;
DOWNLINK_SEND_FBW_STATUS(DefaultChannel, DefaultDevice, pprz_msg_send_FBW_STATUS(trans, dev, AC_ID,
&(rc_status), &(fbw_state->ppm_cpt), &(fbw_status), &(fbw_state->vsupply), &(fbw_state->current)); &(rc_status), &(fbw_state->ppm_cpt), &(fbw_status), &(fbw_state->vsupply), &(fbw_state->current));
} }
#endif #endif
+13 -13
View File
@@ -36,7 +36,7 @@
struct i2c_periph i2c0; struct i2c_periph i2c0;
#if PERIODIC_TELEMETRY #if PERIODIC_TELEMETRY
static void send_i2c0_err(void) { static void send_i2c0_err(struct transport_tx *trans, struct device *dev) {
uint16_t i2c0_queue_full_cnt = i2c0.errors->queue_full_cnt; uint16_t i2c0_queue_full_cnt = i2c0.errors->queue_full_cnt;
uint16_t i2c0_ack_fail_cnt = i2c0.errors->ack_fail_cnt; uint16_t i2c0_ack_fail_cnt = i2c0.errors->ack_fail_cnt;
uint16_t i2c0_miss_start_stop_cnt = i2c0.errors->miss_start_stop_cnt; uint16_t i2c0_miss_start_stop_cnt = i2c0.errors->miss_start_stop_cnt;
@@ -48,7 +48,7 @@ static void send_i2c0_err(void) {
uint16_t i2c0_unexpected_event_cnt = i2c0.errors->unexpected_event_cnt; uint16_t i2c0_unexpected_event_cnt = i2c0.errors->unexpected_event_cnt;
uint32_t i2c0_last_unexpected_event = i2c0.errors->last_unexpected_event; uint32_t i2c0_last_unexpected_event = i2c0.errors->last_unexpected_event;
uint8_t _bus0 = 0; uint8_t _bus0 = 0;
DOWNLINK_SEND_I2C_ERRORS(DefaultChannel, DefaultDevice, pprz_msg_send_I2C_ERRORS(trans, dev, AC_ID,
&i2c0_queue_full_cnt, &i2c0_queue_full_cnt,
&i2c0_ack_fail_cnt, &i2c0_ack_fail_cnt,
&i2c0_miss_start_stop_cnt, &i2c0_miss_start_stop_cnt,
@@ -76,7 +76,7 @@ void i2c0_init(void) {
struct i2c_periph i2c1; struct i2c_periph i2c1;
#if PERIODIC_TELEMETRY #if PERIODIC_TELEMETRY
static void send_i2c1_err(void) { static void send_i2c1_err(struct transport_tx *trans, struct device *dev) {
uint16_t i2c1_queue_full_cnt = i2c1.errors->queue_full_cnt; uint16_t i2c1_queue_full_cnt = i2c1.errors->queue_full_cnt;
uint16_t i2c1_ack_fail_cnt = i2c1.errors->ack_fail_cnt; uint16_t i2c1_ack_fail_cnt = i2c1.errors->ack_fail_cnt;
uint16_t i2c1_miss_start_stop_cnt = i2c1.errors->miss_start_stop_cnt; uint16_t i2c1_miss_start_stop_cnt = i2c1.errors->miss_start_stop_cnt;
@@ -88,7 +88,7 @@ static void send_i2c1_err(void) {
uint16_t i2c1_unexpected_event_cnt = i2c1.errors->unexpected_event_cnt; uint16_t i2c1_unexpected_event_cnt = i2c1.errors->unexpected_event_cnt;
uint32_t i2c1_last_unexpected_event = i2c1.errors->last_unexpected_event; uint32_t i2c1_last_unexpected_event = i2c1.errors->last_unexpected_event;
uint8_t _bus1 = 1; uint8_t _bus1 = 1;
DOWNLINK_SEND_I2C_ERRORS(DefaultChannel, DefaultDevice, pprz_msg_send_I2C_ERRORS(trans, dev, AC_ID,
&i2c1_queue_full_cnt, &i2c1_queue_full_cnt,
&i2c1_ack_fail_cnt, &i2c1_ack_fail_cnt,
&i2c1_miss_start_stop_cnt, &i2c1_miss_start_stop_cnt,
@@ -116,7 +116,7 @@ void i2c1_init(void) {
struct i2c_periph i2c2; struct i2c_periph i2c2;
#if PERIODIC_TELEMETRY #if PERIODIC_TELEMETRY
static void send_i2c2_err(void) { static void send_i2c2_err(struct transport_tx *trans, struct device *dev) {
uint16_t i2c2_queue_full_cnt = i2c2.errors->queue_full_cnt; uint16_t i2c2_queue_full_cnt = i2c2.errors->queue_full_cnt;
uint16_t i2c2_ack_fail_cnt = i2c2.errors->ack_fail_cnt; uint16_t i2c2_ack_fail_cnt = i2c2.errors->ack_fail_cnt;
uint16_t i2c2_miss_start_stop_cnt = i2c2.errors->miss_start_stop_cnt; uint16_t i2c2_miss_start_stop_cnt = i2c2.errors->miss_start_stop_cnt;
@@ -128,7 +128,7 @@ static void send_i2c2_err(void) {
uint16_t i2c2_unexpected_event_cnt = i2c2.errors->unexpected_event_cnt; uint16_t i2c2_unexpected_event_cnt = i2c2.errors->unexpected_event_cnt;
uint32_t i2c2_last_unexpected_event = i2c2.errors->last_unexpected_event; uint32_t i2c2_last_unexpected_event = i2c2.errors->last_unexpected_event;
uint8_t _bus2 = 2; uint8_t _bus2 = 2;
DOWNLINK_SEND_I2C_ERRORS(DefaultChannel, DefaultDevice, pprz_msg_send_I2C_ERRORS(trans, dev, AC_ID,
&i2c2_queue_full_cnt, &i2c2_queue_full_cnt,
&i2c2_ack_fail_cnt, &i2c2_ack_fail_cnt,
&i2c2_miss_start_stop_cnt, &i2c2_miss_start_stop_cnt,
@@ -160,7 +160,7 @@ void i2c3_init(void) {
} }
#if PERIODIC_TELEMETRY #if PERIODIC_TELEMETRY
static void send_i2c3_err(void) { static void send_i2c3_err(struct transport_tx *trans, struct device *dev) {
uint16_t i2c3_queue_full_cnt = i2c3.errors->queue_full_cnt; uint16_t i2c3_queue_full_cnt = i2c3.errors->queue_full_cnt;
uint16_t i2c3_ack_fail_cnt = i2c3.errors->ack_fail_cnt; uint16_t i2c3_ack_fail_cnt = i2c3.errors->ack_fail_cnt;
uint16_t i2c3_miss_start_stop_cnt = i2c3.errors->miss_start_stop_cnt; uint16_t i2c3_miss_start_stop_cnt = i2c3.errors->miss_start_stop_cnt;
@@ -172,7 +172,7 @@ static void send_i2c3_err(void) {
uint16_t i2c3_unexpected_event_cnt = i2c3.errors->unexpected_event_cnt; uint16_t i2c3_unexpected_event_cnt = i2c3.errors->unexpected_event_cnt;
uint32_t i2c3_last_unexpected_event = i2c3.errors->last_unexpected_event; uint32_t i2c3_last_unexpected_event = i2c3.errors->last_unexpected_event;
uint8_t _bus3 = 3; uint8_t _bus3 = 3;
DOWNLINK_SEND_I2C_ERRORS(DefaultChannel, DefaultDevice, pprz_msg_send_I2C_ERRORS(trans, dev, AC_ID,
&i2c3_queue_full_cnt, &i2c3_queue_full_cnt,
&i2c3_ack_fail_cnt, &i2c3_ack_fail_cnt,
&i2c3_miss_start_stop_cnt, &i2c3_miss_start_stop_cnt,
@@ -190,27 +190,27 @@ static void send_i2c3_err(void) {
#endif /* USE_I2C3 */ #endif /* USE_I2C3 */
#if PERIODIC_TELEMETRY #if PERIODIC_TELEMETRY
static void send_i2c_err(void) { static void send_i2c_err(struct transport_tx *trans, struct device *dev) {
static uint8_t _i2c_nb_cnt = 0; static uint8_t _i2c_nb_cnt = 0;
switch (_i2c_nb_cnt) { switch (_i2c_nb_cnt) {
case 0: case 0:
#if USE_I2C0 #if USE_I2C0
send_i2c0_err(); send_i2c0_err(trans, dev);
#endif #endif
break; break;
case 1: case 1:
#if USE_I2C1 #if USE_I2C1
send_i2c1_err(); send_i2c1_err(trans, dev);
#endif #endif
break; break;
case 2: case 2:
#if USE_I2C2 #if USE_I2C2
send_i2c2_err(); send_i2c2_err(trans, dev);
#endif #endif
break; break;
case 3: case 3:
#if USE_I2C3 #if USE_I2C3
send_i2c3_err(); send_i2c3_err(trans, dev);
#endif #endif
break; break;
default: default:
+22 -22
View File
@@ -30,12 +30,12 @@
struct uart_periph uart0; struct uart_periph uart0;
#if PERIODIC_TELEMETRY #if PERIODIC_TELEMETRY
static void send_uart0_err(void) { static void send_uart0_err(struct transport_tx *trans, struct device *dev) {
uint16_t ore = uart0.ore; uint16_t ore = uart0.ore;
uint16_t ne_err = uart0.ne_err; uint16_t ne_err = uart0.ne_err;
uint16_t fe_err = uart0.fe_err; uint16_t fe_err = uart0.fe_err;
uint8_t _bus0 = 0; uint8_t _bus0 = 0;
DOWNLINK_SEND_UART_ERRORS(DefaultChannel, DefaultDevice, pprz_msg_send_UART_ERRORS(trans, dev, AC_ID,
&ore, &ne_err, &fe_err, &_bus0); &ore, &ne_err, &fe_err, &_bus0);
} }
#endif #endif
@@ -46,12 +46,12 @@ static void send_uart0_err(void) {
struct uart_periph uart1; struct uart_periph uart1;
#if PERIODIC_TELEMETRY #if PERIODIC_TELEMETRY
static void send_uart1_err(void) { static void send_uart1_err(struct transport_tx *trans, struct device *dev) {
uint16_t ore = uart1.ore; uint16_t ore = uart1.ore;
uint16_t ne_err = uart1.ne_err; uint16_t ne_err = uart1.ne_err;
uint16_t fe_err = uart1.fe_err; uint16_t fe_err = uart1.fe_err;
uint8_t _bus1 = 1; uint8_t _bus1 = 1;
DOWNLINK_SEND_UART_ERRORS(DefaultChannel, DefaultDevice, pprz_msg_send_UART_ERRORS(trans, dev, AC_ID,
&ore, &ne_err, &fe_err, &_bus1); &ore, &ne_err, &fe_err, &_bus1);
} }
#endif #endif
@@ -62,12 +62,12 @@ static void send_uart1_err(void) {
struct uart_periph uart2; struct uart_periph uart2;
#if PERIODIC_TELEMETRY #if PERIODIC_TELEMETRY
static void send_uart2_err(void) { static void send_uart2_err(struct transport_tx *trans, struct device *dev) {
uint16_t ore = uart2.ore; uint16_t ore = uart2.ore;
uint16_t ne_err = uart2.ne_err; uint16_t ne_err = uart2.ne_err;
uint16_t fe_err = uart2.fe_err; uint16_t fe_err = uart2.fe_err;
uint8_t _bus2 = 2; uint8_t _bus2 = 2;
DOWNLINK_SEND_UART_ERRORS(DefaultChannel, DefaultDevice, pprz_msg_send_UART_ERRORS(trans, dev, AC_ID,
&ore, &ne_err, &fe_err, &_bus2); &ore, &ne_err, &fe_err, &_bus2);
} }
#endif #endif
@@ -78,12 +78,12 @@ static void send_uart2_err(void) {
struct uart_periph uart3; struct uart_periph uart3;
#if PERIODIC_TELEMETRY #if PERIODIC_TELEMETRY
static void send_uart3_err(void) { static void send_uart3_err(struct transport_tx *trans, struct device *dev) {
uint16_t ore = uart3.ore; uint16_t ore = uart3.ore;
uint16_t ne_err = uart3.ne_err; uint16_t ne_err = uart3.ne_err;
uint16_t fe_err = uart3.fe_err; uint16_t fe_err = uart3.fe_err;
uint8_t _bus3 = 3; uint8_t _bus3 = 3;
DOWNLINK_SEND_UART_ERRORS(DefaultChannel, DefaultDevice, pprz_msg_send_UART_ERRORS(trans, dev, AC_ID,
&ore, &ne_err, &fe_err, &_bus3); &ore, &ne_err, &fe_err, &_bus3);
} }
#endif #endif
@@ -94,12 +94,12 @@ static void send_uart3_err(void) {
struct uart_periph uart4; struct uart_periph uart4;
#if PERIODIC_TELEMETRY #if PERIODIC_TELEMETRY
static void send_uart4_err(void) { static void send_uart4_err(struct transport_tx *trans, struct device *dev) {
uint16_t ore = uart4.ore; uint16_t ore = uart4.ore;
uint16_t ne_err = uart4.ne_err; uint16_t ne_err = uart4.ne_err;
uint16_t fe_err = uart4.fe_err; uint16_t fe_err = uart4.fe_err;
uint8_t _bus4 = 4; uint8_t _bus4 = 4;
DOWNLINK_SEND_UART_ERRORS(DefaultChannel, DefaultDevice, pprz_msg_send_UART_ERRORS(trans, dev, AC_ID,
&ore, &ne_err, &fe_err, &_bus4); &ore, &ne_err, &fe_err, &_bus4);
} }
#endif #endif
@@ -110,12 +110,12 @@ static void send_uart4_err(void) {
struct uart_periph uart5; struct uart_periph uart5;
#if PERIODIC_TELEMETRY #if PERIODIC_TELEMETRY
static void send_uart5_err(void) { static void send_uart5_err(struct transport_tx *trans, struct device *dev) {
uint16_t ore = uart5.ore; uint16_t ore = uart5.ore;
uint16_t ne_err = uart5.ne_err; uint16_t ne_err = uart5.ne_err;
uint16_t fe_err = uart5.fe_err; uint16_t fe_err = uart5.fe_err;
uint8_t _bus5 = 5; uint8_t _bus5 = 5;
DOWNLINK_SEND_UART_ERRORS(DefaultChannel, DefaultDevice, pprz_msg_send_UART_ERRORS(trans, dev, AC_ID,
&ore, &ne_err, &fe_err, &_bus5); &ore, &ne_err, &fe_err, &_bus5);
} }
#endif #endif
@@ -126,12 +126,12 @@ static void send_uart5_err(void) {
struct uart_periph uart6; struct uart_periph uart6;
#if PERIODIC_TELEMETRY #if PERIODIC_TELEMETRY
static void send_uart6_err(void) { static void send_uart6_err(struct transport_tx *trans, struct device *dev) {
uint16_t ore = uart6.ore; uint16_t ore = uart6.ore;
uint16_t ne_err = uart6.ne_err; uint16_t ne_err = uart6.ne_err;
uint16_t fe_err = uart6.fe_err; uint16_t fe_err = uart6.fe_err;
uint8_t _bus6 = 6; uint8_t _bus6 = 6;
DOWNLINK_SEND_UART_ERRORS(DefaultChannel, DefaultDevice, pprz_msg_send_UART_ERRORS(trans, dev, AC_ID,
&ore, &ne_err, &fe_err, &_bus6); &ore, &ne_err, &fe_err, &_bus6);
} }
#endif #endif
@@ -139,36 +139,36 @@ static void send_uart6_err(void) {
#endif #endif
#if PERIODIC_TELEMETRY #if PERIODIC_TELEMETRY
static void send_uart_err(void) { static void send_uart_err(struct transport_tx *trans, struct device *dev) {
static uint8_t uart_nb_cnt = 0; static uint8_t uart_nb_cnt = 0;
switch (uart_nb_cnt) { switch (uart_nb_cnt) {
#if USE_UART0 #if USE_UART0
case 0: case 0:
send_uart0_err(); break; send_uart0_err(trans, dev); break;
#endif #endif
#if USE_UART1 #if USE_UART1
case 1: case 1:
send_uart1_err(); break; send_uart1_err(trans, dev); break;
#endif #endif
#if USE_UART2 #if USE_UART2
case 2: case 2:
send_uart2_err(); break; send_uart2_err(trans, dev); break;
#endif #endif
#if USE_UART3 #if USE_UART3
case 3: case 3:
send_uart3_err(); break; send_uart3_err(trans, dev); break;
#endif #endif
#if USE_UART4 #if USE_UART4
case 4: case 4:
send_uart4_err(); break; send_uart4_err(trans, dev); break;
#endif #endif
#if USE_UART5 #if USE_UART5
case 5: case 5:
send_uart5_err(); break; send_uart5_err(trans, dev); break;
#endif #endif
#if USE_UART6 #if USE_UART6
case 6: case 6:
send_uart6_err(); break; send_uart6_err(trans, dev); break;
#endif #endif
default: break; default: break;
} }
+3 -3
View File
@@ -140,13 +140,13 @@ static void temperature_cb(uint8_t __attribute__((unused)) sender_id, const floa
static void send_baro_raw(struct transport_tx *trans, struct device *dev) static void send_baro_raw(struct transport_tx *trans, struct device *dev)
{ {
DOWNLINK_SEND_BARO_RAW(DefaultChannel, DefaultDevice, pprz_msg_send_BARO_RAW(trans, dev, AC_ID,
&air_data.pressure, &air_data.differential); &air_data.pressure, &air_data.differential);
} }
static void send_air_data(struct transport_tx *trans, struct device *dev) static void send_air_data(struct transport_tx *trans, struct device *dev)
{ {
DOWNLINK_SEND_AIR_DATA(DefaultChannel, DefaultDevice, pprz_msg_send_AIR_DATA(trans, dev, AC_ID,
&air_data.pressure, &air_data.differential, &air_data.pressure, &air_data.differential,
&air_data.temperature, &air_data.qnh, &air_data.temperature, &air_data.qnh,
&air_data.amsl_baro, &air_data.airspeed, &air_data.amsl_baro, &air_data.airspeed,
@@ -158,7 +158,7 @@ static void send_amsl(struct transport_tx *trans, struct device *dev)
const float MeterPerFeet = 0.3048; const float MeterPerFeet = 0.3048;
float amsl_baro_ft = air_data.amsl_baro / MeterPerFeet; float amsl_baro_ft = air_data.amsl_baro / MeterPerFeet;
float amsl_gps_ft = stateGetPositionLla_f()->alt / MeterPerFeet; float amsl_gps_ft = stateGetPositionLla_f()->alt / MeterPerFeet;
DOWNLINK_SEND_AMSL(DefaultChannel, DefaultDevice, &amsl_baro_ft, &amsl_gps_ft); pprz_msg_send_AMSL(trans, dev, AC_ID, &amsl_baro_ft, &amsl_gps_ft);
} }
#endif #endif
+1 -1
View File
@@ -106,7 +106,7 @@ static void send_cam(struct transport_tx *trans, struct device *dev) {
#ifdef SHOW_CAM_COORDINATES #ifdef SHOW_CAM_COORDINATES
static void send_cam_point(struct transport_tx *trans, struct device *dev) { static void send_cam_point(struct transport_tx *trans, struct device *dev) {
DOWNLINK_SEND_CAM_POINT(DefaultChannel, DefaultDevice, pprz_msg_send_CAM_POINT(trans, dev, AC_ID,
&cam_point_distance_from_home, &cam_point_lat, &cam_point_lon); &cam_point_distance_from_home, &cam_point_lat, &cam_point_lon);
} }
#endif #endif
@@ -89,7 +89,7 @@ int16_t rotorcraft_cam_pan;
#define ROTORCRAFT_CAM_PAN_MAX INT32_ANGLE_2_PI #define ROTORCRAFT_CAM_PAN_MAX INT32_ANGLE_2_PI
static void send_cam(struct transport_tx *trans, struct device *dev) { static void send_cam(struct transport_tx *trans, struct device *dev) {
DOWNLINK_SEND_ROTORCRAFT_CAM(DefaultChannel, DefaultDevice, pprz_msg_send_ROTORCRAFT_CAM(trans, dev, AC_ID,
&rotorcraft_cam_tilt,&rotorcraft_cam_pan); &rotorcraft_cam_tilt,&rotorcraft_cam_pan);
} }
@@ -107,7 +107,7 @@ static void send_thumbnails(struct transport_tx *trans, struct device *dev)
return; return;
} }
} }
DOWNLINK_SEND_PAYLOAD(DefaultChannel, DefaultDevice, THUMB_MSG_SIZE, thumbs[thumb_pointer]); pprz_msg_send_PAYLOAD(trans, dev, AC_ID, THUMB_MSG_SIZE, thumbs[thumb_pointer]);
// Update the write/read pointer: if we receive a new thumb part, that will be sent, otherwise the oldest infor is repeated // Update the write/read pointer: if we receive a new thumb part, that will be sent, otherwise the oldest infor is repeated
thumb_pointer++; thumb_pointer++;
@@ -139,7 +139,7 @@ static Butterworth2LowPass ms45xx_filter;
static void ms45xx_downlink(void) static void ms45xx_downlink(void)
{ {
DOWNLINK_SEND_AIRSPEED_MS45XX(DefaultChannel, DefaultDevice, pprz_msg_send_AIRSPEED_MS45XX(trans, dev, AC_ID,
&ms45xx.diff_pressure, &ms45xx.diff_pressure,
&ms45xx.temperature, &ms45xx.airspeed); &ms45xx.temperature, &ms45xx.airspeed);
} }
+1 -1
View File
@@ -100,7 +100,7 @@ float calc_lm35(int16_t raw_temp)
static void temp_adc_downlink(void) static void temp_adc_downlink(void)
{ {
DOWNLINK_SEND_TEMP_ADC(DefaultChannel, DefaultDevice, &temp_c1, &temp_c2, &temp_c3); pprz_msg_send_TEMP_ADC(trans, dev, AC_ID, &temp_c1, &temp_c2, &temp_c3);
} }
void temp_adc_init(void) void temp_adc_init(void)
+1 -1
View File
@@ -46,7 +46,7 @@ static uint32_t samples_idx;
#include "subsystems/datalink/telemetry.h" #include "subsystems/datalink/telemetry.h"
static void send_aligner(struct transport_tx *trans, struct device *dev) { static void send_aligner(struct transport_tx *trans, struct device *dev) {
DOWNLINK_SEND_FILTER_ALIGNER(DefaultChannel, DefaultDevice, pprz_msg_send_FILTER_ALIGNER(trans, dev, AC_ID,
&ahrs_aligner.lp_gyro.p, &ahrs_aligner.lp_gyro.p,
&ahrs_aligner.lp_gyro.q, &ahrs_aligner.lp_gyro.q,
&ahrs_aligner.lp_gyro.r, &ahrs_aligner.lp_gyro.r,
+1 -1
View File
@@ -50,7 +50,7 @@ unsigned char buffer[4096]; //Packet buffer
#include "subsystems/datalink/telemetry.h" #include "subsystems/datalink/telemetry.h"
static void send_ahrs_ad2(struct transport_tx *trans, struct device *dev) { static void send_ahrs_ad2(struct transport_tx *trans, struct device *dev) {
DOWNLINK_SEND_AHRS_ARDRONE2(DefaultChannel, DefaultDevice, pprz_msg_send_AHRS_ARDRONE2(trans, dev, AC_ID,
&ahrs_impl.state, &ahrs_impl.state,
&ahrs_impl.control_state, &ahrs_impl.control_state,
&ahrs_impl.eulers.phi, &ahrs_impl.eulers.phi,
@@ -99,7 +99,7 @@ static void send_att(struct transport_tx *trans, struct device *dev) {
struct Int32Eulers euler_i; struct Int32Eulers euler_i;
EULERS_BFP_OF_REAL(euler_i, ltp_to_imu_euler); EULERS_BFP_OF_REAL(euler_i, ltp_to_imu_euler);
struct Int32Eulers* eulers_body = stateGetNedToBodyEulers_i(); struct Int32Eulers* eulers_body = stateGetNedToBodyEulers_i();
DOWNLINK_SEND_AHRS_EULER_INT(DefaultChannel, DefaultDevice, pprz_msg_send_AHRS_EULER_INT(trans, dev, AC_ID,
&euler_i.phi, &euler_i.phi,
&euler_i.theta, &euler_i.theta,
&euler_i.psi, &euler_i.psi,
@@ -109,7 +109,7 @@ static void send_att(struct transport_tx *trans, struct device *dev) {
} }
static void send_geo_mag(struct transport_tx *trans, struct device *dev) { static void send_geo_mag(struct transport_tx *trans, struct device *dev) {
DOWNLINK_SEND_GEO_MAG(DefaultChannel, DefaultDevice, pprz_msg_send_GEO_MAG(trans, dev, AC_ID,
&ahrs_impl.mag_h.x, &ahrs_impl.mag_h.y, &ahrs_impl.mag_h.z); &ahrs_impl.mag_h.x, &ahrs_impl.mag_h.y, &ahrs_impl.mag_h.z);
} }
@@ -117,7 +117,7 @@ static void send_geo_mag(struct transport_tx *trans, struct device *dev) {
/* /*
static void send_rmat(struct transport_tx *trans, struct device *dev) { static void send_rmat(struct transport_tx *trans, struct device *dev) {
struct Int32RMat* att_rmat = stateGetNedToBodyRMat_i(); struct Int32RMat* att_rmat = stateGetNedToBodyRMat_i();
DOWNLINK_SEND_AHRS_RMAT(DefaultChannel, DefaultDevice, pprz_msg_send_AHRS_RMAT(trans, dev, AC_ID,
&ahrs_impl.ltp_to_imu_rmat.m[0], &ahrs_impl.ltp_to_imu_rmat.m[0],
&ahrs_impl.ltp_to_imu_rmat.m[1], &ahrs_impl.ltp_to_imu_rmat.m[1],
&ahrs_impl.ltp_to_imu_rmat.m[2], &ahrs_impl.ltp_to_imu_rmat.m[2],
@@ -64,7 +64,7 @@ struct AhrsMlkf ahrs_impl;
#include "subsystems/datalink/telemetry.h" #include "subsystems/datalink/telemetry.h"
static void send_geo_mag(struct transport_tx *trans, struct device *dev) { static void send_geo_mag(struct transport_tx *trans, struct device *dev) {
DOWNLINK_SEND_GEO_MAG(DefaultChannel, DefaultDevice, pprz_msg_send_GEO_MAG(trans, dev, AC_ID,
&ahrs_impl.mag_h.x, &ahrs_impl.mag_h.y, &ahrs_impl.mag_h.z); &ahrs_impl.mag_h.x, &ahrs_impl.mag_h.y, &ahrs_impl.mag_h.z);
} }
#endif #endif
+2 -2
View File
@@ -83,8 +83,8 @@ void ahrs_align(void) {
#if PERIODIC_TELEMETRY #if PERIODIC_TELEMETRY
#include "subsystems/datalink/telemetry.h" #include "subsystems/datalink/telemetry.h"
static send_gx3(void) { static send_gx3(struct transport_tx *trans, struct device *dev) {
DOWNLINK_SEND_GX3_INFO(DefaultChannel, DefaultDevice, pprz_msg_send_GX3_INFO(trans, dev, AC_ID,
&ahrs_impl.gx3_freq, &ahrs_impl.gx3_freq,
&ahrs_impl.gx3_packet.chksm_error, &ahrs_impl.gx3_packet.chksm_error,
&ahrs_impl.gx3_packet.hdr_error, &ahrs_impl.gx3_packet.hdr_error,
+2 -2
View File
@@ -43,7 +43,7 @@ float heading;
#include "subsystems/datalink/telemetry.h" #include "subsystems/datalink/telemetry.h"
static void send_infrared(struct transport_tx *trans, struct device *dev) { static void send_infrared(struct transport_tx *trans, struct device *dev) {
DOWNLINK_SEND_IR_SENSORS(DefaultChannel, DefaultDevice, pprz_msg_send_IR_SENSORS(trans, dev, AC_ID,
&infrared.value.ir1, &infrared.value.ir2, &infrared.pitch, &infrared.roll, &infrared.top); &infrared.value.ir1, &infrared.value.ir2, &infrared.pitch, &infrared.roll, &infrared.top);
} }
@@ -51,7 +51,7 @@ static void send_status(struct transport_tx *trans, struct device *dev) {
uint16_t contrast = abs(infrared.roll) + abs(infrared.pitch) + abs(infrared.top); uint16_t contrast = abs(infrared.roll) + abs(infrared.pitch) + abs(infrared.top);
uint8_t mde = 3; uint8_t mde = 3;
if (contrast < 50) mde = 7; if (contrast < 50) mde = 7;
DOWNLINK_SEND_STATE_FILTER_STATUS(DefaultChannel, DefaultDevice, &mde, &contrast); pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &mde, &contrast);
} }
#endif #endif
@@ -65,7 +65,7 @@ static inline void set_body_state_from_euler(void);
#include "subsystems/datalink/telemetry.h" #include "subsystems/datalink/telemetry.h"
static void send_filter(struct transport_tx *trans, struct device *dev) { static void send_filter(struct transport_tx *trans, struct device *dev) {
DOWNLINK_SEND_FILTER(DefaultChannel, DefaultDevice, pprz_msg_send_FILTER(trans, dev, AC_ID,
&ahrs_impl.ltp_to_imu_euler.phi, &ahrs_impl.ltp_to_imu_euler.phi,
&ahrs_impl.ltp_to_imu_euler.theta, &ahrs_impl.ltp_to_imu_euler.theta,
&ahrs_impl.ltp_to_imu_euler.psi, &ahrs_impl.ltp_to_imu_euler.psi,
@@ -85,7 +85,7 @@ static void send_filter(struct transport_tx *trans, struct device *dev) {
static void send_euler(struct transport_tx *trans, struct device *dev) { static void send_euler(struct transport_tx *trans, struct device *dev) {
struct Int32Eulers* eulers = stateGetNedToBodyEulers_i(); struct Int32Eulers* eulers = stateGetNedToBodyEulers_i();
DOWNLINK_SEND_AHRS_EULER_INT(DefaultChannel, DefaultDevice, pprz_msg_send_AHRS_EULER_INT(trans, dev, AC_ID,
&ahrs_impl.ltp_to_imu_euler.phi, &ahrs_impl.ltp_to_imu_euler.phi,
&ahrs_impl.ltp_to_imu_euler.theta, &ahrs_impl.ltp_to_imu_euler.theta,
&ahrs_impl.ltp_to_imu_euler.psi, &ahrs_impl.ltp_to_imu_euler.psi,
@@ -95,7 +95,7 @@ static void send_euler(struct transport_tx *trans, struct device *dev) {
} }
static void send_bias(struct transport_tx *trans, struct device *dev) { static void send_bias(struct transport_tx *trans, struct device *dev) {
DOWNLINK_SEND_AHRS_GYRO_BIAS_INT(DefaultChannel, DefaultDevice, pprz_msg_send_AHRS_GYRO_BIAS_INT(trans, dev, AC_ID,
&ahrs_impl.gyro_bias.p, &ahrs_impl.gyro_bias.q, &ahrs_impl.gyro_bias.r); &ahrs_impl.gyro_bias.p, &ahrs_impl.gyro_bias.q, &ahrs_impl.gyro_bias.r);
} }
#endif #endif
@@ -116,7 +116,7 @@ static inline void ahrs_update_mag_2d(float dt);
static void send_quat(struct transport_tx *trans, struct device *dev) { static void send_quat(struct transport_tx *trans, struct device *dev) {
struct Int32Quat* quat = stateGetNedToBodyQuat_i(); struct Int32Quat* quat = stateGetNedToBodyQuat_i();
DOWNLINK_SEND_AHRS_QUAT_INT(DefaultChannel, DefaultDevice, pprz_msg_send_AHRS_QUAT_INT(trans, dev, AC_ID,
&ahrs_impl.weight, &ahrs_impl.weight,
&ahrs_impl.ltp_to_imu_quat.qi, &ahrs_impl.ltp_to_imu_quat.qi,
&ahrs_impl.ltp_to_imu_quat.qx, &ahrs_impl.ltp_to_imu_quat.qx,
@@ -132,7 +132,7 @@ static void send_euler(struct transport_tx *trans, struct device *dev) {
struct Int32Eulers ltp_to_imu_euler; struct Int32Eulers ltp_to_imu_euler;
int32_eulers_of_quat(&ltp_to_imu_euler, &ahrs_impl.ltp_to_imu_quat); int32_eulers_of_quat(&ltp_to_imu_euler, &ahrs_impl.ltp_to_imu_quat);
struct Int32Eulers* eulers = stateGetNedToBodyEulers_i(); struct Int32Eulers* eulers = stateGetNedToBodyEulers_i();
DOWNLINK_SEND_AHRS_EULER_INT(DefaultChannel, DefaultDevice, pprz_msg_send_AHRS_EULER_INT(trans, dev, AC_ID,
&ltp_to_imu_euler.phi, &ltp_to_imu_euler.phi,
&ltp_to_imu_euler.theta, &ltp_to_imu_euler.theta,
&ltp_to_imu_euler.psi, &ltp_to_imu_euler.psi,
@@ -142,7 +142,7 @@ static void send_euler(struct transport_tx *trans, struct device *dev) {
} }
static void send_bias(struct transport_tx *trans, struct device *dev) { static void send_bias(struct transport_tx *trans, struct device *dev) {
DOWNLINK_SEND_AHRS_GYRO_BIAS_INT(DefaultChannel, DefaultDevice, pprz_msg_send_AHRS_GYRO_BIAS_INT(trans, dev, AC_ID,
&ahrs_impl.gyro_bias.p, &ahrs_impl.gyro_bias.q, &ahrs_impl.gyro_bias.r); &ahrs_impl.gyro_bias.p, &ahrs_impl.gyro_bias.q, &ahrs_impl.gyro_bias.r);
} }
@@ -151,7 +151,7 @@ static void send_geo_mag(struct transport_tx *trans, struct device *dev) {
h_float.x = MAG_FLOAT_OF_BFP(ahrs_impl.mag_h.x); h_float.x = MAG_FLOAT_OF_BFP(ahrs_impl.mag_h.x);
h_float.y = MAG_FLOAT_OF_BFP(ahrs_impl.mag_h.y); h_float.y = MAG_FLOAT_OF_BFP(ahrs_impl.mag_h.y);
h_float.z = MAG_FLOAT_OF_BFP(ahrs_impl.mag_h.z); h_float.z = MAG_FLOAT_OF_BFP(ahrs_impl.mag_h.z);
DOWNLINK_SEND_GEO_MAG(DefaultChannel, DefaultDevice, pprz_msg_send_GEO_MAG(trans, dev, AC_ID,
&h_float.x, &h_float.y, &h_float.z); &h_float.x, &h_float.y, &h_float.z);
} }
#endif #endif
+1 -1
View File
@@ -181,7 +181,7 @@ static const uint8_t pn_bind[] = { 0x98, 0x88, 0x1B, 0xE4, 0x30, 0x79, 0x03, 0x8
static void send_superbit(struct transport_tx *trans, struct device *dev) { static void send_superbit(struct transport_tx *trans, struct device *dev) {
uint8_t status = superbitrf.status; uint8_t status = superbitrf.status;
uint8_t cyrf6936_status = superbitrf.cyrf6936.status; uint8_t cyrf6936_status = superbitrf.cyrf6936.status;
DOWNLINK_SEND_SUPERBITRF(DefaultChannel, DefaultDevice, pprz_msg_send_SUPERBITRF(trans, dev, AC_ID,
&status, &status,
&cyrf6936_status, &cyrf6936_status,
&superbitrf.irq_count, &superbitrf.irq_count,
+6 -6
View File
@@ -49,14 +49,14 @@ static void send_gps(struct transport_tx *trans, struct device *dev) {
static uint8_t i; static uint8_t i;
int16_t climb = -gps.ned_vel.z; int16_t climb = -gps.ned_vel.z;
int16_t course = (DegOfRad(gps.course)/((int32_t)1e6)); int16_t course = (DegOfRad(gps.course)/((int32_t)1e6));
DOWNLINK_SEND_GPS(DefaultChannel, DefaultDevice, &gps.fix, pprz_msg_send_GPS(trans, dev, AC_ID, &gps.fix,
&gps.utm_pos.east, &gps.utm_pos.north, &gps.utm_pos.east, &gps.utm_pos.north,
&course, &gps.hmsl, &gps.gspeed, &climb, &course, &gps.hmsl, &gps.gspeed, &climb,
&gps.week, &gps.tow, &gps.utm_pos.zone, &i); &gps.week, &gps.tow, &gps.utm_pos.zone, &i);
if ((gps.fix != GPS_FIX_3D) && (i >= gps.nb_channels)) i = 0; 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 * 2) i = 0;
if (i < gps.nb_channels && ((gps.fix != GPS_FIX_3D) || (gps.svinfos[i].cno > 0))) { if (i < gps.nb_channels && ((gps.fix != GPS_FIX_3D) || (gps.svinfos[i].cno > 0))) {
DOWNLINK_SEND_SVINFO(DefaultChannel, DefaultDevice, &i, pprz_msg_send_SVINFO(trans, dev, AC_ID, &i,
&gps.svinfos[i].svid, &gps.svinfos[i].flags, &gps.svinfos[i].svid, &gps.svinfos[i].flags,
&gps.svinfos[i].qi, &gps.svinfos[i].cno, &gps.svinfos[i].qi, &gps.svinfos[i].cno,
&gps.svinfos[i].elev, &gps.svinfos[i].azim); &gps.svinfos[i].elev, &gps.svinfos[i].azim);
@@ -67,7 +67,7 @@ static void send_gps(struct transport_tx *trans, struct device *dev) {
static void send_gps_int(struct transport_tx *trans, struct device *dev) { static void send_gps_int(struct transport_tx *trans, struct device *dev) {
static uint8_t i; static uint8_t i;
static uint8_t last_cnos[GPS_NB_CHANNELS]; static uint8_t last_cnos[GPS_NB_CHANNELS];
DOWNLINK_SEND_GPS_INT(DefaultChannel, DefaultDevice, pprz_msg_send_GPS_INT(trans, dev, AC_ID,
&gps.ecef_pos.x, &gps.ecef_pos.y, &gps.ecef_pos.z, &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.lla_pos.lat, &gps.lla_pos.lon, &gps.lla_pos.alt,
&gps.hmsl, &gps.hmsl,
@@ -79,7 +79,7 @@ static void send_gps_int(struct transport_tx *trans, struct device *dev) {
&gps.fix); &gps.fix);
if (i == gps.nb_channels) i = 0; if (i == gps.nb_channels) i = 0;
if (i < gps.nb_channels && gps.svinfos[i].cno > 0 && gps.svinfos[i].cno != last_cnos[i]) { if (i < gps.nb_channels && gps.svinfos[i].cno > 0 && gps.svinfos[i].cno != last_cnos[i]) {
DOWNLINK_SEND_SVINFO(DefaultChannel, DefaultDevice, &i, pprz_msg_send_SVINFO(trans, dev, AC_ID, &i,
&gps.svinfos[i].svid, &gps.svinfos[i].flags, &gps.svinfos[i].svid, &gps.svinfos[i].flags,
&gps.svinfos[i].qi, &gps.svinfos[i].cno, &gps.svinfos[i].qi, &gps.svinfos[i].cno,
&gps.svinfos[i].elev, &gps.svinfos[i].azim); &gps.svinfos[i].elev, &gps.svinfos[i].azim);
@@ -92,7 +92,7 @@ static void send_gps_lla(struct transport_tx *trans, struct device *dev) {
uint8_t err = 0; uint8_t err = 0;
int16_t climb = -gps.ned_vel.z; int16_t climb = -gps.ned_vel.z;
int16_t course = (DegOfRad(gps.course)/((int32_t)1e6)); int16_t course = (DegOfRad(gps.course)/((int32_t)1e6));
DOWNLINK_SEND_GPS_LLA(DefaultChannel, DefaultDevice, pprz_msg_send_GPS_LLA(trans, dev, AC_ID,
&gps.lla_pos.lat, &gps.lla_pos.lon, &gps.lla_pos.alt, &gps.lla_pos.lat, &gps.lla_pos.lon, &gps.lla_pos.alt,
&course, &gps.gspeed, &climb, &course, &gps.gspeed, &climb,
&gps.week, &gps.tow, &gps.week, &gps.tow,
@@ -100,7 +100,7 @@ static void send_gps_lla(struct transport_tx *trans, struct device *dev) {
} }
static void send_gps_sol(struct transport_tx *trans, struct device *dev) { static void send_gps_sol(struct transport_tx *trans, struct device *dev) {
DOWNLINK_SEND_GPS_SOL(DefaultChannel, DefaultDevice, &gps.pacc, &gps.sacc, &gps.pdop, &gps.num_sv); pprz_msg_send_GPS_SOL(trans, dev, AC_ID, &gps.pacc, &gps.sacc, &gps.pdop, &gps.num_sv);
} }
#endif #endif
+11 -11
View File
@@ -45,65 +45,65 @@
#if USE_IMU_FLOAT #if USE_IMU_FLOAT
static void send_accel(struct transport_tx *trans, struct device *dev) { static void send_accel(struct transport_tx *trans, struct device *dev) {
DOWNLINK_SEND_IMU_ACCEL(DefaultChannel, DefaultDevice, pprz_msg_send_IMU_ACCEL(trans, dev, AC_ID,
&imuf.accel.x, &imuf.accel.y, &imuf.accel.z); &imuf.accel.x, &imuf.accel.y, &imuf.accel.z);
} }
static void send_gyro(struct transport_tx *trans, struct device *dev) { static void send_gyro(struct transport_tx *trans, struct device *dev) {
DOWNLINK_SEND_IMU_GYRO(DefaultChannel, DefaultDevice, pprz_msg_send_IMU_GYRO(trans, dev, AC_ID,
&imuf.gyro.p, &imuf.gyro.q, &imuf.gyro.r); &imuf.gyro.p, &imuf.gyro.q, &imuf.gyro.r);
} }
#else // !USE_IMU_FLOAT #else // !USE_IMU_FLOAT
static void send_accel_raw(struct transport_tx *trans, struct device *dev) { static void send_accel_raw(struct transport_tx *trans, struct device *dev) {
DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel, DefaultDevice, pprz_msg_send_IMU_ACCEL_RAW(trans, dev, AC_ID,
&imu.accel_unscaled.x, &imu.accel_unscaled.y, &imu.accel_unscaled.z); &imu.accel_unscaled.x, &imu.accel_unscaled.y, &imu.accel_unscaled.z);
} }
static void send_accel_scaled(struct transport_tx *trans, struct device *dev) { static void send_accel_scaled(struct transport_tx *trans, struct device *dev) {
DOWNLINK_SEND_IMU_ACCEL_SCALED(DefaultChannel, DefaultDevice, pprz_msg_send_IMU_ACCEL_SCALED(trans, dev, AC_ID,
&imu.accel.x, &imu.accel.y, &imu.accel.z); &imu.accel.x, &imu.accel.y, &imu.accel.z);
} }
static void send_accel(struct transport_tx *trans, struct device *dev) { static void send_accel(struct transport_tx *trans, struct device *dev) {
struct FloatVect3 accel_float; struct FloatVect3 accel_float;
ACCELS_FLOAT_OF_BFP(accel_float, imu.accel); ACCELS_FLOAT_OF_BFP(accel_float, imu.accel);
DOWNLINK_SEND_IMU_ACCEL(DefaultChannel, DefaultDevice, pprz_msg_send_IMU_ACCEL(trans, dev, AC_ID,
&accel_float.x, &accel_float.y, &accel_float.z); &accel_float.x, &accel_float.y, &accel_float.z);
} }
static void send_gyro_raw(struct transport_tx *trans, struct device *dev) { static void send_gyro_raw(struct transport_tx *trans, struct device *dev) {
DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel, DefaultDevice, pprz_msg_send_IMU_GYRO_RAW(trans, dev, AC_ID,
&imu.gyro_unscaled.p, &imu.gyro_unscaled.q, &imu.gyro_unscaled.r); &imu.gyro_unscaled.p, &imu.gyro_unscaled.q, &imu.gyro_unscaled.r);
} }
static void send_gyro_scaled(struct transport_tx *trans, struct device *dev) { static void send_gyro_scaled(struct transport_tx *trans, struct device *dev) {
DOWNLINK_SEND_IMU_GYRO_SCALED(DefaultChannel, DefaultDevice, pprz_msg_send_IMU_GYRO_SCALED(trans, dev, AC_ID,
&imu.gyro.p, &imu.gyro.q, &imu.gyro.r); &imu.gyro.p, &imu.gyro.q, &imu.gyro.r);
} }
static void send_gyro(struct transport_tx *trans, struct device *dev) { static void send_gyro(struct transport_tx *trans, struct device *dev) {
struct FloatRates gyro_float; struct FloatRates gyro_float;
RATES_FLOAT_OF_BFP(gyro_float, imu.gyro); RATES_FLOAT_OF_BFP(gyro_float, imu.gyro);
DOWNLINK_SEND_IMU_GYRO(DefaultChannel, DefaultDevice, pprz_msg_send_IMU_GYRO(trans, dev, AC_ID,
&gyro_float.p, &gyro_float.q, &gyro_float.r); &gyro_float.p, &gyro_float.q, &gyro_float.r);
} }
static void send_mag_raw(struct transport_tx *trans, struct device *dev) { static void send_mag_raw(struct transport_tx *trans, struct device *dev) {
DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice, pprz_msg_send_IMU_MAG_RAW(trans, dev, AC_ID,
&imu.mag_unscaled.x, &imu.mag_unscaled.y, &imu.mag_unscaled.z); &imu.mag_unscaled.x, &imu.mag_unscaled.y, &imu.mag_unscaled.z);
} }
static void send_mag_scaled(struct transport_tx *trans, struct device *dev) { static void send_mag_scaled(struct transport_tx *trans, struct device *dev) {
DOWNLINK_SEND_IMU_MAG_SCALED(DefaultChannel, DefaultDevice, pprz_msg_send_IMU_MAG_SCALED(trans, dev, AC_ID,
&imu.mag.x, &imu.mag.y, &imu.mag.z); &imu.mag.x, &imu.mag.y, &imu.mag.z);
} }
static void send_mag(struct transport_tx *trans, struct device *dev) { static void send_mag(struct transport_tx *trans, struct device *dev) {
struct FloatVect3 mag_float; struct FloatVect3 mag_float;
MAGS_FLOAT_OF_BFP(mag_float, imu.mag); MAGS_FLOAT_OF_BFP(mag_float, imu.mag);
DOWNLINK_SEND_IMU_MAG(DefaultChannel, DefaultDevice, pprz_msg_send_IMU_MAG(trans, dev, AC_ID,
&mag_float.x, &mag_float.y, &mag_float.z); &mag_float.x, &mag_float.y, &mag_float.z);
} }
#endif // !USE_IMU_FLOAT #endif // !USE_IMU_FLOAT
+3 -3
View File
@@ -230,7 +230,7 @@ static void b2_hff_update_ydot(struct HfilterFloat* hff_work, float vel, float R
#include "subsystems/datalink/telemetry.h" #include "subsystems/datalink/telemetry.h"
static void send_hff(struct transport_tx *trans, struct device *dev) { static void send_hff(struct transport_tx *trans, struct device *dev) {
DOWNLINK_SEND_HFF(DefaultChannel, DefaultDevice, pprz_msg_send_HFF(trans, dev, AC_ID,
&b2_hff_state.x, &b2_hff_state.x,
&b2_hff_state.y, &b2_hff_state.y,
&b2_hff_state.xdot, &b2_hff_state.xdot,
@@ -240,7 +240,7 @@ static void send_hff(struct transport_tx *trans, struct device *dev) {
} }
static void send_hff_debug(struct transport_tx *trans, struct device *dev) { static void send_hff_debug(struct transport_tx *trans, struct device *dev) {
DOWNLINK_SEND_HFF_DBG(DefaultChannel, DefaultDevice, pprz_msg_send_HFF_DBG(trans, dev, AC_ID,
&b2_hff_x_meas, &b2_hff_x_meas,
&b2_hff_y_meas, &b2_hff_y_meas,
&b2_hff_xd_meas, &b2_hff_xd_meas,
@@ -253,7 +253,7 @@ static void send_hff_debug(struct transport_tx *trans, struct device *dev) {
#ifdef GPS_LAG #ifdef GPS_LAG
static void send_hff_gps(struct transport_tx *trans, struct device *dev) { static void send_hff_gps(struct transport_tx *trans, struct device *dev) {
DOWNLINK_SEND_HFF_GPS(DefaultChannel, DefaultDevice, pprz_msg_send_HFF_GPS(trans, dev, AC_ID,
&(b2_hff_rb_last->lag_counter), &(b2_hff_rb_last->lag_counter),
&lag_counter_err, &lag_counter_err,
&save_counter); &save_counter);
@@ -65,7 +65,7 @@
static void send_ins_ref(struct transport_tx *trans, struct device *dev) { static void send_ins_ref(struct transport_tx *trans, struct device *dev) {
float foo = 0.; float foo = 0.;
if (state.ned_initialized_i) { if (state.ned_initialized_i) {
DOWNLINK_SEND_INS_REF(DefaultChannel, DefaultDevice, pprz_msg_send_INS_REF(trans, dev, AC_ID,
&state.ned_origin_i.ecef.x, &state.ned_origin_i.ecef.y, &state.ned_origin_i.ecef.z, &state.ned_origin_i.ecef.x, &state.ned_origin_i.ecef.y, &state.ned_origin_i.ecef.z,
&state.ned_origin_i.lla.lat, &state.ned_origin_i.lla.lon, &state.ned_origin_i.lla.alt, &state.ned_origin_i.lla.lat, &state.ned_origin_i.lla.lon, &state.ned_origin_i.lla.alt,
&state.ned_origin_i.hmsl, &foo); &state.ned_origin_i.hmsl, &foo);
@@ -396,7 +396,7 @@ void ahrs_propagate(float dt) {
struct FloatEulers eulers; struct FloatEulers eulers;
FLOAT_EULERS_OF_QUAT(eulers, ins_impl.state.quat); FLOAT_EULERS_OF_QUAT(eulers, ins_impl.state.quat);
RunOnceEvery(3,{ RunOnceEvery(3,{
DOWNLINK_SEND_INV_FILTER(DefaultChannel, DefaultDevice, pprz_msg_send_INV_FILTER(trans, dev, AC_ID,
&ins_impl.state.quat.qi, &ins_impl.state.quat.qi,
&eulers.phi, &eulers.phi,
&eulers.theta, &eulers.theta,
@@ -59,7 +59,7 @@ struct InsGpsPassthrough ins_impl;
#include "subsystems/datalink/telemetry.h" #include "subsystems/datalink/telemetry.h"
static void send_ins(struct transport_tx *trans, struct device *dev) { static void send_ins(struct transport_tx *trans, struct device *dev) {
DOWNLINK_SEND_INS(DefaultChannel, DefaultDevice, pprz_msg_send_INS(trans, dev, AC_ID,
&ins_impl.ltp_pos.x, &ins_impl.ltp_pos.y, &ins_impl.ltp_pos.z, &ins_impl.ltp_pos.x, &ins_impl.ltp_pos.y, &ins_impl.ltp_pos.z,
&ins_impl.ltp_speed.x, &ins_impl.ltp_speed.y, &ins_impl.ltp_speed.z, &ins_impl.ltp_speed.x, &ins_impl.ltp_speed.y, &ins_impl.ltp_speed.z,
&ins_impl.ltp_accel.x, &ins_impl.ltp_accel.y, &ins_impl.ltp_accel.z); &ins_impl.ltp_accel.x, &ins_impl.ltp_accel.y, &ins_impl.ltp_accel.z);
@@ -67,14 +67,14 @@ static void send_ins(struct transport_tx *trans, struct device *dev) {
static void send_ins_z(struct transport_tx *trans, struct device *dev) { static void send_ins_z(struct transport_tx *trans, struct device *dev) {
static const float fake_baro_z = 0.0; static const float fake_baro_z = 0.0;
DOWNLINK_SEND_INS_Z(DefaultChannel, DefaultDevice, pprz_msg_send_INS_Z(trans, dev, AC_ID,
&fake_baro_z, &ins_impl.ltp_pos.z, &ins_impl.ltp_speed.z, &ins_impl.ltp_accel.z); &fake_baro_z, &ins_impl.ltp_pos.z, &ins_impl.ltp_speed.z, &ins_impl.ltp_accel.z);
} }
static void send_ins_ref(struct transport_tx *trans, struct device *dev) { static void send_ins_ref(struct transport_tx *trans, struct device *dev) {
static const float fake_qfe = 0.0; static const float fake_qfe = 0.0;
if (ins_impl.ltp_initialized) { if (ins_impl.ltp_initialized) {
DOWNLINK_SEND_INS_REF(DefaultChannel, DefaultDevice, pprz_msg_send_INS_REF(trans, dev, AC_ID,
&ins_impl.ltp_def.ecef.x, &ins_impl.ltp_def.ecef.y, &ins_impl.ltp_def.ecef.z, &ins_impl.ltp_def.ecef.x, &ins_impl.ltp_def.ecef.y, &ins_impl.ltp_def.ecef.z,
&ins_impl.ltp_def.lla.lat, &ins_impl.ltp_def.lla.lon, &ins_impl.ltp_def.lla.alt, &ins_impl.ltp_def.lla.lat, &ins_impl.ltp_def.lla.lon, &ins_impl.ltp_def.lla.alt,
&ins_impl.ltp_def.hmsl, &fake_qfe); &ins_impl.ltp_def.hmsl, &fake_qfe);
+3 -3
View File
@@ -122,20 +122,20 @@ struct InsInt ins_impl;
#include "subsystems/datalink/telemetry.h" #include "subsystems/datalink/telemetry.h"
static void send_ins(struct transport_tx *trans, struct device *dev) { static void send_ins(struct transport_tx *trans, struct device *dev) {
DOWNLINK_SEND_INS(DefaultChannel, DefaultDevice, pprz_msg_send_INS(trans, dev, AC_ID,
&ins_impl.ltp_pos.x, &ins_impl.ltp_pos.y, &ins_impl.ltp_pos.z, &ins_impl.ltp_pos.x, &ins_impl.ltp_pos.y, &ins_impl.ltp_pos.z,
&ins_impl.ltp_speed.x, &ins_impl.ltp_speed.y, &ins_impl.ltp_speed.z, &ins_impl.ltp_speed.x, &ins_impl.ltp_speed.y, &ins_impl.ltp_speed.z,
&ins_impl.ltp_accel.x, &ins_impl.ltp_accel.y, &ins_impl.ltp_accel.z); &ins_impl.ltp_accel.x, &ins_impl.ltp_accel.y, &ins_impl.ltp_accel.z);
} }
static void send_ins_z(struct transport_tx *trans, struct device *dev) { static void send_ins_z(struct transport_tx *trans, struct device *dev) {
DOWNLINK_SEND_INS_Z(DefaultChannel, DefaultDevice, pprz_msg_send_INS_Z(trans, dev, AC_ID,
&ins_impl.baro_z, &ins_impl.ltp_pos.z, &ins_impl.ltp_speed.z, &ins_impl.ltp_accel.z); &ins_impl.baro_z, &ins_impl.ltp_pos.z, &ins_impl.ltp_speed.z, &ins_impl.ltp_accel.z);
} }
static void send_ins_ref(struct transport_tx *trans, struct device *dev) { static void send_ins_ref(struct transport_tx *trans, struct device *dev) {
if (ins_impl.ltp_initialized) { if (ins_impl.ltp_initialized) {
DOWNLINK_SEND_INS_REF(DefaultChannel, DefaultDevice, pprz_msg_send_INS_REF(trans, dev, AC_ID,
&ins_impl.ltp_def.ecef.x, &ins_impl.ltp_def.ecef.y, &ins_impl.ltp_def.ecef.z, &ins_impl.ltp_def.ecef.x, &ins_impl.ltp_def.ecef.y, &ins_impl.ltp_def.ecef.z,
&ins_impl.ltp_def.lla.lat, &ins_impl.ltp_def.lla.lon, &ins_impl.ltp_def.lla.alt, &ins_impl.ltp_def.lla.lat, &ins_impl.ltp_def.lla.lon, &ins_impl.ltp_def.lla.alt,
&ins_impl.ltp_def.hmsl, &ins_impl.qfe); &ins_impl.ltp_def.hmsl, &ins_impl.qfe);
@@ -70,7 +70,7 @@ struct VffExtended vff;
#include "subsystems/datalink/telemetry.h" #include "subsystems/datalink/telemetry.h"
static void send_vffe(struct transport_tx *trans, struct device *dev) { static void send_vffe(struct transport_tx *trans, struct device *dev) {
DOWNLINK_SEND_VFF_EXTENDED(DefaultChannel, DefaultDevice, pprz_msg_send_VFF_EXTENDED(trans, dev, AC_ID,
&vff.z_meas, &vff.z_meas_baro, &vff.z_meas, &vff.z_meas_baro,
&vff.z, &vff.zdot, &vff.zdotdot, &vff.z, &vff.zdot, &vff.zdotdot,
&vff.bias, &vff.offset); &vff.bias, &vff.offset);
@@ -150,7 +150,7 @@ void vff_propagate(float accel, float dt) {
vff.P[3][3] = FPF33 + Qoffoff; vff.P[3][3] = FPF33 + Qoffoff;
#if DEBUG_VFF_EXTENDED #if DEBUG_VFF_EXTENDED
RunOnceEvery(10, send_vffe()); RunOnceEvery(10, send_vffe(&(DefaultChannel).trans_tx, &(DefaultDevice).device));
#endif #endif
} }
+1 -1
View File
@@ -63,7 +63,7 @@ struct Vff vff;
#include "subsystems/datalink/telemetry.h" #include "subsystems/datalink/telemetry.h"
static void send_vff(struct transport_tx *trans, struct device *dev) { static void send_vff(struct transport_tx *trans, struct device *dev) {
DOWNLINK_SEND_VFF(DefaultChannel, DefaultDevice, pprz_msg_send_VFF(trans, dev, AC_ID,
&vff.z_meas, &vff.z, &vff.zdot, &vff.bias, &vff.z_meas, &vff.z, &vff.zdot, &vff.bias,
&vff.P[0][0], &vff.P[1][1], &vff.P[2][2]); &vff.P[0][0], &vff.P[1][1], &vff.P[2][2]);
} }
+1 -1
View File
@@ -64,7 +64,7 @@ static void send_ppm(struct transport_tx *trans, struct device *dev)
for (int i = 0; i < RADIO_CONTROL_NB_CHANNEL; i++) { for (int i = 0; i < RADIO_CONTROL_NB_CHANNEL; i++) {
ppm_pulses_usec[i] = USEC_OF_RC_PPM_TICKS(ppm_pulses[i]); ppm_pulses_usec[i] = USEC_OF_RC_PPM_TICKS(ppm_pulses[i]);
} }
DOWNLINK_SEND_PPM(DefaultChannel, DefaultDevice, pprz_msg_send_PPM(trans, dev, AC_ID,
&radio_control.frame_rate, PPM_NB_CHANNEL, ppm_pulses_usec); &radio_control.frame_rate, PPM_NB_CHANNEL, ppm_pulses_usec);
} }
#endif #endif
+1 -1
View File
@@ -45,7 +45,7 @@ struct Sbus sbus;
static void send_sbus(struct transport_tx *trans, struct device *dev) static void send_sbus(struct transport_tx *trans, struct device *dev)
{ {
// Using PPM message // Using PPM message
DOWNLINK_SEND_PPM(DefaultChannel, DefaultDevice, pprz_msg_send_PPM(trans, dev, AC_ID,
&radio_control.frame_rate, SBUS_NB_CHANNEL, sbus.ppm); &radio_control.frame_rate, SBUS_NB_CHANNEL, sbus.ppm);
} }
#endif #endif
@@ -48,7 +48,7 @@ struct Sbus sbus1, sbus2;
static void send_sbus(struct transport_tx *trans, struct device *dev) static void send_sbus(struct transport_tx *trans, struct device *dev)
{ {
// Using PPM message // Using PPM message
DOWNLINK_SEND_PPM(DefaultChannel, DefaultDevice, pprz_msg_send_PPM(trans, dev, AC_ID,
&radio_control.frame_rate, SBUS_NB_CHANNEL, sbus1.ppm); &radio_control.frame_rate, SBUS_NB_CHANNEL, sbus1.ppm);
} }
#endif #endif
+1 -1
View File
@@ -124,7 +124,7 @@ let print_dl_settings = fun settings ->
lprintf "default: var = 0.; break;\\\n"; lprintf "default: var = 0.; break;\\\n";
left (); left ();
lprintf "}\\\n"; lprintf "}\\\n";
lprintf "DOWNLINK_SEND_DL_VALUE(_trans, _dev, &i, &var);\\\n"; lprintf "pprz_msg_send_DL_VALUE(_trans, _dev, AC_ID, &i, &var);\\\n";
lprintf "i++;\\\n"; lprintf "i++;\\\n";
left () left ()
end; end;