diff --git a/conf/firmwares/rotorcraft.makefile b/conf/firmwares/rotorcraft.makefile index 4fc5034140..7cec052725 100644 --- a/conf/firmwares/rotorcraft.makefile +++ b/conf/firmwares/rotorcraft.makefile @@ -40,9 +40,6 @@ ROTORCRAFT_INC = -I$(SRC_FIRMWARE) -I$(SRC_BOARD) ap.ARCHDIR = $(ARCH) -# would be better to auto-generate this -$(TARGET).CFLAGS += -DFIRMWARE=ROTORCRAFT - ap.CFLAGS += $(ROTORCRAFT_INC) ap.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG) -DPERIPHERALS_AUTO_INIT ap.srcs = $(SRC_FIRMWARE)/main.c diff --git a/conf/firmwares/subsystems/fixedwing/autopilot.makefile b/conf/firmwares/subsystems/fixedwing/autopilot.makefile index 0265e3edf3..4b836a9394 100644 --- a/conf/firmwares/subsystems/fixedwing/autopilot.makefile +++ b/conf/firmwares/subsystems/fixedwing/autopilot.makefile @@ -35,9 +35,6 @@ $(TARGET).CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG) $(TARGET).CFLAGS += -DPERIPHERALS_AUTO_INIT $(TARGET).CFLAGS += $(FIXEDWING_INC) -# would be better to auto-generate this -$(TARGET).CFLAGS += -DFIRMWARE=FIXEDWING - $(TARGET).srcs += mcu.c $(TARGET).srcs += $(SRC_ARCH)/mcu_arch.c diff --git a/conf/modules/gps_ubx_ucenter.xml b/conf/modules/gps_ubx_ucenter.xml index 056d7dc991..1f2880dfad 100644 --- a/conf/modules/gps_ubx_ucenter.xml +++ b/conf/modules/gps_ubx_ucenter.xml @@ -3,18 +3,20 @@ - Ublox GPS autoconfiguration. +Ublox GPS autoconfiguration. - Automatically configure any Ublox GPS for paparazzi. +Automatically configure any Ublox GPS for paparazzi. - -configures all the messages, and the rates - -automatic baudrate detection +- configures all the messages, and the rates +- automatic baudrate detection - Warning: you still need to tell the driver - a) which paparazzi uart you use - b) inside the ublox gps there are also many ports. the tiny/ppzgps use ublox_internal_port1 but if for instance you use a LS-SAM or I2C device you need to configure: +Warning: you still need to tell the driver +1. Which paparazzi uart you use +2. Inside the ublox gps there are also many ports. + The tiny/ppzgps use ublox_internal_port1 (UART1) but if for instance you use a LS-SAM + or I2C device you need to set this by defining GPS_PORT_ID. - +
diff --git a/sw/airborne/boards/umarim/baro_board.c b/sw/airborne/boards/umarim/baro_board.c index 69f5866403..c02542ed14 100644 --- a/sw/airborne/boards/umarim/baro_board.c +++ b/sw/airborne/boards/umarim/baro_board.c @@ -33,7 +33,7 @@ #include "led.h" // ADC for absolute pressure -#define BARO_ABS_ADS +#ifndef BARO_ABS_ADS #define BARO_ABS_ADS ads1114_1 #endif diff --git a/sw/airborne/firmwares/fixedwing/autopilot.h b/sw/airborne/firmwares/fixedwing/autopilot.h index 0dad775893..c5b651f7b9 100644 --- a/sw/airborne/firmwares/fixedwing/autopilot.h +++ b/sw/airborne/firmwares/fixedwing/autopilot.h @@ -32,7 +32,6 @@ #include #include "std.h" #include "paparazzi.h" -#include "mcu_periph/sys_time.h" #include "generated/airframe.h" diff --git a/sw/airborne/modules/gps/gps_ubx_ucenter.c b/sw/airborne/modules/gps/gps_ubx_ucenter.c index 719e521db3..059d536cd5 100644 --- a/sw/airborne/modules/gps/gps_ubx_ucenter.c +++ b/sw/airborne/modules/gps/gps_ubx_ucenter.c @@ -27,7 +27,6 @@ * */ - #include "gps_ubx_ucenter.h" ////////////////////////////////////////////////////////////////////////////////////// @@ -295,6 +294,9 @@ static inline void gps_ubx_ucenter_config_port(void) #if GPS_PORT_ID == GPS_PORT_UART1 || GPS_PORT_ID == GPS_PORT_UART2 UbxSend_CFG_PRT(GPS_PORT_ID, 0x0, 0x0, 0x000008D0, 38400, UBX_PROTO_MASK, UBX_PROTO_MASK, 0x0, 0x0); #endif + #if GPS_PORT_ID == GPS_PORT_USB + UbxSend_CFG_PRT(GPS_PORT_ID, 0x0, 0x0, 0x0, 0, UBX_PROTO_MASK, UBX_PROTO_MASK, 0x0, 0x0); + #endif } #define GPS_SBAS_ENABLED 0x01 @@ -319,6 +321,9 @@ static inline void gps_ubx_ucenter_enable_msg(uint8_t class, uint8_t id, uint8_t #if GPS_PORT_ID == GPS_PORT_UART2 UbxSend_CFG_MSG(class, id, 0, 0, rate, 0); #endif + #if GPS_PORT_ID == GPS_PORT_USB + UbxSend_CFG_MSG(class, id, 0, 0, 0, rate); + #endif #if GPS_PORT_ID == GPS_PORT_DDC UbxSend_CFG_MSG(class, id, rate, 0, 0, 0); #endif @@ -385,10 +390,10 @@ static bool_t gps_ubx_ucenter_configure(uint8_t nr) gps_ubx_ucenter_enable_msg(UBX_NAV_ID, UBX_NAV_SVINFO_ID, 4); break; case 11: -#if defined FIRMWARE && FIRMWARE == ROTORCRAFT - gps_ubx_ucenter_enable_msg(UBX_NAV_ID, UBX_NAV_SOL_ID, 1); -#else +#if GPS_UBX_UCENTER_SLOW_NAV_SOL gps_ubx_ucenter_enable_msg(UBX_NAV_ID, UBX_NAV_SOL_ID, 8); +#else + gps_ubx_ucenter_enable_msg(UBX_NAV_ID, UBX_NAV_SOL_ID, 1); #endif break; case 12: @@ -416,7 +421,3 @@ static bool_t gps_ubx_ucenter_configure(uint8_t nr) } return TRUE; // Continue, except for the last case } - - - - diff --git a/sw/airborne/modules/sensors/baro_mpl3115.c b/sw/airborne/modules/sensors/baro_mpl3115.c index b4c96b9857..37a97e29be 100644 --- a/sw/airborne/modules/sensors/baro_mpl3115.c +++ b/sw/airborne/modules/sensors/baro_mpl3115.c @@ -20,7 +20,7 @@ */ /** - * @file modules/sensors/baro_mpl3155.c + * @file modules/sensors/baro_mpl3115.c * * Module for the baro MPL3115A2 from Freescale (i2c) * diff --git a/sw/airborne/modules/sensors/baro_mpl3115.h b/sw/airborne/modules/sensors/baro_mpl3115.h index de6568a356..4267386071 100644 --- a/sw/airborne/modules/sensors/baro_mpl3115.h +++ b/sw/airborne/modules/sensors/baro_mpl3115.h @@ -20,7 +20,7 @@ */ /** - * @file modules/sensors/baro_mpl3155.h + * @file modules/sensors/baro_mpl3115.h * * Module for the baro MPL3115A2 from Freescale (i2c) * diff --git a/sw/airborne/test/ahrs/Makefile b/sw/airborne/test/ahrs/Makefile index 7904f638e8..036787a701 100644 --- a/sw/airborne/test/ahrs/Makefile +++ b/sw/airborne/test/ahrs/Makefile @@ -3,14 +3,13 @@ Q=@ CC = gcc -CFLAGS = -std=c99 -I.. -I../.. -I../../../include -I../../booz -I../../../booz -Wall +CFLAGS = -std=c99 -I.. -I../.. -I../../../include -Wall LDFLAGS = -lm -lgsl -lgslcblas CFLAGS += # imu wants airframe to fetch its neutrals # ahrs wants airframe to fetch IMU_BODY_TO_IMU_ANGLES -#CFLAGS += -I../../../../var/BOOZ2_A7 CFLAGS += -I../../../../var/ahrs_test @@ -51,28 +50,15 @@ AHRS_TYPE = AHRS_TYPE_ICQ #AHRS_TYPE = AHRS_TYPE_FCR2 #AHRS_TYPE = AHRS_TYPE_FCQ endif -ifndef PROPAGATE_LOW_PASS_RATES -PROPAGATE_LOW_PASS_RATES = 0 -endif -ifndef GRAVITY_UPDATE_NORM_HEURISTIC -GRAVITY_UPDATE_NORM_HEURISTIC = 0 -endif -ifndef GRAVITY_UPDATE_COORDINATED_TURN -GRAVITY_UPDATE_COORDINATED_TURN = 0 -endif -ifndef MAG_UPDATE_YAW_ONLY -MAG_UPDATE_YAW_ONLY = 0 -endif -ifndef DISABLE_MAG_UPDATE -DISABLE_MAG_UPDATE = 0 -endif -ifndef USE_GPS_HEADING -USE_GPS_HEADING = 0 -endif -ifndef FREQUENCY -FREQENCY = 512 -endif +PROPAGATE_LOW_PASS_RATES ?= 0 +GRAVITY_UPDATE_NORM_HEURISTIC ?= 0 +GRAVITY_UPDATE_COORDINATED_TURN ?= 0 +MAG_UPDATE_YAW_ONLY ?= 0 +DISABLE_MAG_UPDATE ?= 0 +USE_GPS_HEADING ?= 0 + +FREQENCY ?= 512 AHRS_CFLAGS += -DAHRS_TYPE=$(AHRS_TYPE) @@ -114,10 +100,6 @@ ifeq ($(AHRS_TYPE), AHRS_TYPE_ICQ) AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl_quat.h\" AHRS_SRCS += ../../subsystems/ahrs/ahrs_int_cmpl_quat.c endif -ifeq ($(AHRS_TYPE), AHRS_TYPE_FLQ) -AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_lkf_quat.h\" -AHRS_SRCS += ../../subsystems/ahrs/ahrs_float_lkf_quat.c -endif ifeq ($(AHRS_TYPE), AHRS_TYPE_FCR) AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_dcm.h\" AHRS_SRCS += ../../subsystems/ahrs/ahrs_float_dcm.c @@ -135,9 +117,14 @@ endif RAOS_SRCS = ./ahrs_on_synth.c \ + ../../state.c \ ../../subsystems/imu.c \ ../../subsystems/imu/imu_dummy.c \ ../../math/pprz_trig_int.c \ + ../../math/pprz_orientation_conversion.c \ + ../../math/pprz_geodetic_int.c \ + ../../math/pprz_geodetic_float.c \ + ../../math/pprz_geodetic_double.c \ ../../../simulator/nps/nps_random.c diff --git a/sw/airborne/test/ahrs/ahrs_utils.py b/sw/airborne/test/ahrs/ahrs_utils.py index b6636d2584..60d33d7e9a 100644 --- a/sw/airborne/test/ahrs/ahrs_utils.py +++ b/sw/airborne/test/ahrs/ahrs_utils.py @@ -31,48 +31,45 @@ from array import array import numpy import matplotlib.pyplot as plt + def run_simulation(ahrs_type, build_opt, traj_nb): print "\nBuilding ahrs" - args = ["make", "clean", "run_ahrs_on_synth", "AHRS_TYPE=AHRS_TYPE_"+ahrs_type] + build_opt -# print args + args = ["make", "clean", "run_ahrs_on_synth", "AHRS_TYPE=AHRS_TYPE_" + ahrs_type] + build_opt + # print args p = subprocess.Popen(args=args, stdout=subprocess.PIPE, stderr=subprocess.STDOUT, shell=False) outputlines = p.stdout.readlines() p.wait() for i in outputlines: - print " # "+i, + print " # " + i, print print "Running simulation" print " using traj " + str(traj_nb) - p = subprocess.Popen(args=["./run_ahrs_on_synth",str(traj_nb)], stdout=subprocess.PIPE, stderr=subprocess.STDOUT, shell=False) + p = subprocess.Popen(args=["./run_ahrs_on_synth", str(traj_nb)], stdout=subprocess.PIPE, stderr=subprocess.STDOUT, + shell=False) outputlines = p.stdout.readlines() p.wait() -# for i in outputlines: -# print " "+i, -# print "\n" + # for i in outputlines: + # print " "+i, + # print "\n" ahrs_data_type = [('time', 'float32'), ('phi_true', 'float32'), ('theta_true', 'float32'), ('psi_true', 'float32'), - ('p_true', 'float32'), ('q_true', 'float32'), ('r_true', 'float32'), - ('bp_true', 'float32'), ('bq_true', 'float32'), ('br_true', 'float32'), + ('p_true', 'float32'), ('q_true', 'float32'), ('r_true', 'float32'), + ('bp_true', 'float32'), ('bq_true', 'float32'), ('br_true', 'float32'), ('phi_ahrs', 'float32'), ('theta_ahrs', 'float32'), ('psi_ahrs', 'float32'), - ('p_ahrs', 'float32'), ('q_ahrs', 'float32'), ('r_ahrs', 'float32'), - ('bp_ahrs', 'float32'), ('bq_ahrs', 'float32'), ('br_ahrs', 'float32') - ] - pos_data_type = [ ('x0_true', 'float32'), ('y0_true', 'float32'), ('z0_true', 'float32'), - ('x1_true', 'float32'), ('y1_true', 'float32'), ('z1_true', 'float32'), - ('x2_true', 'float32'), ('y2_true', 'float32'), ('z2_true', 'float32'), - ('x3_true', 'float32'), ('y3_true', 'float32'), ('z3_true', 'float32'), - ] + ('p_ahrs', 'float32'), ('q_ahrs', 'float32'), ('r_ahrs', 'float32'), + ('bp_ahrs', 'float32'), ('bq_ahrs', 'float32'), ('br_ahrs', 'float32')] + mydescr = numpy.dtype(ahrs_data_type) data = [[] for dummy in xrange(len(mydescr))] -# import code; code.interact(local=locals()) + # import code; code.interact(local=locals()) for line in outputlines: if line.startswith("#"): - print " "+line, + print " " + line, else: - fields = line.strip().split(' '); -# print fields + fields = line.strip().split(' ') + # print fields for i, number in enumerate(fields): data[i].append(number) @@ -83,77 +80,74 @@ def run_simulation(ahrs_type, build_opt, traj_nb): return numpy.rec.array(data, dtype=mydescr) - def plot_simulation_results(plot_true_state, lsty, type, sim_res): print "Plotting Results" - # f, (ax1, ax2, ax3) = plt.subplots(3, sharex=True, sharey=True) + # f, (ax1, ax2, ax3) = plt.subplots(3, sharex=True, sharey=True) - subplot(3,3,1) + subplot(3, 3, 1) plt.plot(sim_res.time, sim_res.phi_ahrs, lsty, label=type) ylabel('degres') title('phi') legend() - subplot(3,3,2) + subplot(3, 3, 2) plot(sim_res.time, sim_res.theta_ahrs, lsty) title('theta') - subplot(3,3,3) + subplot(3, 3, 3) plot(sim_res.time, sim_res.psi_ahrs, lsty) title('psi') - subplot(3,3,4) + subplot(3, 3, 4) plt.plot(sim_res.time, sim_res.p_ahrs, lsty) ylabel('degres/s') title('p') - subplot(3,3,5) + subplot(3, 3, 5) plt.plot(sim_res.time, sim_res.q_ahrs, lsty) title('q') - subplot(3,3,6) + subplot(3, 3, 6) plt.plot(sim_res.time, sim_res.r_ahrs, lsty) title('r') - subplot(3,3,7) + subplot(3, 3, 7) plt.plot(sim_res.time, sim_res.bp_ahrs, lsty) ylabel('degres/s') xlabel('time in s') title('bp') - subplot(3,3,8) + subplot(3, 3, 8) plt.plot(sim_res.time, sim_res.bq_ahrs, lsty) xlabel('time in s') title('bq') - subplot(3,3,9) + subplot(3, 3, 9) plt.plot(sim_res.time, sim_res.br_ahrs, lsty) xlabel('time in s') title('br') - if plot_true_state: - subplot(3,3,1) + subplot(3, 3, 1) plt.plot(sim_res.time, sim_res.phi_true, 'r--') - subplot(3,3,2) + subplot(3, 3, 2) plot(sim_res.time, sim_res.theta_true, 'r--') - subplot(3,3,3) + subplot(3, 3, 3) plot(sim_res.time, sim_res.psi_true, 'r--') - subplot(3,3,4) + subplot(3, 3, 4) plot(sim_res.time, sim_res.p_true, 'r--') - subplot(3,3,5) + subplot(3, 3, 5) plot(sim_res.time, sim_res.q_true, 'r--') - subplot(3,3,6) + subplot(3, 3, 6) plot(sim_res.time, sim_res.r_true, 'r--') - subplot(3,3,7) + subplot(3, 3, 7) plot(sim_res.time, sim_res.bp_true, 'r--') - subplot(3,3,8) + subplot(3, 3, 8) plot(sim_res.time, sim_res.bq_true, 'r--') - subplot(3,3,9) + subplot(3, 3, 9) plot(sim_res.time, sim_res.br_true, 'r--') - def show_plot(): - plt.show(); + plt.show() diff --git a/sw/airborne/test/ahrs/compare_ahrs.py b/sw/airborne/test/ahrs/compare_ahrs.py index 69fafbac49..524af47d55 100755 --- a/sw/airborne/test/ahrs/compare_ahrs.py +++ b/sw/airborne/test/ahrs/compare_ahrs.py @@ -24,51 +24,51 @@ import ahrs_utils def main(): -# traj_nb = 0; # static -# traj_nb = 1; # sine orientation -# traj_nb = 2; # sine X quad -# traj_nb = 3; # step_phi -# traj_nb = 4; # step_phi_2nd_order -# traj_nb = 5; # step_bias -# traj_nb = 6; # coordinated circle -# traj_nb = 7; # stop stop X quad - traj_nb = 8; # bungee takeoff +# traj_nb = 0 # static +# traj_nb = 1 # sine orientation +# traj_nb = 2 # sine X quad +# traj_nb = 3 # step_phi +# traj_nb = 4 # step_phi_2nd_order +# traj_nb = 5 # step_bias +# traj_nb = 6 # coordinated circle +# traj_nb = 7 # stop stop X quad + traj_nb = 8 # bungee takeoff - build_opt1 = []; -# build_opt1 = ["Q="]; - build_opt1 += ["FREQUENCY=120"]; -# build_opt1 += ["PROPAGATE_LOW_PASS_RATES=1"]; - build_opt1 += ["GRAVITY_UPDATE_COORDINATED_TURN=1"]; - build_opt1 += ["GRAVITY_UPDATE_NORM_HEURISTIC=1"]; -# build_opt1 += ["DISABLE_MAG_UPDATE=1"]; -# build_opt1 += ["USE_GPS_HEADING=1"]; -# build_opt1 += ["MAG_UPDATE_YAW_ONLY=1"]; + build_opt1 = [] +# build_opt1 = ["Q="] + build_opt1 += ["FREQUENCY=120"] +# build_opt1 += ["PROPAGATE_LOW_PASS_RATES=1"] + build_opt1 += ["GRAVITY_UPDATE_COORDINATED_TURN=1"] + build_opt1 += ["GRAVITY_UPDATE_NORM_HEURISTIC=1"] +# build_opt1 += ["DISABLE_MAG_UPDATE=1"] +# build_opt1 += ["USE_GPS_HEADING=1"] +# build_opt1 += ["MAG_UPDATE_YAW_ONLY=1"] -# ahrs_type1 = "FCQ"; -# ahrs_type1 = "FCR2"; -# ahrs_type1 = "FLQ"; - ahrs_type1 = "ICQ"; -# ahrs_type1 = "FCR"; +# ahrs_type1 = "FCQ" +# ahrs_type1 = "FCR2" +# ahrs_type1 = "FLQ" + ahrs_type1 = "ICQ" +# ahrs_type1 = "FCR" sim_res1 = ahrs_utils.run_simulation(ahrs_type1, build_opt1, traj_nb) # import code; code.interact(local=locals()) - build_opt2 = []; - build_opt2 += ["FREQUENCY=120"]; - build_opt2 += ["GRAVITY_UPDATE_COORDINATED_TURN=1"]; - build_opt2 += ["GRAVITY_UPDATE_NORM_HEURISTIC=1"]; - build_opt2 += ["USE_AHRS_GPS_ACCELERATIONS=1"]; -# build_opt2 += ["DISABLE_MAG_UPDATE=1"]; -# build_opt2 += ["USE_GPS_HEADING=1"]; -# build_opt2 += ["MAG_UPDATE_YAW_ONLY=0"]; -# build_opt2 += ["PROPAGATE_LOW_PASS_RATES=1"]; -# build_opt2 = build_opt1; + build_opt2 = [] + build_opt2 += ["FREQUENCY=120"] + build_opt2 += ["GRAVITY_UPDATE_COORDINATED_TURN=1"] + build_opt2 += ["GRAVITY_UPDATE_NORM_HEURISTIC=1"] + build_opt2 += ["USE_AHRS_GPS_ACCELERATIONS=1"] +# build_opt2 += ["DISABLE_MAG_UPDATE=1"] +# build_opt2 += ["USE_GPS_HEADING=1"] +# build_opt2 += ["MAG_UPDATE_YAW_ONLY=0"] +# build_opt2 += ["PROPAGATE_LOW_PASS_RATES=1"] +# build_opt2 = build_opt1 -# ahrs_type2 = "FLQ"; -# ahrs_type2 = "FCR2"; -# ahrs_type2 = "ICQ"; - ahrs_type2 = "FCR"; -# ahrs_type2 = ahrs_type1; +# ahrs_type2 = "FLQ" +# ahrs_type2 = "FCR2" +# ahrs_type2 = "ICQ" + ahrs_type2 = "FCR" +# ahrs_type2 = ahrs_type1 sim_res2 = ahrs_utils.run_simulation(ahrs_type2, build_opt2, traj_nb) diff --git a/sw/airborne/test/ahrs/run_ahrs_on_synth.c b/sw/airborne/test/ahrs/run_ahrs_on_synth.c index a0d45107a7..57f7deef13 100644 --- a/sw/airborne/test/ahrs/run_ahrs_on_synth.c +++ b/sw/airborne/test/ahrs/run_ahrs_on_synth.c @@ -37,10 +37,6 @@ static void report(void) { int output_sensors = FALSE; int output_pos = FALSE; -#if AHRS_TYPE == AHRS_TYPE_ICE || AHRS_TYPE == AHRS_TYPE_ICQ - EULERS_FLOAT_OF_BFP(ahrs_float.ltp_to_imu_euler, ahrs.ltp_to_imu_euler); - RATES_FLOAT_OF_BFP(ahrs_float.imu_rate, ahrs.imu_rate); -#endif printf("%f ", aos.time); printf("%f %f %f ", DegOfRad(aos.ltp_to_imu_euler.phi), @@ -55,23 +51,37 @@ static void report(void) { DegOfRad(aos.gyro_bias.q), DegOfRad(aos.gyro_bias.r)); - - printf("%f %f %f ", DegOfRad(ahrs_float.ltp_to_imu_euler.phi), - DegOfRad(ahrs_float.ltp_to_imu_euler.theta), - DegOfRad(ahrs_float.ltp_to_imu_euler.psi)); - - printf("%f %f %f ", DegOfRad(ahrs_float.imu_rate.p), - DegOfRad(ahrs_float.imu_rate.q), - DegOfRad(ahrs_float.imu_rate.r)); - #if AHRS_TYPE == AHRS_TYPE_ICQ + struct Int32Eulers ltp_to_imu_euler_i; + INT32_EULERS_OF_QUAT(ltp_to_imu_euler_i, ahrs_impl.ltp_to_imu_quat); + struct FloatEulers ltp_to_imu_euler_f; + EULERS_FLOAT_OF_BFP(ltp_to_imu_euler_f, ltp_to_imu_euler_i); + printf("%f %f %f ", DegOfRad(ltp_to_imu_euler_f.phi), + DegOfRad(ltp_to_imu_euler_f.theta), + DegOfRad(ltp_to_imu_euler_f.psi)); + + struct FloatRates imu_rate_f; + RATES_FLOAT_OF_BFP(imu_rate_f, ahrs_impl.imu_rate); + printf("%f %f %f ", DegOfRad(imu_rate_f.p), + DegOfRad(imu_rate_f.q), + DegOfRad(imu_rate_f.r)); + struct FloatRates bias; RATES_FLOAT_OF_BFP(bias, ahrs_impl.gyro_bias); printf("%f %f %f ", DegOfRad(bias.p), DegOfRad(bias.q), DegOfRad(bias.r)); -#endif -#if AHRS_TYPE == AHRS_TYPE_FLQ || AHRS_TYPE == AHRS_TYPE_FCR2 || AHRS_TYPE == AHRS_TYPE_FCQ || AHRS_TYPE == AHRS_TYPE_FCR + +#elif AHRS_TYPE == AHRS_TYPE_FCR2 || AHRS_TYPE == AHRS_TYPE_FCQ || AHRS_TYPE == AHRS_TYPE_FCR + + printf("%f %f %f ", DegOfRad(ahrs_impl.ltp_to_imu_euler.phi), + DegOfRad(ahrs_impl.ltp_to_imu_euler.theta), + DegOfRad(ahrs_impl.ltp_to_imu_euler.psi)); + + printf("%f %f %f ", DegOfRad(ahrs_impl.imu_rate.p), + DegOfRad(ahrs_impl.imu_rate.q), + DegOfRad(ahrs_impl.imu_rate.r)); + printf("%f %f %f ", DegOfRad(ahrs_impl.gyro_bias.p), DegOfRad(ahrs_impl.gyro_bias.q), DegOfRad(ahrs_impl.gyro_bias.r));