mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-29 19:17:28 +08:00
[telemetry] convert macros in registered callback to functions
This commit is contained in:
@@ -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;
|
||||||
|
|||||||
@@ -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, &s,
|
&v_ctl_throttle_slewed, &vsupply, &s,
|
||||||
&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);
|
||||||
|
|||||||
@@ -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);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -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
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -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
|
||||||
|
|
||||||
|
|||||||
@@ -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,
|
||||||
|
|||||||
@@ -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,
|
||||||
|
|||||||
@@ -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
|
||||||
|
|||||||
@@ -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
|
||||||
|
|||||||
@@ -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
|
||||||
|
|||||||
@@ -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:
|
||||||
|
|||||||
@@ -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;
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -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
|
||||||
|
|
||||||
|
|||||||
@@ -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);
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -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)
|
||||||
|
|||||||
@@ -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,
|
||||||
|
|||||||
@@ -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
|
||||||
|
|||||||
@@ -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,
|
||||||
|
|||||||
@@ -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(<p_to_imu_euler, &ahrs_impl.ltp_to_imu_quat);
|
int32_eulers_of_quat(<p_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,
|
||||||
<p_to_imu_euler.phi,
|
<p_to_imu_euler.phi,
|
||||||
<p_to_imu_euler.theta,
|
<p_to_imu_euler.theta,
|
||||||
<p_to_imu_euler.psi,
|
<p_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
|
||||||
|
|||||||
@@ -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,
|
||||||
|
|||||||
@@ -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
|
||||||
|
|
||||||
|
|||||||
@@ -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
|
||||||
|
|||||||
@@ -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);
|
||||||
|
|||||||
@@ -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
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -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]);
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -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
|
||||||
|
|||||||
@@ -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
|
||||||
|
|||||||
@@ -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;
|
||||||
|
|||||||
Reference in New Issue
Block a user