Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-04 05:42:49 +08:00
Code Issues Actions 25 Packages Projects Releases Wiki Activity

[telemetry] adapt rotorcraft to new telemetry system

Browse Source
This commit is contained in:
Gautier Hattenberger
2013-06-14 09:49:16 +02:00
parent 1bff48ec2d
commit 8377d60184
32 changed files with 1038 additions and 1076 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/firmwares/subsystems/rotorcraft/telemetry_transparent.makefile
+2 -2
View File
@@ -9,6 +9,6 @@ ap.CFLAGS += -DUSE_$(MODEM_PORT)
ap.CFLAGS += -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD)
ap.CFLAGS += -DDOWNLINK -DDOWNLINK_DEVICE=$(MODEM_PORT) -DPPRZ_UART=$(MODEM_PORT)
ap.CFLAGS += -DDOWNLINK_TRANSPORT=PprzTransport -DDATALINK=PPRZ
ap.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
ap.CFLAGS += -DDOWNLINK_TRANSPORT=PprzTransport -DDATALINK=PPRZ -DDefaultPeriodic='&telemetry_Main'
ap.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c subsystems/datalink/telemetry.c
ap.srcs += $(SRC_FIRMWARE)/datalink.c $(SRC_FIRMWARE)/telemetry.c
conf/firmwares/subsystems/rotorcraft/telemetry_transparent_usb.makefile
+2 -2
View File
@@ -3,8 +3,8 @@
ifeq ($(ARCH), lpc21)
ap.CFLAGS += -DDOWNLINK -DDOWNLINK_DEVICE=UsbS -DPPRZ_UART=UsbS
ap.CFLAGS += -DDOWNLINK_TRANSPORT=PprzTransport -DDATALINK=PPRZ -DUSE_USB_SERIAL
ap.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
ap.CFLAGS += -DDOWNLINK_TRANSPORT=PprzTransport -DDATALINK=PPRZ -DUSE_USB_SERIAL -DDefaultPeriodic='&telemetry_Main'
ap.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c subsystems/datalink/telemetry.c
ap.srcs += $(SRC_FIRMWARE)/datalink.c $(SRC_FIRMWARE)/telemetry.c
ap.srcs += $(SRC_ARCH)/usb_ser_hw.c $(SRC_ARCH)/lpcusb/usbhw_lpc.c $(SRC_ARCH)/lpcusb/usbcontrol.c
ap.srcs += $(SRC_ARCH)/lpcusb/usbstdreq.c $(SRC_ARCH)/lpcusb/usbinit.c
conf/firmwares/subsystems/rotorcraft/telemetry_udp.makefile
+2 -2
View File
@@ -2,7 +2,7 @@
# Udp telemetry
ap.CFLAGS += -DDOWNLINK -DDOWNLINK_DEVICE=Udp
ap.CFLAGS += -DDOWNLINK_TRANSPORT=PprzTransport -DDATALINK=UDP
ap.srcs += subsystems/datalink/downlink.c subsystems/datalink/udp.c subsystems/datalink/pprz_transport.c
ap.CFLAGS += -DDOWNLINK_TRANSPORT=PprzTransport -DDATALINK=UDP -DDefaultPeriodic='&telemetry_Main'
ap.srcs += subsystems/datalink/downlink.c subsystems/datalink/udp.c subsystems/datalink/pprz_transport.c subsystems/datalink/telemetry.c
ap.srcs += $(SRC_FIRMWARE)/datalink.c $(SRC_FIRMWARE)/telemetry.c
ap.srcs += fms/fms_network.c
conf/firmwares/subsystems/rotorcraft/telemetry_xbee_api.makefile
+2 -2
View File
@@ -10,6 +10,6 @@ ap.CFLAGS += -DUSE_$(MODEM_PORT)
ap.CFLAGS += -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD) -DXBEE_BAUD=$(MODEM_BAUD)
ap.CFLAGS += -DDOWNLINK -DDOWNLINK_DEVICE=$(MODEM_PORT) -DXBEE_UART=$(MODEM_PORT)
ap.CFLAGS += -DDOWNLINK_TRANSPORT=XBeeTransport -DDATALINK=XBEE
ap.srcs += subsystems/datalink/downlink.c subsystems/datalink/xbee.c
ap.CFLAGS += -DDOWNLINK_TRANSPORT=XBeeTransport -DDATALINK=XBEE -DDefaultPeriodic='&telemetry_Main'
ap.srcs += subsystems/datalink/downlink.c subsystems/datalink/xbee.c subsystems/datalink/telemetry.c
ap.srcs += $(SRC_FIRMWARE)/datalink.c $(SRC_FIRMWARE)/telemetry.c
sw/airborne/boards/ardrone/navdata.c
+36
View File
@@ -36,8 +36,43 @@
#include <math.h>
#include "navdata.h"
#include "subsystems/datalink/downlink.h"
#include "generated/periodic_telemetry.h"
int nav_fd;
static void send_navdata(void) {
DOWNLINK_SEND_ARDRONE_NAVDATA(DefaultChannel, DefaultDevice,
&navdata->taille,
&navdata->nu_trame,
&navdata->ax,
&navdata->ay,
&navdata->az,
&navdata->vx,
&navdata->vy,
&navdata->vz,
&navdata->temperature_acc,
&navdata->temperature_gyro,
&navdata->ultrasound,
&navdata->us_debut_echo,
&navdata->us_fin_echo,
&navdata->us_association_echo,
&navdata->us_distance_echo,
&navdata->us_curve_time,
&navdata->us_curve_value,
&navdata->us_curve_ref,
&navdata->nb_echo,
&navdata->sum_echo,
&navdata->gradient,
&navdata->flag_echo_ini,
&navdata->pressure,
&navdata->temperature_pressure,
&navdata->mx,
&navdata->my,
&navdata->mz,
&navdata->chksum);
}
int navdata_init()
{
port = malloc(sizeof(navdata_port));
@@ -87,6 +122,7 @@ int navdata_init()
previousUltrasoundHeight = 0;
register_periodic_telemetry(DefaultPeriodic, "ARDRONE_NAVDATA", send_navdata);
return 0;
}
sw/airborne/firmwares/rotorcraft/autopilot.c
+108
View File
@@ -28,12 +28,17 @@
#include "firmwares/rotorcraft/autopilot.h"
#include "mcu_periph/uart.h"
#include "subsystems/radio_control.h"
#include "subsystems/gps.h"
#include "subsystems/commands.h"
#include "subsystems/actuators.h"
#include "subsystems/electrical.h"
#include "subsystems/settings.h"
#include "firmwares/rotorcraft/navigation.h"
#include "firmwares/rotorcraft/guidance.h"
#include "firmwares/rotorcraft/stabilization.h"
#include "firmwares/rotorcraft/telemetry.h"
#include "led.h"
uint8_t autopilot_mode;
@@ -73,6 +78,95 @@ static inline int ahrs_is_aligned(void) {
#include "autopilot_arming_yaw.h"
#endif
static void send_alive(void) {
DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM);
}
static void send_status(void) {
uint32_t imu_nb_err = 0;
uint8_t _twi_blmc_nb_err = 0;
#if USE_GPS
uint8_t fix = gps.fix;
#else
uint8_t fix = GPS_FIX_NONE;
#endif
uint16_t time_sec = sys_time.nb_sec;
DOWNLINK_SEND_ROTORCRAFT_STATUS(DefaultChannel, DefaultDevice,
&imu_nb_err, &_twi_blmc_nb_err,
&radio_control.status, &radio_control.frame_rate,
&fix, &autopilot_mode,
&autopilot_in_flight, &autopilot_motors_on,
&guidance_h_mode, &guidance_v_mode,
&electrical.vsupply, &time_sec);
}
static void send_fp(void) {
int32_t carrot_up = -guidance_v_z_sp;
DOWNLINK_SEND_ROTORCRAFT_FP(DefaultChannel, DefaultDevice,
&(stateGetPositionEnu_i()->x),
&(stateGetPositionEnu_i()->y),
&(stateGetPositionEnu_i()->z),
&(stateGetSpeedEnu_i()->x),
&(stateGetSpeedEnu_i()->y),
&(stateGetSpeedEnu_i()->z),
&(stateGetNedToBodyEulers_i()->phi),
&(stateGetNedToBodyEulers_i()->theta),
&(stateGetNedToBodyEulers_i()->psi),
&guidance_h_pos_sp.y,
&guidance_h_pos_sp.x,
&carrot_up,
&guidance_h_command_body.psi,
&stabilization_cmd[COMMAND_THRUST],
&autopilot_flight_time);
}
#ifdef RADIO_CONTROL
static void send_rc(void) {
DOWNLINK_SEND_RC(DefaultChannel, DefaultDevice, RADIO_CONTROL_NB_CHANNEL, radio_control.values);
}
static void send_rotorcraft_rc(void) {
#ifdef RADIO_KILL_SWITCH
int16_t _kill_switch = radio_control.values[RADIO_KILL_SWITCH];
#else
int16_t _kill_switch = 42;
#endif
DOWNLINK_SEND_ROTORCRAFT_RADIO_CONTROL(DefaultChannel, DefaultDevice,
&radio_control.values[RADIO_ROLL],
&radio_control.values[RADIO_PITCH],
&radio_control.values[RADIO_YAW],
&radio_control.values[RADIO_THROTTLE],
&radio_control.values[RADIO_MODE],
&_kill_switch,
&radio_control.status);
}
#endif
#ifdef ACTUATORS
static void send_actuators(void) {
DOWNLINK_SEND_ACTUATORS(DefaultChannel, DefaultDevice , ACTUATORS_NB, actuators);
}
#endif
static void send_dl_value(void) {
PeriodicSendDlValue(DefaultChannel, DefaultDevice);
}
static void send_rotorcraft_cmd(void) {
DOWNLINK_SEND_ROTORCRAFT_CMD(DefaultChannel, DefaultDevice,
&stabilization_cmd[COMMAND_ROLL],
&stabilization_cmd[COMMAND_PITCH],
&stabilization_cmd[COMMAND_YAW],
&stabilization_cmd[COMMAND_THRUST]);
}
// FIXME we really need a baro subsystem
#include "subsystems/sensors/baro.h"
static void send_baro_raw(void) {
DOWNLINK_SEND_BARO_RAW(DefaultChannel, DefaultDevice,
&baro.absolute, &baro.differential);
}
void autopilot_init(void) {
autopilot_mode = AP_MODE_KILL;
autopilot_motors_on = FALSE;
@@ -89,6 +183,20 @@ void autopilot_init(void) {
LED_ON(POWER_SWITCH_LED); // POWER OFF
#endif
autopilot_arming_init();
register_periodic_telemetry(DefaultPeriodic, "ALIVE", send_alive);
register_periodic_telemetry(DefaultPeriodic, "ROTORCRAFT_STATUS", send_status);
register_periodic_telemetry(DefaultPeriodic, "ROTORCRAFT_FP", send_fp);
register_periodic_telemetry(DefaultPeriodic, "ROTORCRAFT_CMD", send_rotorcraft_cmd);
register_periodic_telemetry(DefaultPeriodic, "DL_VALUE", send_dl_value);
register_periodic_telemetry(DefaultPeriodic, "BARO_RAW", send_baro_raw);
#ifdef ACTUATORS
register_periodic_telemetry(DefaultPeriodic, "ACTUATORS", send_actuators);
#endif
#ifdef RADIO_CONTROL
register_periodic_telemetry(DefaultPeriodic, "RC", send_rc);
register_periodic_telemetry(DefaultPeriodic, "ROTORCRAFT_RADIO_CONTROL", send_rotorcraft_rc);
#endif
}
sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
+42
View File
@@ -34,6 +34,10 @@
#include "generated/airframe.h"
#include "mcu_periph/uart.h"
#include "subsystems/datalink/downlink.h"
#include "generated/periodic_telemetry.h"
/* error if some gains are negative */
#if (GUIDANCE_H_PGAIN < 0) || \
(GUIDANCE_H_DGAIN < 0) || \
@@ -89,6 +93,40 @@ static void guidance_h_hover_enter(void);
static void guidance_h_nav_enter(void);
static inline void transition_run(void);
static void send_gh(void) {
struct NedCoor_i* pos = stateGetPositionNed_i();
DOWNLINK_SEND_GUIDANCE_H_INT(DefaultChannel, DefaultDevice,
&guidance_h_pos_sp.x, &guidance_h_pos_sp.y,
&guidance_h_pos_ref.x, &guidance_h_pos_ref.y,
&(pos->x), &(pos->y));
}
static void send_hover_loop(void) {
struct NedCoor_i* pos = stateGetPositionNed_i();
struct NedCoor_i* speed = stateGetSpeedNed_i();
struct NedCoor_i* accel = stateGetAccelNed_i();
DOWNLINK_SEND_HOVER_LOOP(DefaultChannel, DefaultDevice,
&guidance_h_pos_sp.x, &guidance_h_pos_sp.y,
&(pos->x), &(pos->y),
&(speed->x), &(speed->y),
&(accel->x), &(accel->y),
&guidance_h_pos_err.x, &guidance_h_pos_err.y,
&guidance_h_speed_err.x, &guidance_h_speed_err.y,
&guidance_h_pos_err_sum.x, &guidance_h_pos_err_sum.y,
&guidance_h_nav_err.x, &guidance_h_nav_err.y,
&guidance_h_command_earth.x, &guidance_h_command_earth.y,
&guidance_h_command_body.phi,
&guidance_h_command_body.theta,
&guidance_h_command_body.psi);
}
static void send_href(void) {
DOWNLINK_SEND_GUIDANCE_H_REF_INT(DefaultChannel, DefaultDevice,
&guidance_h_pos_sp.x, &guidance_h_pos_ref.x,
&guidance_h_speed_ref.x, &guidance_h_accel_ref.x,
&guidance_h_pos_sp.y, &guidance_h_pos_ref.y,
&guidance_h_speed_ref.y, &guidance_h_accel_ref.y);
}
void guidance_h_init(void) {
@@ -105,6 +143,10 @@ void guidance_h_init(void) {
guidance_h_again = GUIDANCE_H_AGAIN;
transition_percentage = 0;
transition_theta_offset = 0;
register_periodic_telemetry(DefaultPeriodic, "GUIDANCE_H_INT", send_gh);
register_periodic_telemetry(DefaultPeriodic, "HOVER_LOOP", send_hover_loop);
register_periodic_telemetry(DefaultPeriodic, "GUIDANCE_H_REF", send_href);
}
sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c
+21
View File
@@ -39,6 +39,9 @@
#include "generated/airframe.h"
#include "subsystems/datalink/downlink.h"
#include "generated/periodic_telemetry.h"
/* warn if some gains are still negative */
#if (GUIDANCE_V_HOVER_KP < 0) || \
@@ -93,6 +96,22 @@ int32_t guidance_v_z_sum_err;
__attribute__ ((always_inline)) static inline void run_hover_loop(bool_t in_flight);
static void send_vert_loop(void) {
DOWNLINK_SEND_VERT_LOOP(DefaultChannel, DefaultDevice,
&guidance_v_z_sp, &guidance_v_zd_sp,
&(stateGetPositionNed_i()->z),
&(stateGetSpeedNed_i()->z),
&(stateGetAccelNed_i()->z),
&guidance_v_z_ref, &guidance_v_zd_ref,
&guidance_v_zdd_ref,
&gv_adapt_X,
&gv_adapt_P,
&gv_adapt_Xmeas,
&guidance_v_z_sum_err,
&guidance_v_ff_cmd,
&guidance_v_fb_cmd,
&guidance_v_delta_t);
}
void guidance_v_init(void) {
@@ -109,6 +128,8 @@ void guidance_v_init(void) {
#endif
gv_adapt_init();
register_periodic_telemetry(DefaultPeriodic, "VERT_LOOP", send_vert_loop);
}
sw/airborne/firmwares/rotorcraft/main.c
+4 -1
View File
@@ -57,10 +57,13 @@
#include "firmwares/rotorcraft/autopilot.h"
#include "subsystems/radio_control.h"
#include "firmwares/rotorcraft/stabilization.h"
#include "firmwares/rotorcraft/guidance.h"
#include "subsystems/ahrs.h"
#include "subsystems/ahrs/ahrs_aligner.h"
#include "subsystems/ins.h"
#include "state.h"
@@ -212,7 +215,7 @@ STATIC_INLINE void main_periodic( void ) {
}
STATIC_INLINE void telemetry_periodic(void) {
PeriodicSendMain(DefaultChannel,DefaultDevice);
periodic_telemetry_send_Main();
}
STATIC_INLINE void failsafe_check( void ) {
sw/airborne/firmwares/rotorcraft/navigation.c
+35
View File
@@ -44,6 +44,9 @@
#include "math/pprz_algebra_int.h"
#include "subsystems/datalink/downlink.h"
#include "generated/periodic_telemetry.h"
const uint8_t nb_waypoint = NB_WAYPOINT;
struct EnuCoor_i waypoints[NB_WAYPOINT];
@@ -83,6 +86,36 @@ static inline void nav_set_altitude( void );
#define ARRIVED_AT_WAYPOINT (3 << 8)
#define CARROT_DIST (12 << 8)
static void send_nav_status(void) {
DOWNLINK_SEND_ROTORCRAFT_NAV_STATUS(DefaultChannel, DefaultDevice,
&block_time, &stage_time,
&nav_block, &nav_stage,
&horizontal_mode);
if (horizontal_mode == HORIZONTAL_MODE_ROUTE) {
float sx = POS_FLOAT_OF_BFP(waypoints[nav_segment_start].x);
float sy = POS_FLOAT_OF_BFP(waypoints[nav_segment_start].y);
float ex = POS_FLOAT_OF_BFP(waypoints[nav_segment_end].x);
float ey = POS_FLOAT_OF_BFP(waypoints[nav_segment_end].y);
DOWNLINK_SEND_SEGMENT(DefaultChannel, DefaultDevice, &sx, &sy, &ex, &ey);
}
else if (horizontal_mode == HORIZONTAL_MODE_CIRCLE) {
float cx = POS_FLOAT_OF_BFP(waypoints[nav_circle_centre].x);
float cy = POS_FLOAT_OF_BFP(waypoints[nav_circle_centre].y);
float r = POS_FLOAT_OF_BFP(nav_circle_radius);
DOWNLINK_SEND_CIRCLE(DefaultChannel, DefaultDevice, &cx, &cy, &r);
}
}
static void send_wp_moved(void) {
static uint8_t i;
i++; if (i >= nb_waypoint) i = 0;
DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice,
&i,
&(waypoints[i].x),
&(waypoints[i].y),
&(waypoints[i].z));
}
void nav_init(void) {
// convert to
const struct EnuCoor_f wp_tmp_float[NB_WAYPOINT] = WAYPOINTS;
@@ -115,6 +148,8 @@ void nav_init(void) {
nav_leg_progress = 0;
nav_leg_length = 1;
register_periodic_telemetry(DefaultPeriodic, "ROTORCRAFT_NAV_STATUS", send_nav_status);
register_periodic_telemetry(DefaultPeriodic, "WP_MOVED", send_wp_moved);
}
static inline void nav_advance_carrot(void) {
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c
+43
View File
@@ -35,6 +35,47 @@ struct FloatEulers stabilization_att_sum_err;
float stabilization_att_fb_cmd[COMMANDS_NB];
float stabilization_att_ff_cmd[COMMANDS_NB];
static void send_att(void) {
struct FloatRates* body_rate = stateGetBodyRates_i();
struct FloatEulers* att = stateGetNedToBodyEulers_i();
float foo;
DOWNLINK_SEND_STAB_ATTITUDE_FLOAT(DefaultChannel, DefaultDevice,
&(body_rate->p), &(body_rate->q), &(body_rate->r),
&(att->phi), &(att->theta), &(att->psi),
&stab_att_sp_euler.phi,
&stab_att_sp_euler.theta,
&stab_att_sp_euler.psi,
&stabilization_att_sum_err_eulers.phi,
&stabilization_att_sum_err_eulers.theta,
&stabilization_att_sum_err_eulers.psi,
&stabilization_att_fb_cmd[COMMAND_ROLL],
&stabilization_att_fb_cmd[COMMAND_PITCH],
&stabilization_att_fb_cmd[COMMAND_YAW],
&stabilization_att_ff_cmd[COMMAND_ROLL],
&stabilization_att_ff_cmd[COMMAND_PITCH],
&stabilization_att_ff_cmd[COMMAND_YAW],
&stabilization_cmd[COMMAND_ROLL],
&stabilization_cmd[COMMAND_PITCH],
&stabilization_cmd[COMMAND_YAW],
&foo, &foo, &foo);
}
static void send_att_ref(void) {
DOWNLINK_SEND_STAB_ATTITUDE_REF_FLOAT(DefaultChannel, DefaultDevice,
&stab_att_sp_euler.phi,
&stab_att_sp_euler.theta,
&stab_att_sp_euler.psi,
&stab_att_ref_euler.phi,
&stab_att_ref_euler.theta,
&stab_att_ref_euler.psi,
&stab_att_ref_rate.p,
&stab_att_ref_rate.q,
&stab_att_ref_rate.r,
&stab_att_ref_accel.p,
&stab_att_ref_accel.q,
&stab_att_ref_accel.r);
}
void stabilization_attitude_init(void) {
@@ -62,6 +103,8 @@ void stabilization_attitude_init(void) {
FLOAT_EULERS_ZERO( stabilization_att_sum_err );
register_periodic_telemetry(DefaultPeriodic, "STAB_ATTITUDE", send_att);
register_periodic_telemetry(DefaultPeriodic, "STAB_ATTITUDE_REF", send_att_ref);
}
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c
+44
View File
@@ -27,6 +27,9 @@
#include "generated/airframe.h"
#include "subsystems/datalink/downlink.h"
#include "generated/periodic_telemetry.h"
struct Int32AttitudeGains stabilization_gains;
/* warn if some gains are still negative */
@@ -54,6 +57,45 @@ static inline void reset_psi_ref_from_body(void) {
stab_att_ref_accel.r = 0;
}
static void send_att(void) {
struct Int32Rates* body_rate = stateGetBodyRates_i();
struct Int32Eulers* att = stateGetNedToBodyEulers_i();
DOWNLINK_SEND_STAB_ATTITUDE_INT(DefaultChannel, DefaultDevice,
&(body_rate->p), &(body_rate->q), &(body_rate->r),
&(att->phi), &(att->theta), &(att->psi),
&stab_att_sp_euler.phi,
&stab_att_sp_euler.theta,
&stab_att_sp_euler.psi,
&stabilization_att_sum_err.phi,
&stabilization_att_sum_err.theta,
&stabilization_att_sum_err.psi,
&stabilization_att_fb_cmd[COMMAND_ROLL],
&stabilization_att_fb_cmd[COMMAND_PITCH],
&stabilization_att_fb_cmd[COMMAND_YAW],
&stabilization_att_ff_cmd[COMMAND_ROLL],
&stabilization_att_ff_cmd[COMMAND_PITCH],
&stabilization_att_ff_cmd[COMMAND_YAW],
&stabilization_cmd[COMMAND_ROLL],
&stabilization_cmd[COMMAND_PITCH],
&stabilization_cmd[COMMAND_YAW]);
}
static void send_att_ref(void) {
DOWNLINK_SEND_STAB_ATTITUDE_REF_INT(DefaultChannel, DefaultDevice,
&stab_att_sp_euler.phi,
&stab_att_sp_euler.theta,
&stab_att_sp_euler.psi,
&stab_att_ref_euler.phi,
&stab_att_ref_euler.theta,
&stab_att_ref_euler.psi,
&stab_att_ref_rate.p,
&stab_att_ref_rate.q,
&stab_att_ref_rate.r,
&stab_att_ref_accel.p,
&stab_att_ref_accel.q,
&stab_att_ref_accel.r);
}
void stabilization_attitude_init(void) {
stabilization_attitude_ref_init();
@@ -82,6 +124,8 @@ void stabilization_attitude_init(void) {
INT_EULERS_ZERO( stabilization_att_sum_err );
register_periodic_telemetry(DefaultPeriodic, "STAB_ATTITUDE", send_att);
register_periodic_telemetry(DefaultPeriodic, "STAB_ATTITUDE_REF", send_att_ref);
}
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c
+42
View File
@@ -32,6 +32,9 @@
#include "state.h"
#include "generated/airframe.h"
#include "subsystems/datalink/downlink.h"
#include "generated/periodic_telemetry.h"
struct Int32AttitudeGains stabilization_gains = {
{STABILIZATION_ATTITUDE_PHI_PGAIN, STABILIZATION_ATTITUDE_THETA_PGAIN, STABILIZATION_ATTITUDE_PSI_PGAIN },
{STABILIZATION_ATTITUDE_PHI_DGAIN, STABILIZATION_ATTITUDE_THETA_DGAIN, STABILIZATION_ATTITUDE_PSI_DGAIN },
@@ -64,6 +67,42 @@ int32_t stabilization_att_ff_cmd[COMMANDS_NB];
#define GAIN_PRESCALER_D 48
#define GAIN_PRESCALER_I 48
static void send_att(void) { //FIXME really use this message here ?
struct Int32Rates* body_rate = stateGetBodyRates_i();
struct Int32Eulers* att = stateGetNedToBodyEulers_i();
DOWNLINK_SEND_STAB_ATTITUDE_INT(DefaultChannel, DefaultDevice,
&(body_rate->p), &(body_rate->q), &(body_rate->r),
&(att->phi), &(att->theta), &(att->psi),
&stab_att_sp_euler.phi,
&stab_att_sp_euler.theta,
&stab_att_sp_euler.psi,
&stabilization_att_sum_err.phi,
&stabilization_att_sum_err.theta,
&stabilization_att_sum_err.psi,
&stabilization_att_fb_cmd[COMMAND_ROLL],
&stabilization_att_fb_cmd[COMMAND_PITCH],
&stabilization_att_fb_cmd[COMMAND_YAW],
&stabilization_att_ff_cmd[COMMAND_ROLL],
&stabilization_att_ff_cmd[COMMAND_PITCH],
&stabilization_att_ff_cmd[COMMAND_YAW],
&stabilization_cmd[COMMAND_ROLL],
&stabilization_cmd[COMMAND_PITCH],
&stabilization_cmd[COMMAND_YAW]);
}
static void send_att_ref(void) {
struct Int32Quat* quat = stateGetNedToBodyQuat_i();
DOWNLINK_SEND_AHRS_REF_QUAT(DefaultChannel, DefaultDevice,
&stab_att_ref_quat.qi,
&stab_att_ref_quat.qx,
&stab_att_ref_quat.qy,
&stab_att_ref_quat.qz,
&(quat->qi),
&(quat->qx),
&(quat->qy),
&(quat->qz));
}
void stabilization_attitude_init(void) {
stabilization_attitude_ref_init();
@@ -81,6 +120,9 @@ void stabilization_attitude_enter(void) {
INT32_QUAT_ZERO(stabilization_att_sum_err_quat);
INT_EULERS_ZERO(stabilization_att_sum_err);
register_periodic_telemetry(DefaultPeriodic, "STAB_ATTITUDE", send_att);
register_periodic_telemetry(DefaultPeriodic, "STAB_AHRS_REF_QUAT", send_att_ref);
}
void stabilization_attitude_set_failsafe_setpoint(void) {
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c
+27
View File
@@ -34,6 +34,9 @@
#include "subsystems/radio_control.h"
#include "generated/airframe.h"
#include "subsystems/datalink/downlink.h"
#include "generated/periodic_telemetry.h"
#define F_UPDATE_RES 9
#define REF_DOT_FRAC 11
#define REF_FRAC 16
@@ -119,6 +122,28 @@ struct Int32Rates stabilization_rate_ff_cmd;
(radio_control.values[RADIO_YAW] > STABILIZATION_RATE_DEADBAND_R || \
radio_control.values[RADIO_YAW] < -STABILIZATION_RATE_DEADBAND_R)
static void send_rate(void) {
DOWNLINK_SEND_RATE_LOOP(DefaultChannel, DefaultDevice,
&stabilization_rate_sp.p,
&stabilization_rate_sp.q,
&stabilization_rate_sp.r,
&stabilization_rate_ref.p,
&stabilization_rate_ref.q,
&stabilization_rate_ref.r,
&stabilization_rate_refdot.p,
&stabilization_rate_refdot.q,
&stabilization_rate_refdot.r,
&stabilization_rate_sum_err.p,
&stabilization_rate_sum_err.q,
&stabilization_rate_sum_err.r,
&stabilization_rate_ff_cmd.p,
&stabilization_rate_ff_cmd.q,
&stabilization_rate_ff_cmd.r,
&stabilization_rate_fb_cmd.p,
&stabilization_rate_fb_cmd.q,
&stabilization_rate_fb_cmd.r,
&stabilization_cmd[COMMAND_THRUST]);
}
void stabilization_rate_init(void) {
@@ -140,6 +165,8 @@ void stabilization_rate_init(void) {
INT_RATES_ZERO(stabilization_rate_ref);
INT_RATES_ZERO(stabilization_rate_refdot);
INT_RATES_ZERO(stabilization_rate_sum_err);
register_periodic_telemetry(DefaultPeriodic, "RATE_LOOP", send_rate);
}
sw/airborne/firmwares/rotorcraft/telemetry.h
+19 -1045
View File
File diff suppressed because it is too large Load Diff
sw/airborne/mcu_periph/i2c.c
+104
View File
@@ -27,10 +27,37 @@
#include "mcu_periph/i2c.h"
#include "subsystems/datalink/downlink.h"
#include "generated/periodic_telemetry.h"
#ifdef USE_I2C0
struct i2c_periph i2c0;
static void send_i2c0_err(void) {
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_arb_lost_cnt = i2c0.errors->arb_lost_cnt;
uint16_t i2c0_over_under_cnt = i2c0.errors->over_under_cnt;
uint16_t i2c0_pec_recep_cnt = i2c0.errors->pec_recep_cnt;
uint16_t i2c0_timeout_tlow_cnt = i2c0.errors->timeout_tlow_cnt;
uint16_t i2c0_smbus_alert_cnt = i2c0.errors->smbus_alert_cnt;
uint16_t i2c0_unexpected_event_cnt = i2c0.errors->unexpected_event_cnt;
uint32_t i2c0_last_unexpected_event = i2c0.errors->last_unexpected_event;
const uint8_t _bus0 = 0;
DOWNLINK_SEND_I2C_ERRORS(DefaultChannel, DefaultDevice,
&i2c0_ack_fail_cnt,
&i2c0_miss_start_stop_cnt,
&i2c0_arb_lost_cnt,
&i2c0_over_under_cnt,
&i2c0_pec_recep_cnt,
&i2c0_timeout_tlow_cnt,
&i2c0_smbus_alert_cnt,
&i2c0_unexpected_event_cnt,
&i2c0_last_unexpected_event,
&_bus0);
}
void i2c0_init(void) {
i2c_init(&i2c0);
i2c0_hw_init();
@@ -43,6 +70,30 @@ void i2c0_init(void) {
struct i2c_periph i2c1;
static void send_i2c1_err(void) {
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_arb_lost_cnt = i2c1.errors->arb_lost_cnt;
uint16_t i2c1_over_under_cnt = i2c1.errors->over_under_cnt;
uint16_t i2c1_pec_recep_cnt = i2c1.errors->pec_recep_cnt;
uint16_t i2c1_timeout_tlow_cnt = i2c1.errors->timeout_tlow_cnt;
uint16_t i2c1_smbus_alert_cnt = i2c1.errors->smbus_alert_cnt;
uint16_t i2c1_unexpected_event_cnt = i2c1.errors->unexpected_event_cnt;
uint32_t i2c1_last_unexpected_event = i2c1.errors->last_unexpected_event;
const uint8_t _bus1 = 1;
DOWNLINK_SEND_I2C_ERRORS(DefaultChannel, DefaultDevice,
&i2c1_ack_fail_cnt,
&i2c1_miss_start_stop_cnt,
&i2c1_arb_lost_cnt,
&i2c1_over_under_cnt,
&i2c1_pec_recep_cnt,
&i2c1_timeout_tlow_cnt,
&i2c1_smbus_alert_cnt,
&i2c1_unexpected_event_cnt,
&i2c1_last_unexpected_event,
&_bus1);
}
void i2c1_init(void) {
i2c_init(&i2c1);
i2c1_hw_init();
@@ -55,6 +106,30 @@ void i2c1_init(void) {
struct i2c_periph i2c2;
static void send_i2c2_err(void) {
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_arb_lost_cnt = i2c2.errors->arb_lost_cnt;
uint16_t i2c2_over_under_cnt = i2c2.errors->over_under_cnt;
uint16_t i2c2_pec_recep_cnt = i2c2.errors->pec_recep_cnt;
uint16_t i2c2_timeout_tlow_cnt = i2c2.errors->timeout_tlow_cnt;
uint16_t i2c2_smbus_alert_cnt = i2c2.errors->smbus_alert_cnt;
uint16_t i2c2_unexpected_event_cnt = i2c2.errors->unexpected_event_cnt;
uint32_t i2c2_last_unexpected_event = i2c2.errors->last_unexpected_event;
const uint8_t _bus2 = 2;
DOWNLINK_SEND_I2C_ERRORS(DefaultChannel, DefaultDevice,
&i2c2_ack_fail_cnt,
&i2c2_miss_start_stop_cnt,
&i2c2_arb_lost_cnt,
&i2c2_over_under_cnt,
&i2c2_pec_recep_cnt,
&i2c2_timeout_tlow_cnt,
&i2c2_smbus_alert_cnt,
&i2c2_unexpected_event_cnt,
&i2c2_last_unexpected_event,
&_bus2);
}
void i2c2_init(void) {
i2c_init(&i2c2);
i2c2_hw_init();
@@ -62,10 +137,39 @@ void i2c2_init(void) {
#endif /* USE_I2C2 */
static void send_i2c_err(void) {
static uint8_t _i2c_nb_cnt = 0;
switch (_i2c_nb_cnt) {
case 0:
#if USE_I2C0
send_i2c0_err();
#endif
break;
case 1:
#if USE_I2C1
send_i2c1_err();
#endif
break;
case 2:
#if USE_I2C2
send_i2c2_err();
#endif
break;
default:
break;
}
_i2c_nb_cnt++;
if (_i2c_nb_cnt == 3)
_i2c_nb_cnt = 0;
}
void i2c_init(struct i2c_periph* p) {
p->trans_insert_idx = 0;
p->trans_extract_idx = 0;
p->status = I2CIdle;
// the first to register do it for the others
register_periodic_telemetry(DefaultPeriodic, "I2C_ERRORS", send_i2c_err);
}
sw/airborne/mcu_periph/uart.c
+114
View File
@@ -22,34 +22,145 @@
#include "mcu_periph/uart.h"
#include "subsystems/datalink/downlink.h"
#include "generated/periodic_telemetry.h"
#ifdef USE_UART0
struct uart_periph uart0;
static void send_uart0_err(void) {
uint16_t ore = uart0.ore;
uint16_t ne_err = uart0.ne_err;
uint16_t fe_err = uart0.fe_err;
const uint8_t _bus0 = 0;
DOWNLINK_SEND_UART_ERRORS(DefaultChannel, DefaultDevice,
&ore, &ne_err, &fe_err, &_bus0);
}
#endif
#ifdef USE_UART1
struct uart_periph uart1;
static void send_uart1_err(void) {
uint16_t ore = uart1.ore;
uint16_t ne_err = uart1.ne_err;
uint16_t fe_err = uart1.fe_err;
const uint8_t _bus1 = 1;
DOWNLINK_SEND_UART_ERRORS(DefaultChannel, DefaultDevice,
&ore, &ne_err, &fe_err, &_bus1);
}
#endif
#ifdef USE_UART2
struct uart_periph uart2;
static void send_uart2_err(void) {
uint16_t ore = uart2.ore;
uint16_t ne_err = uart2.ne_err;
uint16_t fe_err = uart2.fe_err;
const uint8_t _bus2 = 2;
DOWNLINK_SEND_UART_ERRORS(DefaultChannel, DefaultDevice,
&ore, &ne_err, &fe_err, &_bus2);
}
#endif
#ifdef USE_UART3
struct uart_periph uart3;
static void send_uart3_err(void) {
uint16_t ore = uart3.ore;
uint16_t ne_err = uart3.ne_err;
uint16_t fe_err = uart3.fe_err;
const uint8_t _bus3 = 3;
DOWNLINK_SEND_UART_ERRORS(DefaultChannel, DefaultDevice,
&ore, &ne_err, &fe_err, &_bus3);
}
#endif
#ifdef USE_UART4
struct uart_periph uart4;
static void send_uart4_err(void) {
uint16_t ore = uart4.ore;
uint16_t ne_err = uart4.ne_err;
uint16_t fe_err = uart4.fe_err;
const uint8_t _bus4 = 4;
DOWNLINK_SEND_UART_ERRORS(DefaultChannel, DefaultDevice,
&ore, &ne_err, &fe_err, &_bus4);
}
#endif
#ifdef USE_UART5
struct uart_periph uart5;
static void send_uart5_err(void) {
uint16_t ore = uart5.ore;
uint16_t ne_err = uart5.ne_err;
uint16_t fe_err = uart5.fe_err;
const uint8_t _bus5 = 5;
DOWNLINK_SEND_UART_ERRORS(DefaultChannel, DefaultDevice,
&ore, &ne_err, &fe_err, &_bus5);
}
#endif
#ifdef USE_UART6
struct uart_periph uart6;
static void send_uart6_err(void) {
const uint8_t _bus6 = 6;
uint16_t ore = uart6.ore;
uint16_t ne_err = uart6.ne_err;
uint16_t fe_err = uart6.fe_err;
DOWNLINK_SEND_UART_ERRORS(DefaultChannel, DefaultDevice,
&ore, &ne_err, &fe_err, &_bus6);
}
#endif
static void send_uart_err(void) {
static uint8_t uart_nb_cnt = 0;
switch (uart_nb_cnt) {
#if USE_UART0
case 0:
send_uart0_err(); break;
#endif
#if USE_UART1
case 1:
send_uart1_err(); break;
#endif
#if USE_UART2
case 2:
send_uart2_err(); break;
#endif
#if USE_UART3
case 3:
send_uart3_err(); break;
#endif
#if USE_UART4
case 4:
send_uart4_err(); break;
#endif
#if USE_UART5
case 5:
send_uart5_err(); break;
#endif
#if USE_UART6
case 6:
send_uart6_err(); break;
#endif
default: break;
}
uart_nb_cnt++;
if (uart_nb_cnt == 6)
uart_nb_cnt = 0;
}
void uart_periph_init(struct uart_periph* p) {
p->rx_insert_idx = 0;
p->rx_extract_idx = 0;
@@ -59,6 +170,9 @@ void uart_periph_init(struct uart_periph* p) {
p->ore = 0;
p->ne_err = 0;
p->fe_err = 0;
// the first to register do it for the others
register_periodic_telemetry(DefaultPeriodic, "UART_ERRORS", send_uart_err);
}
bool_t uart_check_free_space(struct uart_periph* p, uint8_t len) {
sw/airborne/modules/cam_control/cam_track.c
+11
View File
@@ -29,6 +29,9 @@
#include "subsystems/ins/hf_float.h"
#endif
#include "subsystems/datalink/downlink.h"
#include "generated/periodic_telemetry.h"
struct FloatVect3 target_pos_ned;
struct FloatVect3 target_speed_ned;
struct FloatVect3 target_accel_ned;
@@ -54,6 +57,13 @@ volatile uint8_t cam_msg_received;
uint8_t cam_status;
uint8_t cam_data_len;
static void send_cam_track(void) {
DOWNLINK_SEND_NPS_SPEED_POS(DefaultChannel, DefaultDevice,
&target_accel_ned.x, &target_accel_ned.y, &target_accel_ned.z,
&target_speed_ned.x, &target_speed_ned.y, &target_speed_ned.z,
&target_pos_ned.x, &target_pos_ned.y, &target_pos_ned.z);
}
void track_init(void) {
ins_ltp_initialised = TRUE; // ltp is initialized and centered on the target
ins_update_on_agl = TRUE; // use sonar to update agl (assume flat ground)
@@ -61,6 +71,7 @@ void track_init(void) {
cam_status = UNINIT;
cam_data_len = CAM_DATA_LEN;
register_periodic_telemetry(DefaultPeriodic, "CAM_TRACK", send_cam_track);
}
#include <stdio.h>
sw/airborne/modules/cam_control/rotorcraft_cam.c
+10
View File
@@ -27,6 +27,9 @@
#include "firmwares/rotorcraft/navigation.h"
#include "std.h"
#include "subsystems/datalink/downlink.h"
#include "generated/periodic_telemetry.h"
uint8_t rotorcraft_cam_mode;
#define _SERVO_PARAM(_s,_p) SERVO_ ## _s ## _ ## _p
@@ -49,6 +52,11 @@ int16_t rotorcraft_cam_pan;
#define ROTORCRAFT_CAM_PAN_MIN 0
#define ROTORCRAFT_CAM_PAN_MAX INT32_ANGLE_2_PI
static void send_cam(void) {
DOWNLINK_SEND_ROTORCRAFT_CAM(DefaultChannel, DefaultDevice,
&rotorcraft_cam_tilt,&rotorcraft_cam_pan);
}
void rotorcraft_cam_init(void) {
rotorcraft_cam_SetCamMode(ROTORCRAFT_CAM_DEFAULT_MODE);
#if ROTORCRAFT_CAM_USE_TILT
@@ -59,6 +67,8 @@ void rotorcraft_cam_init(void) {
#endif
rotorcraft_cam_tilt = 0;
rotorcraft_cam_pan = 0;
register_periodic_telemetry(DefaultPeriodic, "ROTORCRAFT_CAM", send_cam);
}
void rotorcraft_cam_periodic(void) {
sw/airborne/subsystems/ahrs/ahrs_aligner.c
+18
View File
@@ -32,6 +32,9 @@
#include "subsystems/imu.h"
#include "led.h"
#include "subsystems/datalink/downlink.h"
#include "generated/periodic_telemetry.h"
struct AhrsAligner ahrs_aligner;
#define SAMPLES_NB PERIODIC_FREQUENCY
@@ -42,6 +45,19 @@ static struct Int32Vect3 mag_sum;
static int32_t ref_sensor_samples[SAMPLES_NB];
static uint32_t samples_idx;
static void send_aligner(void) {
DOWNLINK_SEND_FILTER_ALIGNER(DefaultChannel, DefaultDevice,
&ahrs_aligner.lp_gyro.p,
&ahrs_aligner.lp_gyro.q,
&ahrs_aligner.lp_gyro.r,
&imu.gyro.p,
&imu.gyro.q,
&imu.gyro.r,
&ahrs_aligner.noise,
&ahrs_aligner.low_noise_cnt,
&ahrs_aligner.status);
}
void ahrs_aligner_init(void) {
ahrs_aligner.status = AHRS_ALIGNER_RUNNING;
@@ -51,6 +67,8 @@ void ahrs_aligner_init(void) {
samples_idx = 0;
ahrs_aligner.noise = 0;
ahrs_aligner.low_noise_cnt = 0;
register_periodic_telemetry(DefaultPeriodic, "FILTER_ALIGNER", send_aligner);
}
#ifndef LOW_NOISE_THRESHOLD
sw/airborne/subsystems/ahrs/ahrs_ardrone2.c
+22
View File
@@ -33,10 +33,30 @@
#include "boards/ardrone/at_com.h"
#include "subsystems/electrical.h"
#include "subsystems/datalink/downlink.h"
#include "generated/periodic_telemetry.h"
struct AhrsARDrone ahrs_impl;
struct AhrsAligner ahrs_aligner;
unsigned char buffer[2048]; //Packet buffer
static void send_ahrs_ad2(void) {
DOWNLINK_SEND_AHRS_ARDRONE2(DefaultChannel, DefaultDevice,
&ahrs_impl.state,
&ahrs_impl.control_state,
&ahrs_impl.eulers.phi,
&ahrs_impl.eulers.theta,
&ahrs_impl.eulers.psi,
&ahrs_impl.speed.x,
&ahrs_impl.speed.y,
&ahrs_impl.speed.z,
&ahrs_impl.accel.x,
&ahrs_impl.accel.y,
&ahrs_impl.accel.z,
&ahrs_impl.altitude,
&ahrs_impl.battery);
}
void ahrs_init(void) {
init_at_com();
@@ -45,6 +65,8 @@ void ahrs_init(void) {
at_com_send_ftrim();
ahrs.status = AHRS_RUNNING;
register_periodic_telemetry(DefaultPeriodic, "AHRS_ARDRONE2", send_ahrs_ad2);
}
void ahrs_align(void) {
sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c
+45
View File
@@ -40,6 +40,9 @@
#include "subsystems/gps.h"
#endif
#include "subsystems/datalink/downlink.h"
#include "generated/periodic_telemetry.h"
//#include "../../test/pprz_algebra_print.h"
@@ -84,6 +87,47 @@ static inline void compute_body_orientation_and_rates(void);
struct AhrsFloatCmpl ahrs_impl;
static void send_att(void) {
struct FloatEulers ltp_to_imu_euler;
FLOAT_EULERS_OF_QUAT(ltp_to_imu_euler, ahrs_impl.ltp_to_imu_quat);
struct Int32Eulers euler_i;
EULERS_BFP_OF_REAL(euler_i, ltp_to_imu_euler);
struct Int32Eulers* eulers_body = stateGetNedToBodyEulers_i();
DOWNLINK_SEND_AHRS_EULER_INT(DefaultChannel, DefaultDevice,
&euler_i.phi,
&euler_i.theta,
&euler_i.psi,
&(eulers_body->phi),
&(eulers_body->theta),
&(eulers_body->psi));
}
// TODO convert from float to int if we really need this one
/*
static void send_rmat(void) {
struct Int32RMat* att_rmat = stateGetNedToBodyRMat_i();
DOWNLINK_SEND_AHRS_RMAT(DefaultChannel, DefaultDevice,
&ahrs_impl.ltp_to_imu_rmat.m[0],
&ahrs_impl.ltp_to_imu_rmat.m[1],
&ahrs_impl.ltp_to_imu_rmat.m[2],
&ahrs_impl.ltp_to_imu_rmat.m[3],
&ahrs_impl.ltp_to_imu_rmat.m[4],
&ahrs_impl.ltp_to_imu_rmat.m[5],
&ahrs_impl.ltp_to_imu_rmat.m[6],
&ahrs_impl.ltp_to_imu_rmat.m[7],
&ahrs_impl.ltp_to_imu_rmat.m[8],
&(att_rmat->m[0]),
&(att_rmat->m[1]),
&(att_rmat->m[2]),
&(att_rmat->m[3]),
&(att_rmat->m[4]),
&(att_rmat->m[5]),
&(att_rmat->m[6]),
&(att_rmat->m[7]),
&(att_rmat->m[8]));
}
*/
void ahrs_init(void) {
ahrs.status = AHRS_UNINIT;
ahrs_impl.ltp_vel_norm_valid = FALSE;
@@ -107,6 +151,7 @@ void ahrs_init(void) {
ahrs_impl.correct_gravity = FALSE;
#endif
register_periodic_telemetry(DefaultPeriodic, "AHRS_EULER_INT", send_att);
}
void ahrs_align(void) {
sw/airborne/subsystems/ahrs/ahrs_float_lkf.c
+68
View File
@@ -35,6 +35,9 @@
#include "generated/airframe.h"
#include "math/pprz_algebra_float.h"
#include "subsystems/datalink/downlink.h"
#include "generated/periodic_telemetry.h"
#include <stdio.h>
@@ -214,6 +217,67 @@ float bafl_R_mag;
INT32_RMAT_TRANSP_RATEMULT(ahrs.body_rate, imu.body_to_imu_rmat, ahrs.imu_rate); \
}
static void send_lkf(void) {
DOWNLINK_SEND_AHRS_LKF(&bafl_eulers.phi,
DefaultChannel, DefaultDevice,
&bafl_eulers.theta,
&bafl_eulers.psi,
&bafl_quat.qi,
&bafl_quat.qx,
&bafl_quat.qy,
&bafl_quat.qz,
&bafl_rates.p,
&bafl_rates.q,
&bafl_rates.r,
&bafl_accel_measure.x,
&bafl_accel_measure.y,
&bafl_accel_measure.z,
&bafl_mag.x,
&bafl_mag.y,
&bafl_mag.z);
}
static void send_lkf_debug(void) {
DOWNLINK_SEND_AHRS_LKF_DEBUG(DefaultChannel, DefaultDevice,
&bafl_X[0],
&bafl_X[1],
&bafl_X[2],
&bafl_bias.p,
&bafl_bias.q,
&bafl_bias.r,
&bafl_qnorm,
&bafl_phi_accel,
&bafl_theta_accel,
&bafl_P[0][0],
&bafl_P[1][1],
&bafl_P[2][2],
&bafl_P[3][3],
&bafl_P[4][4],
&bafl_P[5][5]);
}
static void send_lkf_acc(void) {
DOWNLINK_SEND_AHRS_LKF_ACC_DBG(DefaultChannel, DefaultDevice,
&bafl_q_a_err.qi,
&bafl_q_a_err.qx,
&bafl_q_a_err.qy,
&bafl_q_a_err.qz,
&bafl_b_a_err.p,
&bafl_b_a_err.q,
&bafl_b_a_err.r);
}
static void send_lkf_mag(void) {
DOWNLINK_SEND_AHRS_LKF_MAG_DBG(DefaultChannel, DefaultDevice,
&bafl_q_m_err.qi,
&bafl_q_m_err.qx,
&bafl_q_m_err.qy,
&bafl_q_m_err.qz,
&bafl_b_m_err.p,
&bafl_b_m_err.q,
&bafl_b_m_err.r);
}
void ahrs_init(void) {
int i, j;
@@ -251,6 +315,10 @@ void ahrs_init(void) {
FLOAT_VECT3_ASSIGN(bafl_h, BAFL_hx,BAFL_hy, BAFL_hz);
register_periodic_telemetry(DefaultPeriodic, "AHRS_LKF", send_lkf);
register_periodic_telemetry(DefaultPeriodic, "AHRS_LKF_DEBUG", send_lkf_debug);
register_periodic_telemetry(DefaultPeriodic, "AHRS_LKF_ACC_DBG", send_lkf_acc);
register_periodic_telemetry(DefaultPeriodic, "AHRS_LKF_MAG_DBG", send_lkf_mag);
}
void ahrs_align(void) {
sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c
+42
View File
@@ -38,6 +38,9 @@
#include "generated/airframe.h"
#include "subsystems/datalink/downlink.h"
#include "generated/periodic_telemetry.h"
#ifndef FACE_REINJ_1
#define FACE_REINJ_1 1024
#endif
@@ -74,6 +77,41 @@ static inline void set_body_state_from_euler(void);
while (_a < -PI_INTEG_EULER) _a += TWO_PI_INTEG_EULER; \
}
static void send_filter(void) {
DOWNLINK_SEND_FILTER(DefaultChannel, DefaultDevice,
&ahrs_impl.ltp_to_imu_euler.phi,
&ahrs_impl.ltp_to_imu_euler.theta,
&ahrs_impl.ltp_to_imu_euler.psi,
&ahrs_impl.measure.phi,
&ahrs_impl.measure.theta,
&ahrs_impl.measure.psi,
&ahrs_impl.hi_res_euler.phi,
&ahrs_impl.hi_res_euler.theta,
&ahrs_impl.hi_res_euler.psi,
&ahrs_impl.residual.phi,
&ahrs_impl.residual.theta,
&ahrs_impl.residual.psi,
&ahrs_impl.gyro_bias.p,
&ahrs_impl.gyro_bias.q,
&ahrs_impl.gyro_bias.r);
}
static void send_euler(void) {
struct Int32Eulers* eulers = stateGetNedToBodyEulers_i();
DOWNLINK_SEND_AHRS_EULER_INT(DefaultChannel, DefaultDevice,
&ahrs_impl.ltp_to_imu_euler.phi,
&ahrs_impl.ltp_to_imu_euler.theta,
&ahrs_impl.ltp_to_imu_euler.psi,
&(eulers->phi),
&(eulers->theta),
&(eulers->psi));
}
static void send_bias(void) {
DOWNLINK_SEND_AHRS_GYRO_BIAS_INT(DefaultChannel, DefaultDevice,
&ahrs_impl.gyro_bias.p, &ahrs_impl.gyro_bias.q, &ahrs_impl.gyro_bias.r);
}
void ahrs_init(void) {
ahrs.status = AHRS_UNINIT;
@@ -85,6 +123,10 @@ void ahrs_init(void) {
ahrs_impl.reinj_1 = FACE_REINJ_1;
ahrs_impl.mag_offset = AHRS_MAG_OFFSET;
register_periodic_telemetry(DefaultPeriodic, "FILTER", send_filter);
register_periodic_telemetry(DefaultPeriodic, "AHRS_EULER_INT", send_euler);
register_periodic_telemetry(DefaultPeriodic, "AHRS_GYRO_BIAS_INT", send_bias);
}
void ahrs_align(void) {
sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c
+37
View File
@@ -43,6 +43,9 @@
#include "generated/airframe.h"
#include "subsystems/datalink/downlink.h"
#include "generated/periodic_telemetry.h"
//#include "../../test/pprz_algebra_print.h"
static inline void ahrs_update_mag_full(void);
@@ -76,6 +79,36 @@ float ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;
static inline void set_body_state_from_quat(void);
static void send_quat(void) {
struct Int32Quat* quat = stateGetNedToBodyQuat_i();
DOWNLINK_SEND_AHRS_QUAT_INT(DefaultChannel, DefaultDevice,
&ahrs_impl.ltp_to_imu_quat.qi,
&ahrs_impl.ltp_to_imu_quat.qx,
&ahrs_impl.ltp_to_imu_quat.qy,
&ahrs_impl.ltp_to_imu_quat.qz,
&(quat->qi),
&(quat->qx),
&(quat->qy),
&(quat->qz));
}
static void send_euler(void) {
struct Int32Eulers ltp_to_imu_euler;
INT32_EULERS_OF_QUAT(ltp_to_imu_euler, ahrs_impl.ltp_to_imu_quat);
struct Int32Eulers* eulers = stateGetNedToBodyEulers_i();
DOWNLINK_SEND_AHRS_EULER_INT(DefaultChannel, DefaultDevice,
&ltp_to_imu_euler.phi,
&ltp_to_imu_euler.theta,
&ltp_to_imu_euler.psi,
&(eulers->phi),
&(eulers->theta),
&(eulers->psi));
}
static void send_bias(void) {
DOWNLINK_SEND_AHRS_GYRO_BIAS_INT(DefaultChannel, DefaultDevice,
&ahrs_impl.gyro_bias.p, &ahrs_impl.gyro_bias.q, &ahrs_impl.gyro_bias.r);
}
void ahrs_init(void) {
@@ -105,6 +138,10 @@ void ahrs_init(void) {
VECT3_ASSIGN(ahrs_impl.mag_h, MAG_BFP_OF_REAL(AHRS_H_X), MAG_BFP_OF_REAL(AHRS_H_Y), MAG_BFP_OF_REAL(AHRS_H_Z));
register_periodic_telemetry(DefaultPeriodic, "AHRS_QUAT_INT", send_quat);
register_periodic_telemetry(DefaultPeriodic, "AHRS_EULER_INT", send_euler);
register_periodic_telemetry(DefaultPeriodic, "AHRS_GYRO_BIAS_INT", send_bias);
}
void ahrs_align(void) {
sw/airborne/subsystems/datalink/downlink.h
+1
View File
@@ -56,6 +56,7 @@
#ifdef USE_USB_SERIAL
#include "mcu_periph/usb_serial.h"
#endif
#include "mcu_periph/uart.h"
#endif /** !SITL */
#ifndef DefaultChannel
sw/airborne/subsystems/gps.c
+14 -6
View File
@@ -52,17 +52,16 @@ static void send_gps(void) {
if (i >= gps.nb_channels * 2) i = 0;
if (i < gps.nb_channels && ((gps.fix != GPS_FIX_3D) || (gps.svinfos[i].cno > 0))) {
DOWNLINK_SEND_SVINFO(DefaultChannel, DefaultDevice, &i,
&gps.svinfos[i].svid,
&gps.svinfos[i].flags,
&gps.svinfos[i].qi,
&gps.svinfos[i].cno,
&gps.svinfos[i].elev,
&gps.svinfos[i].azim);
&gps.svinfos[i].svid, &gps.svinfos[i].flags,
&gps.svinfos[i].qi, &gps.svinfos[i].cno,
&gps.svinfos[i].elev, &gps.svinfos[i].azim);
}
i++;
}
static void send_gps_int(void) {
static uint8_t i;
static uint8_t last_cnos[GPS_NB_CHANNELS];
DOWNLINK_SEND_GPS_INT(DefaultChannel, DefaultDevice,
&gps.ecef_pos.x, &gps.ecef_pos.y, &gps.ecef_pos.z,
&gps.lla_pos.lat, &gps.lla_pos.lon, &gps.lla_pos.alt,
@@ -73,6 +72,15 @@ static void send_gps_int(void) {
&gps.pdop,
&gps.num_sv,
&gps.fix);
if (i == gps.nb_channels) i = 0;
if (i < gps.nb_channels && gps.svinfos[i].cno > 0 && gps.svinfos[i].cno != last_cnos[i]) {
DOWNLINK_SEND_SVINFO(DefaultChannel, DefaultDevice, &i,
&gps.svinfos[i].svid, &gps.svinfos[i].flags,
&gps.svinfos[i].qi, &gps.svinfos[i].cno,
&gps.svinfos[i].elev, &gps.svinfos[i].azim);
last_cnos[i] = gps.svinfos[i].cno;
}
i++;
}
static void send_gps_lla(void) {
sw/airborne/subsystems/imu.c
+37 -14
View File
@@ -37,18 +37,11 @@ static void send_accel_raw(void) {
&imu.accel_unscaled.x, &imu.accel_unscaled.y, &imu.accel_unscaled.z);
}
static void send_gyro_raw(void) {
DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel, DefaultDevice,
&imu.gyro_unscaled.p, &imu.gyro_unscaled.q, &imu.gyro_unscaled.r);
static void send_accel_scaled(void) {
DOWNLINK_SEND_IMU_ACCEL_SCALED(DefaultChannel, DefaultDevice,
&imu.accel.x, &imu.accel.y, &imu.accel.z);
}
#ifdef USE_MAGNETOMETER
static void send_mag_raw(void) {
DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice,
&imu.mag_unscaled.x, &imu.mag_unscaled.y, &imu.mag_unscaled.z);
}
#endif
static void send_accel(void) {
struct FloatVect3 accel_float;
ACCELS_FLOAT_OF_BFP(accel_float, imu.accel);
@@ -56,6 +49,16 @@ static void send_accel(void) {
&accel_float.x, &accel_float.y, &accel_float.z);
}
static void send_gyro_raw(void) {
DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel, DefaultDevice,
&imu.gyro_unscaled.p, &imu.gyro_unscaled.q, &imu.gyro_unscaled.r);
}
static void send_gyro_scaled(void) {
DOWNLINK_SEND_IMU_GYRO_SCALED(DefaultChannel, DefaultDevice,
&imu.gyro.p, &imu.gyro.q, &imu.gyro.r);
}
static void send_gyro(void) {
struct FloatRates gyro_float;
RATES_FLOAT_OF_BFP(gyro_float, imu.gyro);
@@ -64,12 +67,30 @@ static void send_gyro(void) {
}
#ifdef USE_MAGNETOMETER
static void send_mag_raw(void) {
DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice,
&imu.mag_unscaled.x, &imu.mag_unscaled.y, &imu.mag_unscaled.z);
}
static void send_mag_scaled(void) {
DOWNLINK_SEND_IMU_MAG_SCALED(DefaultChannel, DefaultDevice,
&imu.mag.x, &imu.mag.y, &imu.mag.z);
}
static void send_mag(void) {
struct FloatVect3 mag_float;
MAGS_FLOAT_OF_BFP(mag_float, imu.mag);
DOWNLINK_SEND_IMU_MAG(DefaultChannel, DefaultDevice,
&mag_float.x, &mag_float.y, &mag_float.z);
}
// TODO this could be a special module ?
#include "subsystems/electrical.h"
static void send_mag_calib(void) {
DOWNLINK_SEND_IMU_MAG_CURRENT_CALIBRATION(DefaultChannel, DefaultDevice,
&imu.mag_unscaled.x, &imu.mag_unscaled.y, &imu.mag_unscaled.z,
&electrical.current);
}
#endif
struct Imu imu;
@@ -103,14 +124,16 @@ INFO("Magnetometer neutrals are set to zero, you should calibrate!")
INT32_RMAT_OF_EULERS(imu.body_to_imu_rmat, body_to_imu_eulers);
register_periodic_telemetry(DefaultPeriodic, "IMU_ACCEL_RAW", send_accel_raw);
register_periodic_telemetry(DefaultPeriodic, "IMU_GYRO_RAW", send_gyro_raw);
#ifdef USE_MAGNETOMETER
register_periodic_telemetry(DefaultPeriodic, "IMU_MAG_RAW", send_mag_raw);
#endif
register_periodic_telemetry(DefaultPeriodic, "IMU_ACCEL_SCALED", send_accel_scaled);
register_periodic_telemetry(DefaultPeriodic, "IMU_ACCEL", send_accel);
register_periodic_telemetry(DefaultPeriodic, "IMU_GYRO_RAW", send_gyro_raw);
register_periodic_telemetry(DefaultPeriodic, "IMU_GYRO_SCALED", send_gyro_scaled);
register_periodic_telemetry(DefaultPeriodic, "IMU_GYRO", send_gyro);
#ifdef USE_MAGNETOMETER
register_periodic_telemetry(DefaultPeriodic, "IMU_MAG_RAW", send_mag_raw);
register_periodic_telemetry(DefaultPeriodic, "IMU_MAG_SCALED", send_mag_scaled);
register_periodic_telemetry(DefaultPeriodic, "IMU_MAG", send_mag);
register_periodic_telemetry(DefaultPeriodic, "IMU_MAG_CURRENT_CALIBRATION", send_mag_calib);
#endif
imu_impl_init();
sw/airborne/subsystems/ins/hf_float.c
+38
View File
@@ -36,6 +36,9 @@
#include "generated/airframe.h"
#include "subsystems/datalink/downlink.h"
#include "generated/periodic_telemetry.h"
#ifdef SITL
#include <stdio.h>
#define DBG_LEVEL 1
@@ -237,7 +240,36 @@ static inline void b2_hff_update_y(struct HfilterFloat* hff_work, float y_meas,
static inline void b2_hff_update_xdot(struct HfilterFloat* hff_work, float vel, float Rvel);
static inline void b2_hff_update_ydot(struct HfilterFloat* hff_work, float vel, float Rvel);
static void send_hff(void) {
DOWNLINK_SEND_HFF(DefaultChannel, DefaultDevice,
&b2_hff_state.x,
&b2_hff_state.y,
&b2_hff_state.xdot,
&b2_hff_state.ydot,
&b2_hff_state.xdotdot,
&b2_hff_state.ydotdot);
}
static void send_hff_debug(void) {
DOWNLINK_SEND_HFF_DBG(DefaultChannel, DefaultDevice,
&b2_hff_x_meas,
&b2_hff_y_meas,
&b2_hff_xd_meas,
&b2_hff_yd_meas,
&b2_hff_state.xP[0][0],
&b2_hff_state.yP[0][0],
&b2_hff_state.xP[1][1],
&b2_hff_state.yP[1][1]);
}
#ifdef GPS_LAG
static void send_hff_gps(void) {
DOWNLINK_SEND_HFF_GPS(DefaultChannel, DefaultDevice,
&(b2_hff_rb_last->lag_counter),
&lag_counter_err,
&save_counter);
}
#endif
void b2_hff_init(float init_x, float init_xdot, float init_y, float init_ydot) {
Rgps_pos = HFF_R_POS;
@@ -278,6 +310,12 @@ void b2_hff_init(float init_x, float init_xdot, float init_y, float init_ydot) {
b2_hff_ps_counter = 1;
b2_hff_lost_counter = 0;
b2_hff_lost_limit = HFF_LOST_LIMIT;
register_periodic_telemetry(DefaultPeriodic, "HFF", send_hff);
register_periodic_telemetry(DefaultPeriodic, "HFF_DBG", send_hff_debug);
#ifdef GPS_LAG
register_periodic_telemetry(DefaultPeriodic, "HFF_GPS", send_hff_gps);
#endif
}
static inline void b2_hff_init_x(float init_x, float init_xdot) {
sw/airborne/subsystems/ins/ins_int.c
+26 -1
View File
@@ -47,11 +47,13 @@
#include <stdio.h>
#endif
#include "math/pprz_geodetic_int.h"
#include "generated/flight_plan.h"
#include "subsystems/datalink/downlink.h"
#include "generated/periodic_telemetry.h"
#ifndef USE_INS_NAV_INIT
#define USE_INS_NAV_INIT TRUE
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
@@ -84,6 +86,26 @@ struct NedCoor_i ins_ltp_pos;
struct NedCoor_i ins_ltp_speed;
struct NedCoor_i ins_ltp_accel;
static void send_ins(void) {
DOWNLINK_SEND_INS(DefaultChannel, DefaultDevice,
&ins_ltp_pos.x, &ins_ltp_pos.y, &ins_ltp_pos.z,
&ins_ltp_speed.x, &ins_ltp_speed.y, &ins_ltp_speed.z,
&ins_ltp_accel.x, &ins_ltp_accel.y, &ins_ltp_accel.z);
}
static void send_ins_z(void) {
DOWNLINK_SEND_INS_Z(DefaultChannel, DefaultDevice,
&ins_baro_alt, &ins_ltp_pos.z, &ins_ltp_speed.z, &ins_ltp_accel.z);
}
static void send_ins_ref(void) {
if (ins_ltp_initialised) {
DOWNLINK_SEND_INS_REF(DefaultChannel, DefaultDevice,
&ins_ltp_def.ecef.x, &ins_ltp_def.ecef.y, &ins_ltp_def.ecef.z,
&ins_ltp_def.lla.lat, &ins_ltp_def.lla.lon, &ins_ltp_def.lla.alt,
&ins_ltp_def.hmsl, &ins_qfe);
}
}
void ins_init() {
#if USE_INS_NAV_INIT
@@ -124,6 +146,9 @@ void ins_init() {
// TODO correct init
ins.status = INS_RUNNING;
register_periodic_telemetry(DefaultPeriodic, "INS", send_ins);
register_periodic_telemetry(DefaultPeriodic, "INS_Z", send_ins_z);
register_periodic_telemetry(DefaultPeriodic, "INS_REF", send_ins_ref);
}
void ins_periodic( void ) {
sw/airborne/subsystems/ins/vf_extended_float.c
+12 -1
View File
@@ -39,6 +39,9 @@
#include "subsystems/datalink/downlink.h"
#endif
#include "subsystems/datalink/downlink.h"
#include "generated/periodic_telemetry.h"
/*
X = [ z zdot accel_bias baro_offset ]
@@ -71,6 +74,13 @@ float vff_P[VFF_STATE_SIZE][VFF_STATE_SIZE];
float vff_z_meas;
float vff_z_meas_baro;
static void send_vffe(void) {
DOWNLINK_SEND_VFF_EXTENDED(DefaultChannel, DefaultDevice,
&vff_z_meas, &vff_z_meas_baro,
&vff_z, &vff_zdot, &vff_zdotdot,
&vff_bias, &vff_offset);
}
void vff_init(float init_z, float init_zdot, float init_accel_bias, float init_baro_offset) {
vff_z = init_z;
vff_zdot = init_zdot;
@@ -83,6 +93,7 @@ void vff_init(float init_z, float init_zdot, float init_accel_bias, float init_b
vff_P[i][i] = INIT_PXX;
}
register_periodic_telemetry(DefaultPeriodic, "VFF_EXTENDED", send_vffe);
}
@@ -134,7 +145,7 @@ void vff_propagate(float accel) {
vff_P[3][3] = FPF33 + Qoffoff;
#if DEBUG_VFF_EXTENDED
RunOnceEvery(10, DOWNLINK_SEND_VFF_EXTENDED(DefaultChannel, DefaultDevice, &vff_z_meas, &vff_z_meas_baro, &vff_z, &vff_zdot, &vff_zdotdot, &vff_bias, &vff_offset));
RunOnceEvery(10, send_vffe());
#endif
}
sw/airborne/subsystems/ins/vf_float.c
+10
View File
@@ -28,6 +28,9 @@
#include "subsystems/ins/vf_float.h"
#include "subsystems/datalink/downlink.h"
#include "generated/periodic_telemetry.h"
/*
X = [ z zdot bias ]
@@ -66,6 +69,12 @@ float vff_P[VFF_STATE_SIZE][VFF_STATE_SIZE];
float vff_z_meas;
static void send_vff(void) {
DOWNLINK_SEND_VFF(DefaultChannel, DefaultDevice,
&vff_z_meas, &vff_z, &vff_zdot, &vff_bias,
&vff_P[0][0], &vff_P[1][1], &vff_P[2][2]);
}
void vff_init(float init_z, float init_zdot, float init_bias) {
vff_z = init_z;
vff_zdot = init_zdot;
@@ -77,6 +86,7 @@ void vff_init(float init_z, float init_zdot, float init_bias) {
vff_P[i][i] = VF_FLOAT_INIT_PXX;
}
register_periodic_telemetry(DefaultPeriodic, "VFF", send_vff);
}