diff --git a/conf/autopilot/subsystems/rotorcraft/imu_b2_v1.0.makefile b/conf/autopilot/subsystems/rotorcraft/imu_b2_v1.0.makefile index 647b0d7c4d..5c00d4ce6d 100644 --- a/conf/autopilot/subsystems/rotorcraft/imu_b2_v1.0.makefile +++ b/conf/autopilot/subsystems/rotorcraft/imu_b2_v1.0.makefile @@ -35,6 +35,10 @@ # # imu Booz2 v1 + +# add imu arch to include directories +ap.CFLAGS += -I$(SRC_FIRMWARE)/imu/arch/$(ARCH) + ap.CFLAGS += -DBOOZ_IMU_TYPE_H=\"imu/imu_b2.h\" ap.CFLAGS += -DIMU_B2_VERSION_1_0 ap.CFLAGS += -DIMU_B2_MAG_TYPE=IMU_B2_MAG_AMI601 @@ -56,12 +60,15 @@ ap.CFLAGS += -DUSE_I2C1 -DI2C1_SCLL=150 -DI2C1_SCLH=150 -DI2C1_VIC_SLOT=11 # Simulator # +# add imu arch to include directories +sim.CFLAGS += -I$(SRC_FIRMWARE)/imu/arch/$(ARCH) + sim.CFLAGS += -DBOOZ_IMU_TYPE_H=\"imu/imu_b2.h\" sim.CFLAGS += -DIMU_B2_VERSION_1_0 sim.CFLAGS += -DIMU_B2_MAG_TYPE=IMU_B2_MAG_AMI601 sim.srcs += $(SRC_FIRMWARE)/imu.c \ $(SRC_FIRMWARE)/imu/imu_b2.c \ - $(SRC_FIRMWARE)/imu/imu_b2_arch.c + $(SRC_FIRMWARE)/imu/arch/$(ARCH)/imu_b2_arch.c sim.srcs += $(SRC_BOOZ)/peripherals/booz_max1168.c \ $(SRC_BOOZ_SIM)/peripherals/booz_max1168_arch.c diff --git a/conf/autopilot/subsystems/rotorcraft/imu_b2_v1.1.makefile b/conf/autopilot/subsystems/rotorcraft/imu_b2_v1.1.makefile index f2bcd4367c..2c9a338e64 100644 --- a/conf/autopilot/subsystems/rotorcraft/imu_b2_v1.1.makefile +++ b/conf/autopilot/subsystems/rotorcraft/imu_b2_v1.1.makefile @@ -40,6 +40,10 @@ # imu Booz2 v1.1 + +# add imu arch to include directories +ap.CFLAGS += -I$(SRC_FIRMWARE)/imu/arch/$(ARCH) + imu_CFLAGS += -DBOOZ_IMU_TYPE_H=\"imu/imu_b2.h\" imu_CFLAGS += -DIMU_B2_MAG_TYPE=IMU_B2_MAG_MS2001 imu_CFLAGS += -DIMU_B2_VERSION_1_1 @@ -72,12 +76,15 @@ ap.srcs += $(imu_srcs) # Simulator # +# add imu arch to include directories +sim.CFLAGS += -I$(SRC_FIRMWARE)/imu/arch/$(ARCH) + sim.CFLAGS += -DBOOZ_IMU_TYPE_H=\"imu/imu_b2.h\" sim.CFLAGS += -DIMU_B2_VERSION_1_1 sim.CFLAGS += -DIMU_B2_MAG_TYPE=IMU_B2_MAG_AMI601 sim.srcs += $(SRC_FIRMWARE)/imu.c \ $(SRC_FIRMWARE)/imu/imu_b2.c \ - $(SRC_FIRMWARE)/imu/imu_b2_arch.c + $(SRC_FIRMWARE)/imu/arch/$(ARCH)/imu_b2_arch.c sim.srcs += $(SRC_BOOZ)/peripherals/booz_max1168.c \ $(SRC_BOOZ_SIM)/peripherals/booz_max1168_arch.c diff --git a/conf/autopilot/subsystems/rotorcraft/imu_crista.makefile b/conf/autopilot/subsystems/rotorcraft/imu_crista.makefile index 53e36b8a17..08b6138db6 100644 --- a/conf/autopilot/subsystems/rotorcraft/imu_crista.makefile +++ b/conf/autopilot/subsystems/rotorcraft/imu_crista.makefile @@ -60,6 +60,9 @@ +# add imu arch to include directories +ap.CFLAGS += -I$(SRC_FIRMWARE)/imu/arch/$(ARCH) + ap.CFLAGS += -DBOOZ_IMU_TYPE_H=\"imu/imu_crista.h\" ap.srcs += $(SRC_FIRMWARE)/imu.c \ $(SRC_FIRMWARE)/imu/imu_crista.c \ @@ -70,11 +73,17 @@ ap.srcs += $(SRC_BOOZ)/peripherals/booz_ami601.c ap.CFLAGS += -DUSE_I2C1 -DI2C1_SCLL=150 -DI2C1_SCLH=150 -DI2C1_VIC_SLOT=11 -DI2C1_BUF_LEN=16 +# +# Simulator +# + +# add imu arch to include directories +sim.CFLAGS += -I$(SRC_FIRMWARE)/imu/arch/$(ARCH) sim.CFLAGS += -DBOOZ_IMU_TYPE_H=\"imu/imu_crista.h\" sim.srcs += $(SRC_FIRMWARE)/imu.c \ $(SRC_FIRMWARE)/imu/imu_crista.c \ - $(SRC_FIRMWARE)/imu/imu_crista_arch.c + $(SRC_FIRMWARE)/imu/arch/$(ARCH)/imu_crista_arch.c sim.CFLAGS += -DUSE_AMI601 sim.srcs += $(SRC_BOOZ)/peripherals/booz_ami601.c diff --git a/sw/airborne/arch/sim/max1167_hw.c b/sw/airborne/arch/sim/max1167_hw.c index e7dd55a697..28ef09b047 100644 --- a/sw/airborne/arch/sim/max1167_hw.c +++ b/sw/airborne/arch/sim/max1167_hw.c @@ -24,7 +24,7 @@ #include "max1167.h" -#include "booz_imu.h" +#include "imu.h" void max1167_hw_init( void ) {} diff --git a/sw/airborne/beth/main_overo.c b/sw/airborne/beth/main_overo.c index 405440257e..14b5166b7e 100644 --- a/sw/airborne/beth/main_overo.c +++ b/sw/airborne/beth/main_overo.c @@ -37,7 +37,7 @@ #include "fms_debug.h" #include "fms_spi_link.h" #include "fms_autopilot_msg.h" -#include "booz/booz_imu.h" +#include "imu.h" #include "overo_file_logger.h" #include "overo_gcs_com.h" @@ -55,8 +55,8 @@ static void main_talk_with_stm32(void); static struct AutopilotMessageCRCFrame msg_in; static struct AutopilotMessageCRCFrame msg_out; -struct BoozImu booz_imu; -struct BoozImuFloat booz_imu_float; +struct Imu imu; +struct ImuFloat imu_float; static uint32_t foo = 0; @@ -68,9 +68,9 @@ int main(int argc, char *argv[]) { (void) signal(SIGINT, main_exit); //set IMU neutrals - RATES_ASSIGN(booz_imu.gyro_neutral, IMU_GYRO_P_NEUTRAL, IMU_GYRO_Q_NEUTRAL, IMU_GYRO_R_NEUTRAL); - VECT3_ASSIGN(booz_imu.accel_neutral, IMU_ACCEL_X_NEUTRAL, IMU_ACCEL_Y_NEUTRAL, IMU_ACCEL_Z_NEUTRAL); - VECT3_ASSIGN(booz_imu.mag_neutral, IMU_MAG_X_NEUTRAL, IMU_MAG_Y_NEUTRAL, IMU_MAG_Z_NEUTRAL); + RATES_ASSIGN(imu.gyro_neutral, IMU_GYRO_P_NEUTRAL, IMU_GYRO_Q_NEUTRAL, IMU_GYRO_R_NEUTRAL); + VECT3_ASSIGN(imu.accel_neutral, IMU_ACCEL_X_NEUTRAL, IMU_ACCEL_Y_NEUTRAL, IMU_ACCEL_Z_NEUTRAL); + VECT3_ASSIGN(imu.mag_neutral, IMU_MAG_X_NEUTRAL, IMU_MAG_Y_NEUTRAL, IMU_MAG_Z_NEUTRAL); if (spi_link_init()) { TRACE(TRACE_ERROR, "%s", "failed to open SPI link \n"); @@ -120,7 +120,7 @@ static void main_periodic(int my_sig_num) { main_talk_with_stm32(); - BoozImuScaleGyro(booz_imu); + ImuScaleGyro(imu); RunOnceEvery(50, {DOWNLINK_SEND_BETH(gcs_com.udp_transport, &msg_in.payload.msg_up.bench_sensor.x,&msg_in.payload.msg_up.bench_sensor.y, @@ -155,19 +155,19 @@ static void main_periodic(int my_sig_num) { /* RunOnceEvery(10, {DOWNLINK_SEND_IMU_GYRO_RAW(gcs_com.udp_transport, //&msg_in.payload.msg_up.gyro.p,&msg_in.payload.msg_up.gyro.q,&msg_in.payload.msg_up.gyro.r) - &booz_imu.gyro_unscaled.p,&booz_imu.gyro_unscaled.q,&booz_imu.gyro_unscaled.r);}); + &imu.gyro_unscaled.p,&imu.gyro_unscaled.q,&imu.gyro_unscaled.r);}); RunOnceEvery(10, {DOWNLINK_SEND_IMU_ACCEL_RAW(gcs_com.udp_transport, //&msg_in.payload.msg_up.accel.x,&msg_in.payload.msg_up.accel.y,&msg_in.payload.msg_up.accel.z - &booz_imu.accel_unscaled.x,&booz_imu.accel_unscaled.y,&booz_imu.accel_unscaled.z);}) + &imu.accel_unscaled.x,&imu.accel_unscaled.y,&imu.accel_unscaled.z);}) RunOnceEvery(50, {DOWNLINK_SEND_BOOZ2_GYRO(gcs_com.udp_transport, //&msg_in.payload.msg_up.gyro.p,&msg_in.payload.msg_up.gyro.q,&msg_in.payload.msg_up.gyro.r) - &booz_imu.gyro.p,&booz_imu.gyro.q,&booz_imu.gyro.r);}); + &imu.gyro.p,&imu.gyro.q,&imu.gyro.r);}); RunOnceEvery(50, {DOWNLINK_SEND_AHRS_EULER(gcs_com.udp_transport, &estimator.tilt, &estimator.elevation, &estimator.azimuth );}); RunOnceEvery(50, {DOWNLINK_SEND_BOOZ2_ACCEL(DefaultChannel, //&msg_in.payload.msg_up.accel.x,&msg_in.payload.msg_up.accel.y,&msg_in.payload.msg_up.accel.z - &booz_imu.accel.x,&booz_imu.accel.y,&booz_imu.accel.z);});*/ + &imu.accel.x,&imu.accel.y,&imu.accel.z);});*/ RunOnceEvery(33, gcs_com_periodic()); @@ -293,12 +293,12 @@ static void main_talk_with_stm32() { foo++; - booz_imu.gyro_unscaled.p = (msg_in.payload.msg_up.gyro.p&0xFFFF); - booz_imu.gyro_unscaled.q = (msg_in.payload.msg_up.gyro.q&0xFFFF); - booz_imu.gyro_unscaled.r = (msg_in.payload.msg_up.gyro.r&0xFFFF); - booz_imu.accel_unscaled.x = (msg_in.payload.msg_up.accel.x&0xFFFF); - booz_imu.accel_unscaled.y = (msg_in.payload.msg_up.accel.y&0xFFFF); - booz_imu.accel_unscaled.z = (msg_in.payload.msg_up.accel.z&0xFFFF); + imu.gyro_unscaled.p = (msg_in.payload.msg_up.gyro.p&0xFFFF); + imu.gyro_unscaled.q = (msg_in.payload.msg_up.gyro.q&0xFFFF); + imu.gyro_unscaled.r = (msg_in.payload.msg_up.gyro.r&0xFFFF); + imu.accel_unscaled.x = (msg_in.payload.msg_up.accel.x&0xFFFF); + imu.accel_unscaled.y = (msg_in.payload.msg_up.accel.y&0xFFFF); + imu.accel_unscaled.z = (msg_in.payload.msg_up.accel.z&0xFFFF); } diff --git a/sw/airborne/beth/main_stm32.c b/sw/airborne/beth/main_stm32.c index 768cf648cd..247544cc7c 100644 --- a/sw/airborne/beth/main_stm32.c +++ b/sw/airborne/beth/main_stm32.c @@ -31,7 +31,7 @@ #include "booz/booz2_commands.h" #include "booz/booz_actuators.h" //#include "booz/booz_radio_control.h" -#include "booz/booz_imu.h" +#include "imu.h" #include "lisa/lisa_overo_link.h" #include "beth/bench_sensors.h" @@ -65,7 +65,7 @@ static inline void main_init( void ) { sys_time_init(); actuators_init(); //radio_control_init(); - booz_imu_init(); + imu_init(); overo_link_init(); bench_sensors_init(); booz2_commands[COMMAND_ROLL] = 0; @@ -76,7 +76,7 @@ static inline void main_init( void ) { static inline void main_periodic( void ) { int8_t pitch_out,thrust_out; - booz_imu_periodic(); + imu_periodic(); OveroLinkPeriodic(main_on_overo_link_lost) @@ -114,7 +114,7 @@ static inline void main_periodic( void ) { } static inline void main_event( void ) { - BoozImuEvent(on_gyro_accel_event, on_mag_event); + ImuEvent(on_gyro_accel_event, on_mag_event); OveroLinkEvent(main_on_overo_msg_received,main_on_overo_link_error); } @@ -124,13 +124,13 @@ static inline void main_on_overo_msg_received(void) { overo_link.up.msg.bench_sensor.y = bench_sensors.angle_2; overo_link.up.msg.bench_sensor.z = bench_sensors.angle_3; - overo_link.up.msg.accel.x = booz_imu.accel_unscaled.x; - overo_link.up.msg.accel.y = booz_imu.accel_unscaled.y; - overo_link.up.msg.accel.z = booz_imu.accel_unscaled.z; + overo_link.up.msg.accel.x = imu.accel_unscaled.x; + overo_link.up.msg.accel.y = imu.accel_unscaled.y; + overo_link.up.msg.accel.z = imu.accel_unscaled.z; - overo_link.up.msg.gyro.p = booz_imu.gyro_unscaled.p; - overo_link.up.msg.gyro.q = booz_imu.gyro_unscaled.q; - overo_link.up.msg.gyro.r = booz_imu.gyro_unscaled.r; + overo_link.up.msg.gyro.p = imu.gyro_unscaled.p; + overo_link.up.msg.gyro.q = imu.gyro_unscaled.q; + overo_link.up.msg.gyro.r = imu.gyro_unscaled.r; //can_err_flags (uint16) represents the board number that is not communicating regularly //spi_errors (uint16) reflects the number of crc errors on the spi link @@ -151,8 +151,8 @@ static inline void main_on_overo_link_lost(void) { static inline void on_gyro_accel_event(void) { - BoozImuScaleGyro(booz_imu); - BoozImuScaleAccel(booz_imu); + ImuScaleGyro(imu); + ImuScaleAccel(imu); //LED_TOGGLE(2); static uint8_t cnt; @@ -161,46 +161,46 @@ static inline void on_gyro_accel_event(void) { if (cnt == 0) { DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel, - &booz_imu.gyro_unscaled.p, - &booz_imu.gyro_unscaled.q, - &booz_imu.gyro_unscaled.r); + &imu.gyro_unscaled.p, + &imu.gyro_unscaled.q, + &imu.gyro_unscaled.r); DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel, - &booz_imu.accel_unscaled.x, - &booz_imu.accel_unscaled.y, - &booz_imu.accel_unscaled.z); + &imu.accel_unscaled.x, + &imu.accel_unscaled.y, + &imu.accel_unscaled.z); } else if (cnt == 7) { DOWNLINK_SEND_BOOZ2_GYRO(DefaultChannel, - &booz_imu.gyro.p, - &booz_imu.gyro.q, - &booz_imu.gyro.r); + &imu.gyro.p, + &imu.gyro.q, + &imu.gyro.r); DOWNLINK_SEND_BOOZ2_ACCEL(DefaultChannel, - &booz_imu.accel.x, - &booz_imu.accel.y, - &booz_imu.accel.z); + &imu.accel.x, + &imu.accel.y, + &imu.accel.z); } } static inline void on_mag_event(void) { - BoozImuScaleMag(booz_imu); + ImuScaleMag(imu); static uint8_t cnt; cnt++; if (cnt > 1) cnt = 0; if (cnt%2) { DOWNLINK_SEND_BOOZ2_MAG(DefaultChannel, - &booz_imu.mag.x, - &booz_imu.mag.y, - &booz_imu.mag.z); + &imu.mag.x, + &imu.mag.y, + &imu.mag.z); } else { DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, - &booz_imu.mag_unscaled.x, - &booz_imu.mag_unscaled.y, - &booz_imu.mag_unscaled.z); + &imu.mag_unscaled.x, + &imu.mag_unscaled.y, + &imu.mag_unscaled.z); } } diff --git a/sw/airborne/beth/overo_estimator.c b/sw/airborne/beth/overo_estimator.c index d248d00e3b..db19630d53 100644 --- a/sw/airborne/beth/overo_estimator.c +++ b/sw/airborne/beth/overo_estimator.c @@ -1,6 +1,6 @@ #include "overo_estimator.h" -#include "booz/booz_imu.h" +#include "imu.h" #include #include "messages2.h" @@ -37,10 +37,10 @@ void estimator_run(uint16_t tilt_measure, uint16_t elevation_measure, uint16_t a Bound(estimator.tilt,-89,89); //low pass filter tilt gyro estimator.tilt_dot = estimator.tilt_dot + - estimator.tilt_lp_coeff * (RATE_FLOAT_OF_BFP(booz_imu.gyro.q) - estimator.tilt_dot); + estimator.tilt_lp_coeff * (RATE_FLOAT_OF_BFP(imu.gyro.q) - estimator.tilt_dot); /* Second order filter yet to be tested estimator.tilt_dot = estimator.tilt_dot * (2 - estimator.tilt_lp_coeff1 - estimator.tilt_lp_coeff2) + - estimator.tilt_lp_coeff1 * estimator.tilt_lp_coeff2 * RATE_FLOAT_OF_BFP(booz_imu.gyro.q) - + estimator.tilt_lp_coeff1 * estimator.tilt_lp_coeff2 * RATE_FLOAT_OF_BFP(imu.gyro.q) - estimator.tilt_dot_old * (1 - estimator.tilt_lp_coeff1 - estimator.tilt_lp_coeff2 + estimator.tilt_lp_coeff1*estimator.tilt_lp_coeff2); */ @@ -49,8 +49,8 @@ void estimator_run(uint16_t tilt_measure, uint16_t elevation_measure, uint16_t a Bound(estimator.elevation,-45,45); //rotation compensation (mixing of two gyro values to generate a reading that reflects rate along beth axes - float rotated_elev_dot = RATE_FLOAT_OF_BFP(booz_imu.gyro.p) * cos(estimator.tilt) + - RATE_FLOAT_OF_BFP(booz_imu.gyro.r) * sin(estimator.tilt); + float rotated_elev_dot = RATE_FLOAT_OF_BFP(imu.gyro.p) * cos(estimator.tilt) + + RATE_FLOAT_OF_BFP(imu.gyro.r) * sin(estimator.tilt); //low pass filter -- should probably increase order and maybe move filtering to measured values. estimator.elevation_dot = estimator.elevation_dot + estimator.elevation_lp_coeff * (rotated_elev_dot - estimator.elevation_dot); @@ -60,7 +60,7 @@ void estimator_run(uint16_t tilt_measure, uint16_t elevation_measure, uint16_t a //low pass filter azimuth gyro //TODO: compensate rotation and increase order estimator.azimuth_dot = estimator.azimuth_dot + - estimator.azimuth_lp_coeff * (RATE_FLOAT_OF_BFP(booz_imu.gyro.r) - estimator.azimuth_dot); + estimator.azimuth_lp_coeff * (RATE_FLOAT_OF_BFP(imu.gyro.r) - estimator.azimuth_dot); } diff --git a/sw/airborne/beth/overo_file_logger.c b/sw/airborne/beth/overo_file_logger.c index d41c79b58d..a942edadd9 100644 --- a/sw/airborne/beth/overo_file_logger.c +++ b/sw/airborne/beth/overo_file_logger.c @@ -1,6 +1,6 @@ #include "overo_file_logger.h" -#include "booz/booz_imu.h" +#include "imu.h" struct FileLogger file_logger; @@ -15,8 +15,8 @@ void file_logger_init(char* filename) { void file_logger_periodic(void) { static uint32_t foo = 0; foo++; - fprintf(file_logger.outfile,"%f %d IMU_ACCEL_RAW %d %d %d\n",foo/512.,42,booz_imu.accel_unscaled.x,booz_imu.accel_unscaled.y,booz_imu.accel_unscaled.z); - fprintf(file_logger.outfile,"%f %d IMU_GYRO_RAW %d %d %d\n",foo/512.,42,booz_imu.gyro_unscaled.p,booz_imu.gyro_unscaled.q,booz_imu.gyro_unscaled.r); + fprintf(file_logger.outfile,"%f %d IMU_ACCEL_RAW %d %d %d\n",foo/512.,42,imu.accel_unscaled.x,imu.accel_unscaled.y,imu.accel_unscaled.z); + fprintf(file_logger.outfile,"%f %d IMU_GYRO_RAW %d %d %d\n",foo/512.,42,imu.gyro_unscaled.p,imu.gyro_unscaled.q,imu.gyro_unscaled.r); } diff --git a/sw/airborne/booz/ahrs/booz_ahrs_aligner.c b/sw/airborne/booz/ahrs/booz_ahrs_aligner.c index 4d9e57f8a2..bd629b80da 100644 --- a/sw/airborne/booz/ahrs/booz_ahrs_aligner.c +++ b/sw/airborne/booz/ahrs/booz_ahrs_aligner.c @@ -24,7 +24,7 @@ #include "booz_ahrs_aligner.h" #include /* for abs() */ -#include "booz_imu.h" +#include "imu.h" #include "led.h" struct BoozAhrsAligner booz_ahrs_aligner; @@ -52,11 +52,11 @@ void booz_ahrs_aligner_init(void) { void booz_ahrs_aligner_run(void) { - RATES_ADD(gyro_sum, booz_imu.gyro); - VECT3_ADD(accel_sum, booz_imu.accel); - VECT3_ADD(mag_sum, booz_imu.mag); + RATES_ADD(gyro_sum, imu.gyro); + VECT3_ADD(accel_sum, imu.accel); + VECT3_ADD(mag_sum, imu.mag); - ref_sensor_samples[samples_idx] = booz_imu.accel.z; + ref_sensor_samples[samples_idx] = imu.accel.z; samples_idx++; #ifdef AHRS_ALIGNER_LED diff --git a/sw/airborne/booz/ahrs/booz_ahrs_cmpl_euler.c b/sw/airborne/booz/ahrs/booz_ahrs_cmpl_euler.c index dbfab46dea..edf0408434 100644 --- a/sw/airborne/booz/ahrs/booz_ahrs_cmpl_euler.c +++ b/sw/airborne/booz/ahrs/booz_ahrs_cmpl_euler.c @@ -23,7 +23,7 @@ #include "booz_ahrs_cmpl_euler.h" -#include "booz_imu.h" +#include "imu.h" #include "booz_ahrs_aligner.h" #include "airframe.h" @@ -97,7 +97,7 @@ void booz_ahrs_propagate(void) { /* unbias gyro */ struct Int32Rates uf_rate; - RATES_DIFF(uf_rate, booz_imu.gyro, booz2_face_gyro_bias); + RATES_DIFF(uf_rate, imu.gyro, booz2_face_gyro_bias); /* low pass rate */ RATES_ADD(booz_ahrs.imu_rate, uf_rate); RATES_SDIV(booz_ahrs.imu_rate, booz_ahrs.imu_rate, 2); @@ -130,24 +130,24 @@ void booz_ahrs_propagate(void) { INT32_RMAT_OF_EULERS(booz_ahrs.ltp_to_imu_rmat, booz_ahrs.ltp_to_imu_euler); /* Compute LTP to BODY quaternion */ - INT32_QUAT_COMP_INV(booz_ahrs.ltp_to_body_quat, booz_ahrs.ltp_to_imu_quat, booz_imu.body_to_imu_quat); + INT32_QUAT_COMP_INV(booz_ahrs.ltp_to_body_quat, booz_ahrs.ltp_to_imu_quat, imu.body_to_imu_quat); /* Compute LTP to BODY rotation matrix */ - INT32_RMAT_COMP_INV(booz_ahrs.ltp_to_body_rmat, booz_ahrs.ltp_to_imu_rmat, booz_imu.body_to_imu_rmat); + INT32_RMAT_COMP_INV(booz_ahrs.ltp_to_body_rmat, booz_ahrs.ltp_to_imu_rmat, imu.body_to_imu_rmat); /* compute LTP to BODY eulers */ INT32_EULERS_OF_RMAT(booz_ahrs.ltp_to_body_euler, booz_ahrs.ltp_to_body_rmat); /* compute body rates */ - INT32_RMAT_TRANSP_RATEMULT(booz_ahrs.body_rate, booz_imu.body_to_imu_rmat, booz_ahrs.imu_rate); + INT32_RMAT_TRANSP_RATEMULT(booz_ahrs.body_rate, imu.body_to_imu_rmat, booz_ahrs.imu_rate); } void booz_ahrs_update_accel(void) { /* build a measurement assuming constant acceleration ?!! */ - INT32_ATAN2(measurement.phi, -booz_imu.accel.y, -booz_imu.accel.z); + INT32_ATAN2(measurement.phi, -imu.accel.y, -imu.accel.z); int32_t cphi; PPRZ_ITRIG_COS(cphi, measurement.phi); - int32_t cphi_ax = -INT_MULT_RSHIFT(cphi, booz_imu.accel.x, INT32_TRIG_FRAC); - INT32_ATAN2(measurement.theta, -cphi_ax, -booz_imu.accel.z); + int32_t cphi_ax = -INT_MULT_RSHIFT(cphi, imu.accel.x, INT32_TRIG_FRAC); + INT32_ATAN2(measurement.theta, -cphi_ax, -imu.accel.z); measurement.phi *= F_UPDATE; measurement.theta *= F_UPDATE; @@ -171,17 +171,17 @@ void booz_ahrs_update_mag(void) { //int32_t cphi_ctheta = (cphi*ctheta)>>INT32_TRIG_FRAC; const int32_t mn = - ctheta * booz_imu.mag.x + - sphi_stheta * booz_imu.mag.y + - cphi_stheta * booz_imu.mag.z; + ctheta * imu.mag.x + + sphi_stheta * imu.mag.y + + cphi_stheta * imu.mag.z; const int32_t me = - 0 * booz_imu.mag.x + - cphi * booz_imu.mag.y + - -sphi * booz_imu.mag.z; + 0 * imu.mag.x + + cphi * imu.mag.y + + -sphi * imu.mag.z; //const int32_t md = - // -stheta * booz_imu.mag.x + - // sphi_ctheta * booz_imu.mag.y + - // cphi_ctheta * booz_imu.mag.z; + // -stheta * imu.mag.x + + // sphi_ctheta * imu.mag.y + + // cphi_ctheta * imu.mag.z; float m_psi = -atan2(me, mn); measurement.psi = ((m_psi - RadOfDeg(booz_ahrs_mag_offset))*(float)(1<<(INT32_ANGLE_FRAC))*F_UPDATE); diff --git a/sw/airborne/booz/ahrs/booz_ahrs_float_ekf.c b/sw/airborne/booz/ahrs/booz_ahrs_float_ekf.c index 2496a9bd43..ddec19b360 100644 --- a/sw/airborne/booz/ahrs/booz_ahrs_float_ekf.c +++ b/sw/airborne/booz/ahrs/booz_ahrs_float_ekf.c @@ -121,7 +121,7 @@ extern void booz_ahrs_align(void); void booz_ahrs_propagate(void) { /* compute unbiased rates */ - RATES_FLOAT_OF_BFP(bafe_rates, booz_imu.gyro); + RATES_FLOAT_OF_BFP(bafe_rates, imu.gyro); RATES_SUB(bafe_rates, bafe_bias); /* compute F diff --git a/sw/airborne/booz/ahrs/booz_ahrs_float_lkf.c b/sw/airborne/booz/ahrs/booz_ahrs_float_lkf.c index 66c269145b..e36aacd68a 100644 --- a/sw/airborne/booz/ahrs/booz_ahrs_float_lkf.c +++ b/sw/airborne/booz/ahrs/booz_ahrs_float_lkf.c @@ -24,7 +24,7 @@ #include "booz_ahrs_float_lkf.h" -#include "booz_imu.h" +#include "imu.h" #include "booz_ahrs_aligner.h" #include "airframe.h" @@ -200,13 +200,13 @@ float bafl_R_mag; #define AHRS_LTP_TO_BODY() { \ /* Compute LTP to BODY quaternion */ \ - INT32_QUAT_COMP_INV(booz_ahrs.ltp_to_body_quat, booz_ahrs.ltp_to_imu_quat, booz_imu.body_to_imu_quat); \ + INT32_QUAT_COMP_INV(booz_ahrs.ltp_to_body_quat, booz_ahrs.ltp_to_imu_quat, imu.body_to_imu_quat); \ /* Compute LTP to BODY rotation matrix */ \ - INT32_RMAT_COMP_INV(booz_ahrs.ltp_to_body_rmat, booz_ahrs.ltp_to_imu_rmat, booz_imu.body_to_imu_rmat); \ + INT32_RMAT_COMP_INV(booz_ahrs.ltp_to_body_rmat, booz_ahrs.ltp_to_imu_rmat, imu.body_to_imu_rmat); \ /* compute LTP to BODY eulers */ \ INT32_EULERS_OF_RMAT(booz_ahrs.ltp_to_body_euler, booz_ahrs.ltp_to_body_rmat); \ /* compute body rates */ \ - INT32_RMAT_TRANSP_RATEMULT(booz_ahrs.body_rate, booz_imu.body_to_imu_rmat, booz_ahrs.imu_rate); \ + INT32_RMAT_TRANSP_RATEMULT(booz_ahrs.body_rate, imu.body_to_imu_rmat, booz_ahrs.imu_rate); \ } @@ -256,7 +256,7 @@ void booz_ahrs_align(void) { static inline void booz_ahrs_lowpass_accel(void) { // get latest measurement - ACCELS_FLOAT_OF_BFP(bafl_accel_last, booz_imu.accel); + ACCELS_FLOAT_OF_BFP(bafl_accel_last, imu.accel); // low pass it VECT3_ADD(bafl_accel_measure, bafl_accel_last); VECT3_SDIV(bafl_accel_measure, bafl_accel_measure, 2); @@ -290,7 +290,7 @@ void booz_ahrs_propagate(void) { booz_ahrs_lowpass_accel(); /* compute unbiased rates */ - RATES_FLOAT_OF_BFP(bafl_rates, booz_imu.gyro); + RATES_FLOAT_OF_BFP(bafl_rates, imu.gyro); RATES_SUB(bafl_rates, bafl_bias); @@ -647,7 +647,7 @@ static void booz_ahrs_do_update_mag(void) { printf("Mag update.\n"); #endif - MAGS_FLOAT_OF_BFP(bafl_mag, booz_imu.mag); + MAGS_FLOAT_OF_BFP(bafl_mag, imu.mag); /* P_prio = P */ for (i = 0; i < BAFL_SSIZE; i++) { diff --git a/sw/airborne/booz/booz2_ins.c b/sw/airborne/booz/booz2_ins.c index 5e687bbe32..66d6e4daae 100644 --- a/sw/airborne/booz/booz2_ins.c +++ b/sw/airborne/booz/booz2_ins.c @@ -24,7 +24,7 @@ #include "booz2_ins.h" -#include "booz_imu.h" +#include "imu.h" #include "firmwares/rotorcraft/baro.h" #include "booz_gps.h" @@ -149,7 +149,7 @@ void booz_ins_realign_v(float z) { void booz_ins_propagate() { /* untilt accels */ struct Int32Vect3 accel_body; - INT32_RMAT_TRANSP_VMULT(accel_body, booz_imu.body_to_imu_rmat, booz_imu.accel); + INT32_RMAT_TRANSP_VMULT(accel_body, imu.body_to_imu_rmat, imu.accel); struct Int32Vect3 accel_ltp; INT32_RMAT_TRANSP_VMULT(accel_ltp, booz_ahrs.ltp_to_body_rmat, accel_body); float z_accel_float = ACCEL_FLOAT_OF_BFP(accel_ltp.z); diff --git a/sw/airborne/booz/booz2_telemetry.h b/sw/airborne/booz/booz2_telemetry.h index 2f637a2fa4..0d7ddb3ff3 100644 --- a/sw/airborne/booz/booz2_telemetry.h +++ b/sw/airborne/booz/booz2_telemetry.h @@ -46,7 +46,7 @@ #define PERIODIC_SEND_ALIVE(_chan) DOWNLINK_SEND_ALIVE(_chan, 16, MD5SUM) #include "booz2_battery.h" -#include "booz_imu.h" +#include "imu.h" #include "booz_gps.h" #include "booz2_ins.h" #include "booz_ahrs.h" @@ -57,10 +57,10 @@ extern uint8_t telemetry_mode_Main_DefaultChannel; #ifdef USE_GPS #define PERIODIC_SEND_BOOZ_STATUS(_chan) { \ - uint32_t booz_imu_nb_err = 0; \ + uint32_t imu_nb_err = 0; \ uint8_t _twi_blmc_nb_err = 0; \ DOWNLINK_SEND_BOOZ_STATUS(_chan, \ - &booz_imu_nb_err, \ + &imu_nb_err, \ &_twi_blmc_nb_err, \ &radio_control.status, \ &booz_gps_state.fix, \ @@ -75,11 +75,11 @@ extern uint8_t telemetry_mode_Main_DefaultChannel; } #else /* !USE_GPS */ #define PERIODIC_SEND_BOOZ_STATUS(_chan) { \ - uint32_t booz_imu_nb_err = 0; \ + uint32_t imu_nb_err = 0; \ uint8_t twi_blmc_nb_err = 0; \ uint8_t fix = BOOZ2_GPS_FIX_NONE; \ DOWNLINK_SEND_BOOZ_STATUS(_chan, \ - &booz_imu_nb_err, \ + &imu_nb_err, \ &twi_blmc_nb_err, \ &radio_control.status, \ &fix, \ @@ -130,44 +130,44 @@ extern uint8_t telemetry_mode_Main_DefaultChannel; #define PERIODIC_SEND_BOOZ2_GYRO(_chan) { \ DOWNLINK_SEND_BOOZ2_GYRO(_chan, \ - &booz_imu.gyro.p, \ - &booz_imu.gyro.q, \ - &booz_imu.gyro.r); \ + &imu.gyro.p, \ + &imu.gyro.q, \ + &imu.gyro.r); \ } #define PERIODIC_SEND_BOOZ2_ACCEL(_chan) { \ DOWNLINK_SEND_BOOZ2_ACCEL(_chan, \ - &booz_imu.accel.x, \ - &booz_imu.accel.y, \ - &booz_imu.accel.z); \ + &imu.accel.x, \ + &imu.accel.y, \ + &imu.accel.z); \ } #define PERIODIC_SEND_BOOZ2_MAG(_chan) { \ DOWNLINK_SEND_BOOZ2_MAG(_chan, \ - &booz_imu.mag.x, \ - &booz_imu.mag.y, \ - &booz_imu.mag.z); \ + &imu.mag.x, \ + &imu.mag.y, \ + &imu.mag.z); \ } #define PERIODIC_SEND_IMU_GYRO_RAW(_chan) { \ DOWNLINK_SEND_IMU_GYRO_RAW(_chan, \ - &booz_imu.gyro_unscaled.p, \ - &booz_imu.gyro_unscaled.q, \ - &booz_imu.gyro_unscaled.r); \ + &imu.gyro_unscaled.p, \ + &imu.gyro_unscaled.q, \ + &imu.gyro_unscaled.r); \ } #define PERIODIC_SEND_IMU_ACCEL_RAW(_chan) { \ DOWNLINK_SEND_IMU_ACCEL_RAW(_chan, \ - &booz_imu.accel_unscaled.x, \ - &booz_imu.accel_unscaled.y, \ - &booz_imu.accel_unscaled.z); \ + &imu.accel_unscaled.x, \ + &imu.accel_unscaled.y, \ + &imu.accel_unscaled.z); \ } #define PERIODIC_SEND_IMU_MAG_RAW(_chan) { \ DOWNLINK_SEND_IMU_MAG_RAW(_chan, \ - &booz_imu.mag_unscaled.x, \ - &booz_imu.mag_unscaled.y, \ - &booz_imu.mag_unscaled.z); \ + &imu.mag_unscaled.x, \ + &imu.mag_unscaled.y, \ + &imu.mag_unscaled.z); \ } /* FIXME: make that depend on board */ @@ -307,9 +307,9 @@ extern uint8_t telemetry_mode_Main_DefaultChannel; &booz_ahrs_aligner.lp_gyro.p, \ &booz_ahrs_aligner.lp_gyro.q, \ &booz_ahrs_aligner.lp_gyro.r, \ - &booz_imu.gyro.p, \ - &booz_imu.gyro.q, \ - &booz_imu.gyro.r, \ + &imu.gyro.p, \ + &imu.gyro.q, \ + &imu.gyro.r, \ &booz_ahrs_aligner.noise, \ &booz_ahrs_aligner.low_noise_cnt); \ } diff --git a/sw/airborne/booz/booz_ahrs.c b/sw/airborne/booz/booz_ahrs.c index 00445295c4..15b228abb0 100644 --- a/sw/airborne/booz/booz_ahrs.c +++ b/sw/airborne/booz/booz_ahrs.c @@ -22,7 +22,7 @@ */ #include "booz_ahrs.h" -#include "booz_imu.h" +#include "imu.h" struct BoozAhrs booz_ahrs; struct BoozAhrsFloat booz_ahrs_float; diff --git a/sw/airborne/booz/booz_fms.c b/sw/airborne/booz/booz_fms.c index 2a994d4853..8d90d79d5f 100644 --- a/sw/airborne/booz/booz_fms.c +++ b/sw/airborne/booz/booz_fms.c @@ -23,7 +23,7 @@ #include "booz_fms.h" -#include "booz_imu.h" +#include "imu.h" #include "booz_gps.h" #include "booz_ahrs.h" @@ -71,9 +71,9 @@ void booz_fms_set_enabled(bool_t enabled) { void booz_fms_update_info(void) { - // PPRZ_INT16_OF_INT32_RATES(booz_fms_info.imu.gyro, booz_imu.gyro); - // PPRZ_INT16_OF_INT32_VECT3(booz_fms_info.imu.accel, booz_imu.accel); - //PPRZ_INT16_OF_INT32_VECT3(booz_fms_info.imu.mag, booz_imu.mag); + // PPRZ_INT16_OF_INT32_RATES(booz_fms_info.imu.gyro, imu.gyro); + // PPRZ_INT16_OF_INT32_VECT3(booz_fms_info.imu.accel, imu.accel); + //PPRZ_INT16_OF_INT32_VECT3(booz_fms_info.imu.mag, imu.mag); //PPRZ_INT32_VECT3_COPY(booz_fms_info.gps.pos, booz_gps_state.ecef_pos); //PPRZ_INT16_OF_INT32_VECT3(booz_fms_info.gps.speed, booz_gps_state.ecef_speed); diff --git a/sw/airborne/booz/ins/booz2_hf_float-old.c b/sw/airborne/booz/ins/booz2_hf_float-old.c index 1714cbe1ce..10855052a9 100644 --- a/sw/airborne/booz/ins/booz2_hf_float-old.c +++ b/sw/airborne/booz/ins/booz2_hf_float-old.c @@ -24,7 +24,7 @@ #include "booz2_hf_float.h" #include "booz2_ins.h" -#include "booz_imu.h" +#include "imu.h" #include "booz_ahrs.h" #include "math/pprz_algebra_int.h" @@ -54,7 +54,7 @@ void b2ins_propagate(void) { VECT3_SDIV(scaled_biases, b2ins_accel_bias, (1<<(B2INS_ACCEL_BIAS_FRAC-B2INS_ACCEL_LTP_FRAC))); struct Int32Vect3 accel_imu; /* unbias accelerometers */ - VECT3_DIFF(accel_imu, booz_imu.accel, scaled_biases); + VECT3_DIFF(accel_imu, imu.accel, scaled_biases); /* convert to LTP */ // BOOZ_IQUAT_VDIV(b2ins_accel_ltp, booz_ahrs.ltp_to_imu_quat, accel_imu); INT32_RMAT_TRANSP_VMULT(b2ins_accel_ltp, booz_ahrs.ltp_to_imu_rmat, accel_imu); diff --git a/sw/airborne/booz/ins/booz2_hf_float.c b/sw/airborne/booz/ins/booz2_hf_float.c index 7ef8780521..7506e3d2d2 100644 --- a/sw/airborne/booz/ins/booz2_hf_float.c +++ b/sw/airborne/booz/ins/booz2_hf_float.c @@ -24,7 +24,7 @@ #include "booz2_hf_float.h" #include "booz2_ins.h" -#include "booz_imu.h" +#include "imu.h" #include "booz_ahrs.h" #include "booz_gps.h" #include @@ -112,7 +112,7 @@ struct AccBuf acc_body; struct Int32Vect3 acc_body_mean; void b2_hff_store_accel_body(void) { - INT32_RMAT_TRANSP_VMULT(acc_body.buf[acc_body.w], booz_imu.body_to_imu_rmat, booz_imu.accel); + INT32_RMAT_TRANSP_VMULT(acc_body.buf[acc_body.w], imu.body_to_imu_rmat, imu.accel); acc_body.w = (acc_body.w + 1) < acc_body.size ? (acc_body.w + 1) : 0; /* once the buffer is full it always has the last acc_body.size accel measurements */ diff --git a/sw/airborne/booz/stabilization/booz_stabilization_rate.c b/sw/airborne/booz/stabilization/booz_stabilization_rate.c index efa08952aa..b0592d99d8 100644 --- a/sw/airborne/booz/stabilization/booz_stabilization_rate.c +++ b/sw/airborne/booz/stabilization/booz_stabilization_rate.c @@ -26,7 +26,7 @@ #include "booz_ahrs.h" -#include "booz_imu.h" +#include "imu.h" #include "booz_radio_control.h" #include "airframe.h" diff --git a/sw/airborne/booz/test/booz_test_imu.c b/sw/airborne/booz/test/booz_test_imu.c index 0585c26fa7..6121178693 100644 --- a/sw/airborne/booz/test/booz_test_imu.c +++ b/sw/airborne/booz/test/booz_test_imu.c @@ -31,7 +31,7 @@ #include "messages.h" #include "downlink.h" -#include "booz_imu.h" +#include "imu.h" #include "interrupt_hw.h" @@ -58,7 +58,7 @@ static inline void main_init( void ) { hw_init(); sys_time_init(); - booz_imu_init(); + imu_init(); DEBUG_SERVO1_INIT(); DEBUG_SERVO2_INIT(); @@ -86,19 +86,19 @@ static inline void main_periodic_task( void ) { &i2c2_errors.last_unexpected_event); }); #endif - if (cpu_time_sec > 1) booz_imu_periodic(); + if (cpu_time_sec > 1) imu_periodic(); RunOnceEvery(10, { LED_PERIODIC();}); } static inline void main_event_task( void ) { - BoozImuEvent(on_gyro_accel_event, on_mag_event); + ImuEvent(on_gyro_accel_event, on_mag_event); } static inline void on_gyro_accel_event(void) { - BoozImuScaleGyro(booz_imu); - BoozImuScaleAccel(booz_imu); + ImuScaleGyro(imu); + ImuScaleAccel(imu); LED_TOGGLE(2); static uint8_t cnt; @@ -107,45 +107,45 @@ static inline void on_gyro_accel_event(void) { if (cnt == 0) { DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel, - &booz_imu.gyro_unscaled.p, - &booz_imu.gyro_unscaled.q, - &booz_imu.gyro_unscaled.r); + &imu.gyro_unscaled.p, + &imu.gyro_unscaled.q, + &imu.gyro_unscaled.r); DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel, - &booz_imu.accel_unscaled.x, - &booz_imu.accel_unscaled.y, - &booz_imu.accel_unscaled.z); + &imu.accel_unscaled.x, + &imu.accel_unscaled.y, + &imu.accel_unscaled.z); } else if (cnt == 7) { DOWNLINK_SEND_BOOZ2_GYRO(DefaultChannel, - &booz_imu.gyro.p, - &booz_imu.gyro.q, - &booz_imu.gyro.r); + &imu.gyro.p, + &imu.gyro.q, + &imu.gyro.r); DOWNLINK_SEND_BOOZ2_ACCEL(DefaultChannel, - &booz_imu.accel.x, - &booz_imu.accel.y, - &booz_imu.accel.z); + &imu.accel.x, + &imu.accel.y, + &imu.accel.z); } } static inline void on_mag_event(void) { - BoozImuScaleMag(booz_imu); + ImuScaleMag(imu); static uint8_t cnt; cnt++; if (cnt > 1) cnt = 0; if (cnt%2) { DOWNLINK_SEND_BOOZ2_MAG(DefaultChannel, - &booz_imu.mag.x, - &booz_imu.mag.y, - &booz_imu.mag.z); + &imu.mag.x, + &imu.mag.y, + &imu.mag.z); } else { DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, - &booz_imu.mag_unscaled.x, - &booz_imu.mag_unscaled.y, - &booz_imu.mag_unscaled.z); + &imu.mag_unscaled.x, + &imu.mag_unscaled.y, + &imu.mag_unscaled.z); } } diff --git a/sw/airborne/booz/test/imu_dummy.c b/sw/airborne/booz/test/imu_dummy.c index 2b7acc1dc1..563a922335 100644 --- a/sw/airborne/booz/test/imu_dummy.c +++ b/sw/airborne/booz/test/imu_dummy.c @@ -1,3 +1,3 @@ -#include "booz_imu.h" +#include "imu.h" -void booz_imu_impl_init(void) {} +void imu_impl_init(void) {} diff --git a/sw/airborne/booz/test/test_mlkf.c b/sw/airborne/booz/test/test_mlkf.c index 8571da662a..c12f945c15 100644 --- a/sw/airborne/booz/test/test_mlkf.c +++ b/sw/airborne/booz/test/test_mlkf.c @@ -2,7 +2,7 @@ #include #include "math/pprz_algebra_double.h" -#include "booz_imu.h" +#include "imu.h" #include "booz_ahrs.h" #include "ahrs/booz_ahrs_mlkf.h" @@ -39,7 +39,7 @@ int main(int argc, char** argv) { read_data(IN_FILE); - booz_imu_init(); + imu_init(); booz_ahrs_init(); for (int i=0; i0) { - RATES_COPY(booz_imu.gyro_prev, booz_imu.gyro); + RATES_COPY(imu.gyro_prev, imu.gyro); } else { - RATES_BFP_OF_REAL(booz_imu.gyro_prev, samples[0].gyro); + RATES_BFP_OF_REAL(imu.gyro_prev, samples[0].gyro); } - RATES_BFP_OF_REAL(booz_imu.gyro, samples[i].gyro); - ACCELS_BFP_OF_REAL(booz_imu.accel, samples[i].accel); - MAGS_BFP_OF_REAL(booz_imu.mag, samples[i].mag); + RATES_BFP_OF_REAL(imu.gyro, samples[i].gyro); + ACCELS_BFP_OF_REAL(imu.accel, samples[i].accel); + MAGS_BFP_OF_REAL(imu.mag, samples[i].mag); } diff --git a/sw/airborne/csc/mercury_main.c b/sw/airborne/csc/mercury_main.c index a586a6df91..b8e62a43d5 100644 --- a/sw/airborne/csc/mercury_main.c +++ b/sw/airborne/csc/mercury_main.c @@ -40,7 +40,7 @@ #include "csc_msg_def.h" #include ACTUATORS -#include "booz/booz_imu.h" +#include "imu.h" #include "booz/ahrs/booz_ahrs_aligner.h" #include "booz/booz_ahrs.h" #include "mercury_xsens.h" @@ -112,7 +112,7 @@ static inline void csc_main_init( void ) { Uart0Init(); Uart1Init(); - booz_imu_init(); + imu_init(); booz_ahrs_aligner_init(); booz_ahrs_init(); diff --git a/sw/airborne/csc/mercury_xsens.c b/sw/airborne/csc/mercury_xsens.c index 0874608719..48dafeb207 100644 --- a/sw/airborne/csc/mercury_xsens.c +++ b/sw/airborne/csc/mercury_xsens.c @@ -27,10 +27,10 @@ */ #include "mercury_xsens.h" -#include "booz/booz_imu.h" +#include "imu.h" #include "booz/booz_ahrs.h" #include "booz/ahrs/booz_ahrs_aligner.h" -#include "booz/booz_imu.h" +#include "imu.h" #include "csc_booz2_ins.h" #include @@ -202,7 +202,7 @@ static uint8_t xsens_msg_idx[XSENS_COUNT]; static uint8_t ck[XSENS_COUNT]; static uint8_t send_ck[XSENS_COUNT]; -void booz_imu_impl_init( void ) +void imu_impl_init( void ) { } @@ -220,17 +220,17 @@ void xsens_init( void ) xsens_msg_buf_ci[i] = 0; FLOAT_RMAT_ZERO(xsens_rmat_neutral[i]); } - booz_imu.accel_neutral.x = IMU_ACCEL_X_NEUTRAL; - booz_imu.accel_neutral.y = IMU_ACCEL_Y_NEUTRAL; - booz_imu.accel_neutral.z = IMU_ACCEL_Z_NEUTRAL; + imu.accel_neutral.x = IMU_ACCEL_X_NEUTRAL; + imu.accel_neutral.y = IMU_ACCEL_Y_NEUTRAL; + imu.accel_neutral.z = IMU_ACCEL_Z_NEUTRAL; - booz_imu.gyro_neutral.p = IMU_GYRO_P_NEUTRAL; - booz_imu.gyro_neutral.q = IMU_GYRO_Q_NEUTRAL; - booz_imu.gyro_neutral.r = IMU_GYRO_R_NEUTRAL; + imu.gyro_neutral.p = IMU_GYRO_P_NEUTRAL; + imu.gyro_neutral.q = IMU_GYRO_Q_NEUTRAL; + imu.gyro_neutral.r = IMU_GYRO_R_NEUTRAL; - booz_imu.mag_neutral.x = IMU_MAG_X_NEUTRAL; - booz_imu.mag_neutral.y = IMU_MAG_Y_NEUTRAL; - booz_imu.mag_neutral.z = IMU_MAG_Z_NEUTRAL; + imu.mag_neutral.x = IMU_MAG_X_NEUTRAL; + imu.mag_neutral.y = IMU_MAG_Y_NEUTRAL; + imu.mag_neutral.z = IMU_MAG_Z_NEUTRAL; // Also TODO: set scenario to aerospace // set magnetic declination angle // Probably quicker to just set everything once via MT Manager software @@ -301,18 +301,18 @@ void xsens_parse_msg( uint8_t xsens_id ) { uint8_t offset = 0; // test RAW modes else calibrated modes if (XSENS_MASK_RAWInertial(xsens_output_mode[xsens_id])){// || (XSENS_MASK_RAWGPS(xsens2_output_mode))) { - booz_imu.accel_unscaled.x = XSENS_DATA_RAWInertial_accZ(xsens_msg_buf[xsens_id][buf_slot],offset); - booz_imu.accel_unscaled.y = XSENS_DATA_RAWInertial_accY(xsens_msg_buf[xsens_id][buf_slot],offset); - booz_imu.accel_unscaled.z = XSENS_DATA_RAWInertial_accX(xsens_msg_buf[xsens_id][buf_slot],offset); - booz_imu.gyro_unscaled.p = XSENS_DATA_RAWInertial_gyrZ(xsens_msg_buf[xsens_id][buf_slot],offset); - booz_imu.gyro_unscaled.q = XSENS_DATA_RAWInertial_gyrY(xsens_msg_buf[xsens_id][buf_slot],offset); - booz_imu.gyro_unscaled.r = XSENS_DATA_RAWInertial_gyrX(xsens_msg_buf[xsens_id][buf_slot],offset); - booz_imu.mag_unscaled.x = XSENS_DATA_RAWInertial_magZ(xsens_msg_buf[xsens_id][buf_slot],offset); - booz_imu.mag_unscaled.y = XSENS_DATA_RAWInertial_magY(xsens_msg_buf[xsens_id][buf_slot],offset); - booz_imu.mag_unscaled.z = XSENS_DATA_RAWInertial_magX(xsens_msg_buf[xsens_id][buf_slot],offset); - BoozImuScaleGyro(booz_imu); - BoozImuScaleAccel(booz_imu); - BoozImuScaleMag(booz_imu); + imu.accel_unscaled.x = XSENS_DATA_RAWInertial_accZ(xsens_msg_buf[xsens_id][buf_slot],offset); + imu.accel_unscaled.y = XSENS_DATA_RAWInertial_accY(xsens_msg_buf[xsens_id][buf_slot],offset); + imu.accel_unscaled.z = XSENS_DATA_RAWInertial_accX(xsens_msg_buf[xsens_id][buf_slot],offset); + imu.gyro_unscaled.p = XSENS_DATA_RAWInertial_gyrZ(xsens_msg_buf[xsens_id][buf_slot],offset); + imu.gyro_unscaled.q = XSENS_DATA_RAWInertial_gyrY(xsens_msg_buf[xsens_id][buf_slot],offset); + imu.gyro_unscaled.r = XSENS_DATA_RAWInertial_gyrX(xsens_msg_buf[xsens_id][buf_slot],offset); + imu.mag_unscaled.x = XSENS_DATA_RAWInertial_magZ(xsens_msg_buf[xsens_id][buf_slot],offset); + imu.mag_unscaled.y = XSENS_DATA_RAWInertial_magY(xsens_msg_buf[xsens_id][buf_slot],offset); + imu.mag_unscaled.z = XSENS_DATA_RAWInertial_magX(xsens_msg_buf[xsens_id][buf_slot],offset); + ImuScaleGyro(imu); + ImuScaleAccel(imu); + ImuScaleMag(imu); // Copied from booz2_main -- 5143134f060fcc57ce657e17d8b7fc2e72119fd7 // mmt 6/15/09 diff --git a/sw/airborne/csc/mercury_xsens.h b/sw/airborne/csc/mercury_xsens.h index 5ab1e8326c..d5e501ce4a 100644 --- a/sw/airborne/csc/mercury_xsens.h +++ b/sw/airborne/csc/mercury_xsens.h @@ -65,21 +65,21 @@ extern int xsens_setzero; #include "booz_ahrs.h" #define PERIODIC_SEND_BOOZ2_GYRO() { \ - DOWNLINK_SEND_BOOZ2_GYRO(&booz_imu.gyro.p, \ - &booz_imu.gyro.q, \ - &booz_imu.gyro.r); \ + DOWNLINK_SEND_BOOZ2_GYRO(&imu.gyro.p, \ + &imu.gyro.q, \ + &imu.gyro.r); \ } #define PERIODIC_SEND_BOOZ2_ACCEL() { \ - DOWNLINK_SEND_BOOZ2_ACCEL(&booz_imu.accel.x, \ - &booz_imu.accel.y, \ - &booz_imu.accel.z); \ + DOWNLINK_SEND_BOOZ2_ACCEL(&imu.accel.x, \ + &imu.accel.y, \ + &imu.accel.z); \ } #define PERIODIC_SEND_BOOZ2_MAG() { \ - DOWNLINK_SEND_BOOZ2_MAG(&booz_imu.mag.x, \ - &booz_imu.mag.y, \ - &booz_imu.mag.z); \ + DOWNLINK_SEND_BOOZ2_MAG(&imu.mag.x, \ + &imu.mag.y, \ + &imu.mag.z); \ } #define PERIODIC_SEND_BOOZ2_AHRS_EULER() { \ diff --git a/sw/airborne/firmwares/rotorcraft/imu.c b/sw/airborne/firmwares/rotorcraft/imu.c index ffdeec90c4..db3a8ed312 100644 --- a/sw/airborne/firmwares/rotorcraft/imu.c +++ b/sw/airborne/firmwares/rotorcraft/imu.c @@ -21,18 +21,18 @@ * Boston, MA 02111-1307, USA. */ -#include "booz_imu.h" +#include "imu.h" #include "airframe.h" -struct BoozImu booz_imu; +struct Imu imu; -void booz_imu_init(void) { +void imu_init(void) { /* initialises neutrals */ - RATES_ASSIGN(booz_imu.gyro_neutral, IMU_GYRO_P_NEUTRAL, IMU_GYRO_Q_NEUTRAL, IMU_GYRO_R_NEUTRAL); - VECT3_ASSIGN(booz_imu.accel_neutral, IMU_ACCEL_X_NEUTRAL, IMU_ACCEL_Y_NEUTRAL, IMU_ACCEL_Z_NEUTRAL); - VECT3_ASSIGN(booz_imu.mag_neutral, IMU_MAG_X_NEUTRAL, IMU_MAG_Y_NEUTRAL, IMU_MAG_Z_NEUTRAL); + RATES_ASSIGN(imu.gyro_neutral, IMU_GYRO_P_NEUTRAL, IMU_GYRO_Q_NEUTRAL, IMU_GYRO_R_NEUTRAL); + VECT3_ASSIGN(imu.accel_neutral, IMU_ACCEL_X_NEUTRAL, IMU_ACCEL_Y_NEUTRAL, IMU_ACCEL_Z_NEUTRAL); + VECT3_ASSIGN(imu.mag_neutral, IMU_MAG_X_NEUTRAL, IMU_MAG_Y_NEUTRAL, IMU_MAG_Z_NEUTRAL); /* Compute quaternion and rotation matrix @@ -43,10 +43,10 @@ void booz_imu_init(void) { { ANGLE_BFP_OF_REAL(IMU_BODY_TO_IMU_PHI), ANGLE_BFP_OF_REAL(IMU_BODY_TO_IMU_THETA), ANGLE_BFP_OF_REAL(IMU_BODY_TO_IMU_PSI) }; - INT32_QUAT_OF_EULERS(booz_imu.body_to_imu_quat, body_to_imu_eulers); - INT32_QUAT_NORMALISE(booz_imu.body_to_imu_quat); - INT32_RMAT_OF_EULERS(booz_imu.body_to_imu_rmat, body_to_imu_eulers); + INT32_QUAT_OF_EULERS(imu.body_to_imu_quat, body_to_imu_eulers); + INT32_QUAT_NORMALISE(imu.body_to_imu_quat); + INT32_RMAT_OF_EULERS(imu.body_to_imu_rmat, body_to_imu_eulers); #endif - booz_imu_impl_init(); + imu_impl_init(); } diff --git a/sw/airborne/firmwares/rotorcraft/imu.h b/sw/airborne/firmwares/rotorcraft/imu.h index 38ac9232e5..b029ebbcff 100644 --- a/sw/airborne/firmwares/rotorcraft/imu.h +++ b/sw/airborne/firmwares/rotorcraft/imu.h @@ -28,10 +28,10 @@ #include "math/pprz_algebra_float.h" /* must be defined by underlying hardware */ -extern void booz_imu_impl_init(void); -extern void booz_imu_periodic(void); +extern void imu_impl_init(void); +extern void imu_periodic(void); -struct BoozImu { +struct Imu { struct Int32Rates gyro; struct Int32Vect3 accel; struct Int32Vect3 mag; @@ -48,7 +48,7 @@ struct BoozImu { }; /* abstract IMU interface providing floating point interface */ -struct BoozImuFloat { +struct ImuFloat { struct FloatRates gyro; struct FloatVect3 accel; struct FloatVect3 mag; @@ -64,11 +64,11 @@ struct BoozImuFloat { #include BOOZ_IMU_TYPE_H #endif -extern struct BoozImu booz_imu; +extern struct Imu imu; -extern void booz_imu_init(void); +extern void imu_init(void); -#define BoozImuScaleGyro(_imu) { \ +#define ImuScaleGyro(_imu) { \ RATES_COPY(_imu.gyro_prev, _imu.gyro); \ _imu.gyro.p = ((_imu.gyro_unscaled.p - _imu.gyro_neutral.p)*IMU_GYRO_P_SIGN*IMU_GYRO_P_SENS_NUM)/IMU_GYRO_P_SENS_DEN; \ _imu.gyro.q = ((_imu.gyro_unscaled.q - _imu.gyro_neutral.q)*IMU_GYRO_Q_SIGN*IMU_GYRO_Q_SENS_NUM)/IMU_GYRO_Q_SENS_DEN; \ @@ -76,7 +76,7 @@ extern void booz_imu_init(void); } -#define BoozImuScaleAccel(_imu) { \ +#define ImuScaleAccel(_imu) { \ VECT3_COPY(_imu.accel_prev, _imu.accel); \ _imu.accel.x = ((_imu.accel_unscaled.x - _imu.accel_neutral.x)*IMU_ACCEL_X_SIGN*IMU_ACCEL_X_SENS_NUM)/IMU_ACCEL_X_SENS_DEN; \ _imu.accel.y = ((_imu.accel_unscaled.y - _imu.accel_neutral.y)*IMU_ACCEL_Y_SIGN*IMU_ACCEL_Y_SENS_NUM)/IMU_ACCEL_Y_SENS_DEN; \ @@ -84,7 +84,7 @@ extern void booz_imu_init(void); } #if defined IMU_MAG_45_HACK -#define BoozImuScaleMag(_imu) { \ +#define ImuScaleMag(_imu) { \ int32_t msx = ((_imu.mag_unscaled.x - _imu.mag_neutral.x) * IMU_MAG_X_SIGN * IMU_MAG_X_SENS_NUM) / IMU_MAG_X_SENS_DEN; \ int32_t msy = ((_imu.mag_unscaled.y - _imu.mag_neutral.y) * IMU_MAG_Y_SIGN * IMU_MAG_Y_SENS_NUM) / IMU_MAG_Y_SENS_DEN; \ _imu.mag.x = msx - msy; \ @@ -92,7 +92,7 @@ extern void booz_imu_init(void); _imu.mag.z = ((_imu.mag_unscaled.z - _imu.mag_neutral.z) * IMU_MAG_Z_SIGN * IMU_MAG_Z_SENS_NUM) / IMU_MAG_Z_SENS_DEN; \ } #else -#define BoozImuScaleMag(_imu) { \ +#define ImuScaleMag(_imu) { \ _imu.mag.x = ((_imu.mag_unscaled.x - _imu.mag_neutral.x) * IMU_MAG_X_SIGN * IMU_MAG_X_SENS_NUM) / IMU_MAG_X_SENS_DEN; \ _imu.mag.y = ((_imu.mag_unscaled.y - _imu.mag_neutral.y) * IMU_MAG_Y_SIGN * IMU_MAG_Y_SENS_NUM) / IMU_MAG_Y_SENS_DEN; \ _imu.mag.z = ((_imu.mag_unscaled.z - _imu.mag_neutral.z) * IMU_MAG_Z_SIGN * IMU_MAG_Z_SENS_NUM) / IMU_MAG_Z_SENS_DEN; \ diff --git a/sw/airborne/firmwares/rotorcraft/imu/arch/lpc21/imu_b2_arch.c b/sw/airborne/firmwares/rotorcraft/imu/arch/lpc21/imu_b2_arch.c index 83ae5fab7a..3a3463695d 100644 --- a/sw/airborne/firmwares/rotorcraft/imu/arch/lpc21/imu_b2_arch.c +++ b/sw/airborne/firmwares/rotorcraft/imu/arch/lpc21/imu_b2_arch.c @@ -21,9 +21,9 @@ * Boston, MA 02111-1307, USA. */ -#include "booz_imu.h" +#include "imu.h" -volatile uint8_t booz_imu_ssp_status; +volatile uint8_t imu_ssp_status; static void SSP_ISR(void) __attribute__((naked)); #if 0 static inline bool_t isr_try_mag(void); @@ -52,18 +52,18 @@ static inline bool_t isr_try_mag(void); #define SSP_PINSEL1_MOSI (2<<6) -#define BoozImuSetSSP8bits() { \ +#define ImuSetSSP8bits() { \ SSPCR0 = SSPCR0_VAL8; \ } -#define BoozImuSetSSP16bits() { \ +#define ImuSetSSP16bits() { \ SSPCR0 = SSPCR0_VAL16; \ } -void booz_imu_b2_arch_init(void) { +void imu_b2_arch_init(void) { - booz_imu_ssp_status = BOOZ_IMU_SSP_STA_IDLE; + imu_ssp_status = IMU_SSP_STA_IDLE; /* setup pins for SSP (SCK, MISO, MOSI) */ PINSEL1 |= SSP_PINSEL1_SCK | SSP_PINSEL1_MISO | SSP_PINSEL1_MOSI; @@ -82,14 +82,14 @@ void booz_imu_b2_arch_init(void) { } -void booz_imu_periodic(void) { +void imu_periodic(void) { // check ssp idle - // ASSERT((booz_imu_status == BOOZ_IMU_STA_IDLE), DEBUG_IMU, IMU_ERR_OVERUN); + // ASSERT((imu_status == IMU_STA_IDLE), DEBUG_IMU, IMU_ERR_OVERUN); // setup 16 bits - BoozImuSetSSP16bits(); + ImuSetSSP16bits(); // read adc - booz_imu_ssp_status = BOOZ_IMU_SSP_STA_BUSY_MAX1168; + imu_ssp_status = IMU_SSP_STA_BUSY_MAX1168; booz_max1168_read(); #if defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_AMI601 RunOnceEvery(10, { ami601_read(); }); @@ -147,36 +147,36 @@ static void SSP_ISR(void) { static void SSP_ISR(void) { ISR_ENTRY(); - switch (booz_imu_ssp_status) { - case BOOZ_IMU_SSP_STA_BUSY_MAX1168: + switch (imu_ssp_status) { + case IMU_SSP_STA_BUSY_MAX1168: Max1168OnSpiInt(); #if defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_MS2001 if (ms2001_status == MS2001_IDLE || ms2001_status == MS2001_GOT_EOC) { - BoozImuSetSSP8bits(); + ImuSetSSP8bits(); if (ms2001_status == MS2001_IDLE) { Ms2001SendReq(); } else { /* MS2001_GOT_EOC */ Ms2001ReadRes(); } - booz_imu_ssp_status = BOOZ_IMU_SSP_STA_BUSY_MS2100; + imu_ssp_status = IMU_SSP_STA_BUSY_MS2100; } else { #endif - booz_imu_ssp_status = BOOZ_IMU_SSP_STA_IDLE; + imu_ssp_status = IMU_SSP_STA_IDLE; #if defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_MS2001 } #endif break; #if defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_MS2001 - case BOOZ_IMU_SSP_STA_BUSY_MS2100: + case IMU_SSP_STA_BUSY_MS2100: Ms2001OnSpiIt(); if (ms2001_status == MS2001_IDLE) { Ms2001SendReq(); - booz_imu_ssp_status = BOOZ_IMU_SSP_STA_BUSY_MS2100; + imu_ssp_status = IMU_SSP_STA_BUSY_MS2100; } else - booz_imu_ssp_status = BOOZ_IMU_SSP_STA_IDLE; + imu_ssp_status = IMU_SSP_STA_IDLE; break; #endif // default: diff --git a/sw/airborne/firmwares/rotorcraft/imu/arch/lpc21/imu_b2_arch.h b/sw/airborne/firmwares/rotorcraft/imu/arch/lpc21/imu_b2_arch.h index c60292bd18..d54b22452a 100644 --- a/sw/airborne/firmwares/rotorcraft/imu/arch/lpc21/imu_b2_arch.h +++ b/sw/airborne/firmwares/rotorcraft/imu/arch/lpc21/imu_b2_arch.h @@ -21,8 +21,8 @@ * Boston, MA 02111-1307, USA. */ -#ifndef BOOZ_IMU_B2_ARCH_H -#define BOOZ_IMU_B2_ARCH_H +#ifndef IMU_B2_ARCH_H +#define IMU_B2_ARCH_H /* @@ -41,12 +41,12 @@ #include "LPC21xx.h" #include "interrupt_hw.h" -#define BOOZ_IMU_SSP_STA_IDLE 0 -#define BOOZ_IMU_SSP_STA_BUSY_MAX1168 1 -#define BOOZ_IMU_SSP_STA_BUSY_MS2100 2 -extern volatile uint8_t booz_imu_ssp_status; +#define IMU_SSP_STA_IDLE 0 +#define IMU_SSP_STA_BUSY_MAX1168 1 +#define IMU_SSP_STA_BUSY_MS2100 2 +extern volatile uint8_t imu_ssp_status; -#endif /* BOOZ_IMU_B2_ARCH_H */ +#endif /* IMU_B2_ARCH_H */ diff --git a/sw/airborne/firmwares/rotorcraft/imu/arch/lpc21/imu_crista_arch.c b/sw/airborne/firmwares/rotorcraft/imu/arch/lpc21/imu_crista_arch.c index db1a3a3262..58bcfad7b9 100644 --- a/sw/airborne/firmwares/rotorcraft/imu/arch/lpc21/imu_crista_arch.c +++ b/sw/airborne/firmwares/rotorcraft/imu/arch/lpc21/imu_crista_arch.c @@ -21,7 +21,7 @@ * Boston, MA 02111-1307, USA. */ -#include "booz_imu.h" +#include "imu.h" #include "LPC21xx.h" #include "armVIC.h" @@ -55,7 +55,7 @@ static void SPI1_ISR(void) __attribute__((naked)); static uint8_t channel; -void booz_imu_crista_arch_init(void) { +void imu_crista_arch_init(void) { channel = 0; /* setup pins for SSP (SCK, MISO, MOSI) */ diff --git a/sw/airborne/firmwares/rotorcraft/imu/arch/lpc21/imu_crista_arch.h b/sw/airborne/firmwares/rotorcraft/imu/arch/lpc21/imu_crista_arch.h index 8f9674dd2c..d9e4de990f 100644 --- a/sw/airborne/firmwares/rotorcraft/imu/arch/lpc21/imu_crista_arch.h +++ b/sw/airborne/firmwares/rotorcraft/imu/arch/lpc21/imu_crista_arch.h @@ -28,7 +28,7 @@ -#define BoozImuCristaArchPeriodic() { \ +#define ImuCristaArchPeriodic() { \ ADS8344_start(); \ } diff --git a/sw/airborne/firmwares/rotorcraft/imu/arch/sim/imu_b2_arch.c b/sw/airborne/firmwares/rotorcraft/imu/arch/sim/imu_b2_arch.c index 38a9900eb1..12c391cf22 100644 --- a/sw/airborne/firmwares/rotorcraft/imu/arch/sim/imu_b2_arch.c +++ b/sw/airborne/firmwares/rotorcraft/imu/arch/sim/imu_b2_arch.c @@ -21,21 +21,21 @@ * Boston, MA 02111-1307, USA. */ -#include "booz_imu.h" +#include "imu.h" #include "airframe.h" -void booz_imu_b2_arch_init(void) { +void imu_b2_arch_init(void) { } -void booz_imu_periodic(void) { +void imu_periodic(void) { } #include "nps_sensors.h" -void booz_imu_feed_gyro_accel(void) { +void imu_feed_gyro_accel(void) { booz_max1168_values[IMU_GYRO_P_CHAN] = sensors.gyro.value.x; booz_max1168_values[IMU_GYRO_Q_CHAN] = sensors.gyro.value.y; booz_max1168_values[IMU_GYRO_R_CHAN] = sensors.gyro.value.z; @@ -46,7 +46,7 @@ void booz_imu_feed_gyro_accel(void) { } -void booz_imu_feed_mag(void) { +void imu_feed_mag(void) { #if defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_MS2001 ms2001_values[IMU_MAG_X_CHAN] = sensors.mag.value.x; ms2001_values[IMU_MAG_Y_CHAN] = sensors.mag.value.y; diff --git a/sw/airborne/firmwares/rotorcraft/imu/arch/sim/imu_b2_arch.h b/sw/airborne/firmwares/rotorcraft/imu/arch/sim/imu_b2_arch.h index b501fd9681..9ff2a273b6 100644 --- a/sw/airborne/firmwares/rotorcraft/imu/arch/sim/imu_b2_arch.h +++ b/sw/airborne/firmwares/rotorcraft/imu/arch/sim/imu_b2_arch.h @@ -30,8 +30,8 @@ #define BOOZ2_IMU_B2_ARCH_H -extern void booz_imu_feed_gyro_accel(void); -extern void booz_imu_feed_mag(void); +extern void imu_feed_gyro_accel(void); +extern void imu_feed_mag(void); #endif /* BOOZ2_IMU_B2_HW_H */ diff --git a/sw/airborne/firmwares/rotorcraft/imu/arch/sim/imu_crista_arch.c b/sw/airborne/firmwares/rotorcraft/imu/arch/sim/imu_crista_arch.c index feb0ed9ec6..bbffe4d016 100644 --- a/sw/airborne/firmwares/rotorcraft/imu/arch/sim/imu_crista_arch.c +++ b/sw/airborne/firmwares/rotorcraft/imu/arch/sim/imu_crista_arch.c @@ -2,18 +2,18 @@ * simulator ARCH for rotorcraft imu crista */ -#include "booz/booz_imu.h" +#include "imu.h" #include "airframe.h" -void booz_imu_crista_arch_init(void) { +void imu_crista_arch_init(void) { } #include "nps_sensors.h" -void booz_imu_feed_gyro_accel(void) { +void imu_feed_gyro_accel(void) { ADS8344_values[IMU_GYRO_P_CHAN] = sensors.gyro.value.x; ADS8344_values[IMU_GYRO_Q_CHAN] = sensors.gyro.value.y; ADS8344_values[IMU_GYRO_R_CHAN] = sensors.gyro.value.z; @@ -23,7 +23,7 @@ void booz_imu_feed_gyro_accel(void) { ADS8344_available = TRUE; } -void booz_imu_feed_mag(void) { +void imu_feed_mag(void) { ami601_values[IMU_MAG_X_CHAN] = sensors.mag.value.x; ami601_values[IMU_MAG_Y_CHAN] = sensors.mag.value.y; ami601_values[IMU_MAG_Z_CHAN] = sensors.mag.value.z; diff --git a/sw/airborne/firmwares/rotorcraft/imu/arch/sim/imu_crista_arch.h b/sw/airborne/firmwares/rotorcraft/imu/arch/sim/imu_crista_arch.h index 1d761b0579..6f2f7d28c7 100644 --- a/sw/airborne/firmwares/rotorcraft/imu/arch/sim/imu_crista_arch.h +++ b/sw/airborne/firmwares/rotorcraft/imu/arch/sim/imu_crista_arch.h @@ -29,13 +29,13 @@ #ifndef BOOZ_IMU_CRISTA_ARCH_H #define BOOZ_IMU_CRISTA_ARCH_H -#include "booz/booz_imu.h" +#include "imu.h" -#define BoozImuCristaArchPeriodic() {} +#define ImuCristaArchPeriodic() {} -extern void booz_imu_feed_gyro_accel(void); -extern void booz_imu_feed_mag(void); +extern void imu_feed_gyro_accel(void); +extern void imu_feed_mag(void); #endif /* BOOZ_IMU_CRISTA_HW_H */ diff --git a/sw/airborne/firmwares/rotorcraft/imu/arch/stm32/imu_aspirin_arch.c b/sw/airborne/firmwares/rotorcraft/imu/arch/stm32/imu_aspirin_arch.c index 89352f8fec..cf9ef7dbdb 100644 --- a/sw/airborne/firmwares/rotorcraft/imu/arch/stm32/imu_aspirin_arch.c +++ b/sw/airborne/firmwares/rotorcraft/imu/arch/stm32/imu_aspirin_arch.c @@ -1,4 +1,4 @@ -#include "booz_imu.h" +#include "imu.h" #include #include @@ -21,7 +21,7 @@ void exti2_irq_handler(void); /* accelerometer dma end of rx handler */ void dma1_c4_irq_handler(void); -void booz_imu_aspirin_arch_init(void) { +void imu_aspirin_arch_init(void) { GPIO_InitTypeDef GPIO_InitStructure; EXTI_InitTypeDef EXTI_InitStructure; diff --git a/sw/airborne/firmwares/rotorcraft/imu/arch/stm32/imu_aspirin_arch.h b/sw/airborne/firmwares/rotorcraft/imu/arch/stm32/imu_aspirin_arch.h index 979ec0f6ce..79787800e9 100644 --- a/sw/airborne/firmwares/rotorcraft/imu/arch/stm32/imu_aspirin_arch.h +++ b/sw/airborne/firmwares/rotorcraft/imu/arch/stm32/imu_aspirin_arch.h @@ -1,11 +1,11 @@ #ifndef BOOZ_IMU_ASPIRIN_ARCH_H #define BOOZ_IMU_ASPIRIN_ARCH_H -#include "booz_imu.h" +#include "imu.h" #include "led.h" -extern void booz_imu_aspirin_arch_init(void); +extern void imu_aspirin_arch_init(void); extern void adxl345_write_to_reg(uint8_t addr, uint8_t val); extern void adxl345_clear_rx_buf(void); extern void adxl345_start_reading_data(void); diff --git a/sw/airborne/firmwares/rotorcraft/imu/arch/stm32/imu_b2_arch.c b/sw/airborne/firmwares/rotorcraft/imu/arch/stm32/imu_b2_arch.c index fb8cb49b57..59a894e985 100644 --- a/sw/airborne/firmwares/rotorcraft/imu/arch/stm32/imu_b2_arch.c +++ b/sw/airborne/firmwares/rotorcraft/imu/arch/stm32/imu_b2_arch.c @@ -21,7 +21,7 @@ * Boston, MA 02111-1307, USA. */ -#include "booz_imu.h" +#include "imu.h" #include #include @@ -34,12 +34,12 @@ #define BOOZ_IMU_SSP_STA_BUSY_MAX1168 1 #define BOOZ_IMU_SSP_STA_BUSY_MS2100 2 -volatile uint8_t booz_imu_ssp_status; +volatile uint8_t imu_ssp_status; void dma1_c4_irq_handler(void); void spi2_irq_handler(void); -void booz_imu_b2_arch_init(void) { +void imu_b2_arch_init(void) { /* Enable SPI2 Periph clock -------------------------------------------------*/ RCC_APB1PeriphClockCmd(RCC_APB1Periph_SPI2, ENABLE); @@ -70,13 +70,13 @@ void booz_imu_b2_arch_init(void) { }; NVIC_Init(&NVIC_init_structure_spi); - booz_imu_ssp_status = BOOZ_IMU_SSP_STA_IDLE; + imu_ssp_status = BOOZ_IMU_SSP_STA_IDLE; } -void booz_imu_periodic(void) { +void imu_periodic(void) { // check ssp idle - // ASSERT((booz_imu_status == BOOZ_IMU_STA_IDLE), DEBUG_IMU, IMU_ERR_OVERUN); - booz_imu_ssp_status = BOOZ_IMU_SSP_STA_BUSY_MAX1168; + // ASSERT((imu_status == BOOZ_IMU_STA_IDLE), DEBUG_IMU, IMU_ERR_OVERUN); + imu_ssp_status = BOOZ_IMU_SSP_STA_BUSY_MAX1168; Max1168ConfigureSPI(); SPI_Cmd(SPI2, ENABLE); booz_max1168_read(); @@ -84,27 +84,27 @@ void booz_imu_periodic(void) { } void dma1_c4_irq_handler(void) { - switch (booz_imu_ssp_status) { + switch (imu_ssp_status) { case BOOZ_IMU_SSP_STA_BUSY_MAX1168: Max1168OnDmaIrq(); SPI_Cmd(SPI2, DISABLE); if (ms2001_status == MS2001_IDLE) { Ms2001SendReq(); - booz_imu_ssp_status = BOOZ_IMU_SSP_STA_BUSY_MS2100; + imu_ssp_status = BOOZ_IMU_SSP_STA_BUSY_MS2100; } else if (ms2001_status == MS2001_WAITING_EOC && Ms2001HasEOC()) { Ms2001ReadRes(); - booz_imu_ssp_status = BOOZ_IMU_SSP_STA_BUSY_MS2100; + imu_ssp_status = BOOZ_IMU_SSP_STA_BUSY_MS2100; } else - booz_imu_ssp_status = BOOZ_IMU_SSP_STA_IDLE; + imu_ssp_status = BOOZ_IMU_SSP_STA_IDLE; break; case BOOZ_IMU_SSP_STA_BUSY_MS2100: Ms2001OnDmaIrq(); break; default: // POST_ERROR(DEBUG_IMU, IMU_ERR_SUPRIOUS_DMA1_C4_IRQ); - booz_imu_ssp_status = BOOZ_IMU_SSP_STA_IDLE; + imu_ssp_status = BOOZ_IMU_SSP_STA_IDLE; } } diff --git a/sw/airborne/firmwares/rotorcraft/imu/arch/stm32/imu_b2_arch.h b/sw/airborne/firmwares/rotorcraft/imu/arch/stm32/imu_b2_arch.h index 7936461890..804f398e05 100644 --- a/sw/airborne/firmwares/rotorcraft/imu/arch/stm32/imu_b2_arch.h +++ b/sw/airborne/firmwares/rotorcraft/imu/arch/stm32/imu_b2_arch.h @@ -1,6 +1,6 @@ /* - * $Id: booz_imu_b2_arch.h 3732 2009-07-20 17:46:54Z poine $ - * + * $Id: imu_b2_arch.h 3732 2009-07-20 17:46:54Z poine $ + * * Copyright (C) 2008-2009 Antoine Drouin * * This file is part of paparazzi. diff --git a/sw/airborne/firmwares/rotorcraft/imu/arch/stm32/imu_crista_arch.c b/sw/airborne/firmwares/rotorcraft/imu/arch/stm32/imu_crista_arch.c index f93a2bf6d7..efd2cee557 100644 --- a/sw/airborne/firmwares/rotorcraft/imu/arch/stm32/imu_crista_arch.c +++ b/sw/airborne/firmwares/rotorcraft/imu/arch/stm32/imu_crista_arch.c @@ -21,7 +21,7 @@ * Boston, MA 02111-1307, USA. */ -#include "booz_imu.h" +#include "imu.h" #include #include @@ -44,7 +44,7 @@ static uint8_t buf_out[4]; extern void dma1_c4_irq_handler(void); static void ADS8344_read_channel( void ); -void booz_imu_crista_arch_init(void) { +void imu_crista_arch_init(void) { channel = 0; /* Enable SPI2 Periph clock -------------------------------------------------*/ diff --git a/sw/airborne/firmwares/rotorcraft/imu/arch/stm32/imu_crista_arch.h b/sw/airborne/firmwares/rotorcraft/imu/arch/stm32/imu_crista_arch.h index 4a1db8ba90..44e527b1ac 100644 --- a/sw/airborne/firmwares/rotorcraft/imu/arch/stm32/imu_crista_arch.h +++ b/sw/airborne/firmwares/rotorcraft/imu/arch/stm32/imu_crista_arch.h @@ -24,7 +24,7 @@ #define BOOZ_IMU_CRISTA_ARCH_H -#define BoozImuCristaArchPeriodic() { \ +#define ImuCristaArchPeriodic() { \ ADS8344_start(); \ } diff --git a/sw/airborne/firmwares/rotorcraft/imu/imu_aspirin.c b/sw/airborne/firmwares/rotorcraft/imu/imu_aspirin.c index c7815b66ca..53d5e721a3 100644 --- a/sw/airborne/firmwares/rotorcraft/imu/imu_aspirin.c +++ b/sw/airborne/firmwares/rotorcraft/imu/imu_aspirin.c @@ -1,8 +1,8 @@ -#include "booz_imu.h" +#include "imu.h" #include "i2c.h" -struct BoozImuAspirin imu_aspirin; +struct ImuAspirin imu_aspirin; /* initialize peripherals */ static void configure_gyro(void); @@ -10,7 +10,7 @@ static void configure_mag(void); static void configure_accel(void); -void booz_imu_impl_init(void) { +void imu_impl_init(void) { imu_aspirin.status = AspirinStatusUninit; imu_aspirin.gyro_available = FALSE; @@ -19,12 +19,12 @@ void booz_imu_impl_init(void) { imu_aspirin.mag_available = FALSE; imu_aspirin.accel_available = FALSE; - booz_imu_aspirin_arch_init(); + imu_aspirin_arch_init(); } -void booz_imu_periodic(void) { +void imu_periodic(void) { if (imu_aspirin.status == AspirinStatusUninit) { configure_gyro(); configure_mag(); diff --git a/sw/airborne/firmwares/rotorcraft/imu/imu_aspirin.h b/sw/airborne/firmwares/rotorcraft/imu/imu_aspirin.h index f50c47695b..06576e617f 100644 --- a/sw/airborne/firmwares/rotorcraft/imu/imu_aspirin.h +++ b/sw/airborne/firmwares/rotorcraft/imu/imu_aspirin.h @@ -21,11 +21,11 @@ * Boston, MA 02111-1307, USA. */ -#ifndef BOOZ_IMU_ASPIRIN_H -#define BOOZ_IMU_ASPIRIN_H +#ifndef IMU_ASPIRIN_H +#define IMU_ASPIRIN_H #include "airframe.h" -#include "booz_imu.h" +#include "imu.h" #include "i2c.h" #include "booz/peripherals/booz_itg3200.h" @@ -56,7 +56,7 @@ enum AspirinStatus AspirinStatusReadingMag }; -struct BoozImuAspirin { +struct ImuAspirin { volatile enum AspirinStatus status; struct i2c_transaction i2c_trans_gyro; struct i2c_transaction i2c_trans_mag; @@ -69,18 +69,18 @@ struct BoozImuAspirin { volatile uint8_t accel_rx_buf[7]; }; -extern struct BoozImuAspirin imu_aspirin; +extern struct ImuAspirin imu_aspirin; -#define BoozImuMagEvent(_mag_handler) {} +#define ImuMagEvent(_mag_handler) {} -#define BoozImuEvent(_gyro_accel_handler, _mag_handler) { \ +#define ImuEvent(_gyro_accel_handler, _mag_handler) { \ if (imu_aspirin.status == AspirinStatusReadingGyro && \ - imu_aspirin.i2c_trans_gyro.status == I2CTransSuccess) { \ + imu_aspirin.i2c_trans_gyro.status == I2CTransSuccess) { \ int16_t gp = imu_aspirin.i2c_trans_gyro.buf[0]<<8 | imu_aspirin.i2c_trans_gyro.buf[1]; \ int16_t gq = imu_aspirin.i2c_trans_gyro.buf[2]<<8 | imu_aspirin.i2c_trans_gyro.buf[3]; \ int16_t gr = imu_aspirin.i2c_trans_gyro.buf[4]<<8 | imu_aspirin.i2c_trans_gyro.buf[5]; \ - RATES_ASSIGN(booz_imu.gyro_unscaled, gp, gq, gr); \ + RATES_ASSIGN(imu.gyro_unscaled, gp, gq, gr); \ if (imu_aspirin.mag_ready_for_read ) { \ /* read mag */ \ imu_aspirin.i2c_trans_mag.type = I2CTransRx; \ @@ -91,15 +91,15 @@ extern struct BoozImuAspirin imu_aspirin; imu_aspirin.status = AspirinStatusReadingMag; \ } \ else { \ - imu_aspirin.status = AspirinStatusIdle; \ + imu_aspirin.status = AspirinStatusIdle; \ } \ } \ if (imu_aspirin.status == AspirinStatusReadingMag && \ - imu_aspirin.i2c_trans_mag.status == I2CTransSuccess) { \ + imu_aspirin.i2c_trans_mag.status == I2CTransSuccess) { \ int16_t mx = imu_aspirin.i2c_trans_mag.buf[0]<<8 | imu_aspirin.i2c_trans_mag.buf[1]; \ int16_t my = imu_aspirin.i2c_trans_mag.buf[2]<<8 | imu_aspirin.i2c_trans_mag.buf[3]; \ int16_t mz = imu_aspirin.i2c_trans_mag.buf[4]<<8 | imu_aspirin.i2c_trans_mag.buf[5]; \ - VECT3_ASSIGN(booz_imu.mag_unscaled, mx, my, mz); \ + VECT3_ASSIGN(imu.mag_unscaled, mx, my, mz); \ imu_aspirin.mag_available = TRUE; \ imu_aspirin.status = AspirinStatusIdle; \ \ @@ -117,15 +117,15 @@ extern struct BoozImuAspirin imu_aspirin; const int16_t ax = imu_aspirin.accel_rx_buf[1] | (imu_aspirin.accel_rx_buf[2]<<8); \ const int16_t ay = imu_aspirin.accel_rx_buf[3] | (imu_aspirin.accel_rx_buf[4]<<8); \ const int16_t az = imu_aspirin.accel_rx_buf[5] | (imu_aspirin.accel_rx_buf[6]<<8); \ - VECT3_ASSIGN(booz_imu.accel_unscaled, ax, ay, az); \ + VECT3_ASSIGN(imu.accel_unscaled, ax, ay, az); \ _gyro_accel_handler(); \ } \ } /* underlying architecture */ -#include "imu/booz_imu_aspirin_arch.h" +#include "imu_aspirin_arch.h" /* must be implemented by underlying architecture */ -extern void booz_imu_b2_arch_init(void); +extern void imu_b2_arch_init(void); -#endif /* BOOZ_IMU_ASPIRIN_H */ +#endif /* IMU_ASPIRIN_H */ diff --git a/sw/airborne/firmwares/rotorcraft/imu/imu_b2.c b/sw/airborne/firmwares/rotorcraft/imu/imu_b2.c index 69b081437e..0ad3b6ae86 100644 --- a/sw/airborne/firmwares/rotorcraft/imu/imu_b2.c +++ b/sw/airborne/firmwares/rotorcraft/imu/imu_b2.c @@ -21,11 +21,11 @@ * Boston, MA 02111-1307, USA. */ -#include "booz_imu.h" +#include "imu.h" -void booz_imu_impl_init(void) { +void imu_impl_init(void) { - booz_imu_b2_arch_init(); + imu_b2_arch_init(); booz_max1168_init(); #if defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_MS2001 diff --git a/sw/airborne/firmwares/rotorcraft/imu/imu_b2.h b/sw/airborne/firmwares/rotorcraft/imu/imu_b2.h index 3d4e0d5649..0b5d069163 100644 --- a/sw/airborne/firmwares/rotorcraft/imu/imu_b2.h +++ b/sw/airborne/firmwares/rotorcraft/imu/imu_b2.h @@ -21,10 +21,10 @@ * Boston, MA 02111-1307, USA. */ -#ifndef BOOZ_IMU_B2_H -#define BOOZ_IMU_B2_H +#ifndef IMU_B2_H +#define IMU_B2_H -#include "booz_imu.h" +#include "imu.h" #include "airframe.h" #include "peripherals/booz_max1168.h" @@ -112,11 +112,11 @@ #if defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_MS2001 #include "peripherals/booz_ms2001.h" -#define BoozImuMagEvent(_mag_handler) { \ +#define ImuMagEvent(_mag_handler) { \ if (ms2001_status == MS2001_DATA_AVAILABLE) { \ - booz_imu.mag_unscaled.x = ms2001_values[IMU_MAG_X_CHAN]; \ - booz_imu.mag_unscaled.y = ms2001_values[IMU_MAG_Y_CHAN]; \ - booz_imu.mag_unscaled.z = ms2001_values[IMU_MAG_Z_CHAN]; \ + imu.mag_unscaled.x = ms2001_values[IMU_MAG_X_CHAN]; \ + imu.mag_unscaled.y = ms2001_values[IMU_MAG_Y_CHAN]; \ + imu.mag_unscaled.z = ms2001_values[IMU_MAG_Z_CHAN]; \ ms2001_status = MS2001_IDLE; \ _mag_handler(); \ } \ @@ -124,40 +124,40 @@ #elif defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_AMI601 #include "peripherals/booz_ami601.h" #define foo_handler() {} -#define BoozImuMagEvent(_mag_handler) { \ +#define ImuMagEvent(_mag_handler) { \ AMI601Event(foo_handler); \ if (ami601_status == AMI601_DATA_AVAILABLE) { \ - booz_imu.mag_unscaled.x = ami601_values[IMU_MAG_X_CHAN]; \ - booz_imu.mag_unscaled.y = ami601_values[IMU_MAG_Y_CHAN]; \ - booz_imu.mag_unscaled.z = ami601_values[IMU_MAG_Z_CHAN]; \ + imu.mag_unscaled.x = ami601_values[IMU_MAG_X_CHAN]; \ + imu.mag_unscaled.y = ami601_values[IMU_MAG_Y_CHAN]; \ + imu.mag_unscaled.z = ami601_values[IMU_MAG_Z_CHAN]; \ ami601_status = AMI601_IDLE; \ _mag_handler(); \ } \ } #else -#define BoozImuMagEvent(_mag_handler) {} +#define ImuMagEvent(_mag_handler) {} #endif -#define BoozImuEvent(_gyro_accel_handler, _mag_handler) { \ +#define ImuEvent(_gyro_accel_handler, _mag_handler) { \ if (booz_max1168_status == STA_MAX1168_DATA_AVAILABLE) { \ - booz_imu.gyro_unscaled.p = booz_max1168_values[IMU_GYRO_P_CHAN]; \ - booz_imu.gyro_unscaled.q = booz_max1168_values[IMU_GYRO_Q_CHAN]; \ - booz_imu.gyro_unscaled.r = booz_max1168_values[IMU_GYRO_R_CHAN]; \ - booz_imu.accel_unscaled.x = booz_max1168_values[IMU_ACCEL_X_CHAN]; \ - booz_imu.accel_unscaled.y = booz_max1168_values[IMU_ACCEL_Y_CHAN]; \ - booz_imu.accel_unscaled.z = booz_max1168_values[IMU_ACCEL_Z_CHAN]; \ + imu.gyro_unscaled.p = booz_max1168_values[IMU_GYRO_P_CHAN]; \ + imu.gyro_unscaled.q = booz_max1168_values[IMU_GYRO_Q_CHAN]; \ + imu.gyro_unscaled.r = booz_max1168_values[IMU_GYRO_R_CHAN]; \ + imu.accel_unscaled.x = booz_max1168_values[IMU_ACCEL_X_CHAN]; \ + imu.accel_unscaled.y = booz_max1168_values[IMU_ACCEL_Y_CHAN]; \ + imu.accel_unscaled.z = booz_max1168_values[IMU_ACCEL_Z_CHAN]; \ booz_max1168_status = STA_MAX1168_IDLE; \ _gyro_accel_handler(); \ } \ - BoozImuMagEvent(_mag_handler); \ + ImuMagEvent(_mag_handler); \ } /* underlying architecture */ -#include "imu/booz_imu_b2_arch.h" +#include "imu_b2_arch.h" /* must be implemented by underlying architecture */ -extern void booz_imu_b2_arch_init(void); +extern void imu_b2_arch_init(void); -#endif /* BOOZ_IMU_B2_H */ +#endif /* IMU_B2_H */ diff --git a/sw/airborne/firmwares/rotorcraft/imu/imu_crista.c b/sw/airborne/firmwares/rotorcraft/imu/imu_crista.c index cfb995028f..47ee4a794f 100644 --- a/sw/airborne/firmwares/rotorcraft/imu/imu_crista.c +++ b/sw/airborne/firmwares/rotorcraft/imu/imu_crista.c @@ -21,16 +21,16 @@ * Boston, MA 02111-1307, USA. */ -#include "booz_imu.h" +#include "imu.h" volatile bool_t ADS8344_available; uint16_t ADS8344_values[ADS8344_NB_CHANNELS]; -void booz_imu_impl_init(void) { +void imu_impl_init(void) { ADS8344_available = FALSE; - booz_imu_crista_arch_init(); + imu_crista_arch_init(); #ifdef USE_AMI601 ami601_init(); @@ -38,9 +38,9 @@ void booz_imu_impl_init(void) { } -void booz_imu_periodic(void) { +void imu_periodic(void) { - BoozImuCristaArchPeriodic(); + ImuCristaArchPeriodic(); #ifdef USE_AMI601 RunOnceEvery(10, { ami601_read(); }); #endif diff --git a/sw/airborne/firmwares/rotorcraft/imu/imu_crista.h b/sw/airborne/firmwares/rotorcraft/imu/imu_crista.h index 73d0f166b3..7c82f99abc 100644 --- a/sw/airborne/firmwares/rotorcraft/imu/imu_crista.h +++ b/sw/airborne/firmwares/rotorcraft/imu/imu_crista.h @@ -21,53 +21,53 @@ * Boston, MA 02111-1307, USA. */ -#ifndef BOOZ_IMU_CRISTA_H -#define BOOZ_IMU_CRISTA_H +#ifndef IMU_CRISTA_H +#define IMU_CRISTA_H -#include "booz_imu.h" +#include "imu.h" #include "airframe.h" #define ADS8344_NB_CHANNELS 8 extern uint16_t ADS8344_values[ADS8344_NB_CHANNELS]; extern volatile bool_t ADS8344_available; -#define BoozImuEvent(_gyro_accel_handler, _mag_handler) { \ +#define ImuEvent(_gyro_accel_handler, _mag_handler) { \ if (ADS8344_available) { \ ADS8344_available = FALSE; \ - booz_imu.gyro_unscaled.p = ADS8344_values[IMU_GYRO_P_CHAN]; \ - booz_imu.gyro_unscaled.q = ADS8344_values[IMU_GYRO_Q_CHAN]; \ - booz_imu.gyro_unscaled.r = ADS8344_values[IMU_GYRO_R_CHAN]; \ - booz_imu.accel_unscaled.x = ADS8344_values[IMU_ACCEL_X_CHAN]; \ - booz_imu.accel_unscaled.y = ADS8344_values[IMU_ACCEL_Y_CHAN]; \ - booz_imu.accel_unscaled.z = ADS8344_values[IMU_ACCEL_Z_CHAN]; \ + imu.gyro_unscaled.p = ADS8344_values[IMU_GYRO_P_CHAN]; \ + imu.gyro_unscaled.q = ADS8344_values[IMU_GYRO_Q_CHAN]; \ + imu.gyro_unscaled.r = ADS8344_values[IMU_GYRO_R_CHAN]; \ + imu.accel_unscaled.x = ADS8344_values[IMU_ACCEL_X_CHAN]; \ + imu.accel_unscaled.y = ADS8344_values[IMU_ACCEL_Y_CHAN]; \ + imu.accel_unscaled.z = ADS8344_values[IMU_ACCEL_Z_CHAN]; \ /* spare 3, temp 7 */ \ _gyro_accel_handler(); \ } \ - BoozImuMagEvent(_mag_handler); \ + ImuMagEvent(_mag_handler); \ } #ifdef USE_AMI601 #include "peripherals/booz_ami601.h" #define foo_handler() {} -#define BoozImuMagEvent(_mag_handler) { \ +#define ImuMagEvent(_mag_handler) { \ AMI601Event(foo_handler); \ if (ami601_status == AMI601_DATA_AVAILABLE) { \ - booz_imu.mag_unscaled.x = ami601_values[IMU_MAG_X_CHAN]; \ - booz_imu.mag_unscaled.y = ami601_values[IMU_MAG_Y_CHAN]; \ - booz_imu.mag_unscaled.z = ami601_values[IMU_MAG_Z_CHAN]; \ + imu.mag_unscaled.x = ami601_values[IMU_MAG_X_CHAN]; \ + imu.mag_unscaled.y = ami601_values[IMU_MAG_Y_CHAN]; \ + imu.mag_unscaled.z = ami601_values[IMU_MAG_Z_CHAN]; \ ami601_status = AMI601_IDLE; \ _mag_handler(); \ } \ } #else -#define BoozImuMagEvent(_mag_handler) {} +#define ImuMagEvent(_mag_handler) {} #endif /* underlying architecture */ -#include "imu/booz_imu_crista_arch.h" +#include "imu_crista_arch.h" /* must be defined by underlying architecture */ -extern void booz_imu_crista_arch_init(void); +extern void imu_crista_arch_init(void); -#endif /* BOOZ_IMU_CRISTA_H */ +#endif /* IMU_CRISTA_H */ diff --git a/sw/airborne/firmwares/rotorcraft/main.c b/sw/airborne/firmwares/rotorcraft/main.c index e8b40b335b..f871aaede9 100644 --- a/sw/airborne/firmwares/rotorcraft/main.c +++ b/sw/airborne/firmwares/rotorcraft/main.c @@ -38,7 +38,7 @@ #include "booz_actuators.h" #include "booz_radio_control.h" -#include "booz_imu.h" +#include "imu.h" #include "booz_gps.h" #include "booz/booz2_analog.h" @@ -110,7 +110,7 @@ STATIC_INLINE void booz2_main_init( void ) { #endif booz2_battery_init(); - booz_imu_init(); + imu_init(); booz_fms_init(); autopilot_init(); @@ -137,7 +137,7 @@ STATIC_INLINE void booz2_main_init( void ) { STATIC_INLINE void booz2_main_periodic( void ) { - booz_imu_periodic(); + imu_periodic(); /* run control loops */ autopilot_periodic(); @@ -199,7 +199,7 @@ STATIC_INLINE void booz2_main_event( void ) { RadioControlEvent(autopilot_on_rc_frame); } - BoozImuEvent(on_gyro_accel_event, on_mag_event); + ImuEvent(on_gyro_accel_event, on_mag_event); BaroEvent(on_baro_abs_event, on_baro_dif_event); @@ -217,8 +217,8 @@ STATIC_INLINE void booz2_main_event( void ) { static inline void on_gyro_accel_event( void ) { - BoozImuScaleGyro(booz_imu); - BoozImuScaleAccel(booz_imu); + ImuScaleGyro(imu); + ImuScaleAccel(imu); if (booz_ahrs.status == BOOZ_AHRS_UNINIT) { booz_ahrs_aligner_run(); @@ -248,7 +248,7 @@ static inline void on_gps_event(void) { } static inline void on_mag_event(void) { - BoozImuScaleMag(booz_imu); + ImuScaleMag(imu); if (booz_ahrs.status == BOOZ_AHRS_RUNNING) booz_ahrs_update_mag(); } diff --git a/sw/airborne/fms/fms_spi_autopilot_msg.c b/sw/airborne/fms/fms_spi_autopilot_msg.c index d169554982..ff7599544e 100644 --- a/sw/airborne/fms/fms_spi_autopilot_msg.c +++ b/sw/airborne/fms/fms_spi_autopilot_msg.c @@ -42,11 +42,11 @@ #include "airframe.h" #include "actuators.h" -#include "rdyb_booz_imu.h" +#include "rdyb_imu.h" #include "booz_radio_control.h" #include "rdyb_mahrs.h" -static struct BoozImuFloat imu; +static struct ImuFloat imu; static struct FloatQuat body_to_imu_quat = IMU_POSE_BODY_TO_IMU_QUAT; static void (* vane_callback)(uint8_t vane_id, float alpha, float beta) = NULL; @@ -163,7 +163,7 @@ static void passthrough_up_parse(struct AutopilotMessagePTUp *msg_up) imu.mag.z = MAG_FLOAT_OF_BFP(msg_up->mag.z); if (msg_up->valid.imu) - rdyb_booz_imu_update(&imu); + rdyb_imu_update(&imu); } static void passthrough_down_fill(struct AutopilotMessagePTDown *msg_down) diff --git a/sw/airborne/fms/overo_test_passthrough.h b/sw/airborne/fms/overo_test_passthrough.h index 8f529bbc02..071cfbda62 100644 --- a/sw/airborne/fms/overo_test_passthrough.h +++ b/sw/airborne/fms/overo_test_passthrough.h @@ -2,14 +2,14 @@ #define OVERO_TEST_PASSTHROUGH_H #include "std.h" -#include "booz/booz_imu.h" +#include "imu.h" struct OveroTestPassthrough { /* our network connection */ char* gs_gw; /* our sensors */ - struct BoozImuFloat imu; + struct ImuFloat imu; uint8_t rc_status; int32_t baro_abs; int32_t baro_diff; diff --git a/sw/airborne/lisa/lisa_stm_passthrough_main.c b/sw/airborne/lisa/lisa_stm_passthrough_main.c index f4e13a6060..81bd46c0d1 100644 --- a/sw/airborne/lisa/lisa_stm_passthrough_main.c +++ b/sw/airborne/lisa/lisa_stm_passthrough_main.c @@ -28,7 +28,7 @@ #include "booz/booz2_commands.h" #include "booz/booz_actuators.h" #include "booz/actuators/booz_actuators_pwm.h" -#include "booz/booz_imu.h" +#include "imu.h" #include "booz/booz_radio_control.h" #include "lisa/lisa_overo_link.h" #include "airframe.h" @@ -91,7 +91,7 @@ static inline void main_init(void) { hw_init(); sys_time_init(); - booz_imu_init(); + imu_init(); baro_init(); radio_control_init(); booz_actuators_init(); @@ -121,7 +121,7 @@ static inline void main_periodic(void) { uint16_t v1 = 123; uint16_t v2 = 123; - booz_imu_periodic(); + imu_periodic(); OveroLinkPeriodic(on_overo_link_lost); RunOnceEvery(10, { @@ -160,9 +160,9 @@ static inline void on_overo_link_msg_received(void) { /* IMU up */ overo_link.up.msg.valid.imu = 1; - RATES_COPY(overo_link.up.msg.gyro, booz_imu.gyro); - VECT3_COPY(overo_link.up.msg.accel, booz_imu.accel); - VECT3_COPY(overo_link.up.msg.mag, booz_imu.mag); + RATES_COPY(overo_link.up.msg.gyro, imu.gyro); + VECT3_COPY(overo_link.up.msg.accel, imu.accel); + VECT3_COPY(overo_link.up.msg.mag, imu.mag); /* RC up */ overo_link.up.msg.valid.rc = new_radio_msg; @@ -228,12 +228,12 @@ static inline void on_overo_link_crc_failed(void) { } static inline void on_gyro_accel_event(void) { - BoozImuScaleGyro(booz_imu); - BoozImuScaleAccel(booz_imu); + ImuScaleGyro(imu); + ImuScaleAccel(imu); } static inline void on_mag_event(void) { - BoozImuScaleMag(booz_imu); + ImuScaleMag(imu); } static inline void on_vane_msg(void *data) { @@ -262,7 +262,7 @@ static inline void main_on_baro_abs(void) { static inline void main_event(void) { - BoozImuEvent(on_gyro_accel_event, on_mag_event); + ImuEvent(on_gyro_accel_event, on_mag_event); BaroEvent(main_on_baro_abs, main_on_baro_diff); OveroLinkEvent(on_overo_link_msg_received, on_overo_link_crc_failed); RadioControlEvent(on_rc_message); diff --git a/sw/airborne/lisa/test/hs_gyro.c b/sw/airborne/lisa/test/hs_gyro.c index e1d0b86b7b..ea39465c3c 100644 --- a/sw/airborne/lisa/test/hs_gyro.c +++ b/sw/airborne/lisa/test/hs_gyro.c @@ -31,7 +31,7 @@ #include "messages.h" #include "downlink.h" -#include "booz_imu.h" +#include "imu.h" #include "interrupt_hw.h" @@ -62,7 +62,7 @@ static inline void main_init( void ) { hw_init(); sys_time_init(); - booz_imu_init(); + imu_init(); int_enable(); } @@ -72,21 +72,21 @@ static inline void main_periodic_task( void ) { LED_TOGGLE(3); DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM); }); - booz_imu_periodic(); + imu_periodic(); RunOnceEvery(10, { LED_PERIODIC();}); } static inline void main_event_task( void ) { - BoozImuEvent(on_gyro_accel_event, on_mag_event); + ImuEvent(on_gyro_accel_event, on_mag_event); } #define NB_SAMPLES 20 static inline void on_gyro_accel_event(void) { - BoozImuScaleGyro(); - BoozImuScaleAccel(); + ImuScaleGyro(); + ImuScaleAccel(); LED_TOGGLE(2); @@ -95,16 +95,16 @@ static inline void on_gyro_accel_event(void) { const uint8_t axis = MEASURED_SENSOR_NB; cnt++; if (cnt > NB_SAMPLES) cnt = 0; - samples[cnt] = booz_imu.MEASURED_SENSOR; + samples[cnt] = imu.MEASURED_SENSOR; if (cnt == NB_SAMPLES-1) { DOWNLINK_SEND_IMU_HS_GYRO(DefaultChannel, &axis, NB_SAMPLES, samples); } if (cnt == 10) { DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel, - &booz_imu.gyro_unscaled.p, - &booz_imu.gyro_unscaled.q, - &booz_imu.gyro_unscaled.r); + &imu.gyro_unscaled.p, + &imu.gyro_unscaled.q, + &imu.gyro_unscaled.r); } diff --git a/sw/airborne/modules/vehicle_interface/vi.c b/sw/airborne/modules/vehicle_interface/vi.c index 2403a91739..647cb4e89c 100644 --- a/sw/airborne/modules/vehicle_interface/vi.c +++ b/sw/airborne/modules/vehicle_interface/vi.c @@ -23,7 +23,7 @@ #include "vehicle_interface/vi.h" -#include "booz/booz_imu.h" +#include "imu.h" #include "booz/booz_gps.h" #include "booz/booz_ahrs.h" diff --git a/sw/simulator/nps/nps_autopilot_booz.c b/sw/simulator/nps/nps_autopilot_booz.c index 41c638fd17..47ea91e855 100644 --- a/sw/simulator/nps/nps_autopilot_booz.c +++ b/sw/simulator/nps/nps_autopilot_booz.c @@ -4,7 +4,7 @@ #include "nps_sensors.h" #include "nps_radio_control.h" #include "booz_radio_control.h" -#include "booz/booz_imu.h" +#include "imu.h" #include "firmwares/rotorcraft/baro.h" #include "actuators/booz_supervision.h" @@ -35,12 +35,12 @@ void nps_autopilot_run_step(double time __attribute__ ((unused))) { } if (nps_sensors_gyro_available()) { - booz_imu_feed_gyro_accel(); + imu_feed_gyro_accel(); booz2_main_event(); } if (nps_sensors_mag_available()) { - booz_imu_feed_mag(); + imu_feed_mag(); booz2_main_event(); }