rename booz_imu to imu, fix imu makefiles

This commit is contained in:
Felix Ruess
2010-09-27 22:56:23 +00:00
parent 0c0237d239
commit 28118816bd
54 changed files with 412 additions and 389 deletions
@@ -35,6 +35,10 @@
# #
# imu Booz2 v1 # 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 += -DBOOZ_IMU_TYPE_H=\"imu/imu_b2.h\"
ap.CFLAGS += -DIMU_B2_VERSION_1_0 ap.CFLAGS += -DIMU_B2_VERSION_1_0
ap.CFLAGS += -DIMU_B2_MAG_TYPE=IMU_B2_MAG_AMI601 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 # 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 += -DBOOZ_IMU_TYPE_H=\"imu/imu_b2.h\"
sim.CFLAGS += -DIMU_B2_VERSION_1_0 sim.CFLAGS += -DIMU_B2_VERSION_1_0
sim.CFLAGS += -DIMU_B2_MAG_TYPE=IMU_B2_MAG_AMI601 sim.CFLAGS += -DIMU_B2_MAG_TYPE=IMU_B2_MAG_AMI601
sim.srcs += $(SRC_FIRMWARE)/imu.c \ sim.srcs += $(SRC_FIRMWARE)/imu.c \
$(SRC_FIRMWARE)/imu/imu_b2.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 \ sim.srcs += $(SRC_BOOZ)/peripherals/booz_max1168.c \
$(SRC_BOOZ_SIM)/peripherals/booz_max1168_arch.c $(SRC_BOOZ_SIM)/peripherals/booz_max1168_arch.c
@@ -40,6 +40,10 @@
# imu Booz2 v1.1 # 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 += -DBOOZ_IMU_TYPE_H=\"imu/imu_b2.h\"
imu_CFLAGS += -DIMU_B2_MAG_TYPE=IMU_B2_MAG_MS2001 imu_CFLAGS += -DIMU_B2_MAG_TYPE=IMU_B2_MAG_MS2001
imu_CFLAGS += -DIMU_B2_VERSION_1_1 imu_CFLAGS += -DIMU_B2_VERSION_1_1
@@ -72,12 +76,15 @@ ap.srcs += $(imu_srcs)
# Simulator # 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 += -DBOOZ_IMU_TYPE_H=\"imu/imu_b2.h\"
sim.CFLAGS += -DIMU_B2_VERSION_1_1 sim.CFLAGS += -DIMU_B2_VERSION_1_1
sim.CFLAGS += -DIMU_B2_MAG_TYPE=IMU_B2_MAG_AMI601 sim.CFLAGS += -DIMU_B2_MAG_TYPE=IMU_B2_MAG_AMI601
sim.srcs += $(SRC_FIRMWARE)/imu.c \ sim.srcs += $(SRC_FIRMWARE)/imu.c \
$(SRC_FIRMWARE)/imu/imu_b2.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 \ sim.srcs += $(SRC_BOOZ)/peripherals/booz_max1168.c \
$(SRC_BOOZ_SIM)/peripherals/booz_max1168_arch.c $(SRC_BOOZ_SIM)/peripherals/booz_max1168_arch.c
@@ -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.CFLAGS += -DBOOZ_IMU_TYPE_H=\"imu/imu_crista.h\"
ap.srcs += $(SRC_FIRMWARE)/imu.c \ ap.srcs += $(SRC_FIRMWARE)/imu.c \
$(SRC_FIRMWARE)/imu/imu_crista.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 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.CFLAGS += -DBOOZ_IMU_TYPE_H=\"imu/imu_crista.h\"
sim.srcs += $(SRC_FIRMWARE)/imu.c \ sim.srcs += $(SRC_FIRMWARE)/imu.c \
$(SRC_FIRMWARE)/imu/imu_crista.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.CFLAGS += -DUSE_AMI601
sim.srcs += $(SRC_BOOZ)/peripherals/booz_ami601.c sim.srcs += $(SRC_BOOZ)/peripherals/booz_ami601.c
+1 -1
View File
@@ -24,7 +24,7 @@
#include "max1167.h" #include "max1167.h"
#include "booz_imu.h" #include "imu.h"
void max1167_hw_init( void ) {} void max1167_hw_init( void ) {}
+17 -17
View File
@@ -37,7 +37,7 @@
#include "fms_debug.h" #include "fms_debug.h"
#include "fms_spi_link.h" #include "fms_spi_link.h"
#include "fms_autopilot_msg.h" #include "fms_autopilot_msg.h"
#include "booz/booz_imu.h" #include "imu.h"
#include "overo_file_logger.h" #include "overo_file_logger.h"
#include "overo_gcs_com.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_in;
static struct AutopilotMessageCRCFrame msg_out; static struct AutopilotMessageCRCFrame msg_out;
struct BoozImu booz_imu; struct Imu imu;
struct BoozImuFloat booz_imu_float; struct ImuFloat imu_float;
static uint32_t foo = 0; static uint32_t foo = 0;
@@ -68,9 +68,9 @@ int main(int argc, char *argv[]) {
(void) signal(SIGINT, main_exit); (void) signal(SIGINT, main_exit);
//set IMU neutrals //set IMU neutrals
RATES_ASSIGN(booz_imu.gyro_neutral, IMU_GYRO_P_NEUTRAL, IMU_GYRO_Q_NEUTRAL, IMU_GYRO_R_NEUTRAL); RATES_ASSIGN(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(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); VECT3_ASSIGN(imu.mag_neutral, IMU_MAG_X_NEUTRAL, IMU_MAG_Y_NEUTRAL, IMU_MAG_Z_NEUTRAL);
if (spi_link_init()) { if (spi_link_init()) {
TRACE(TRACE_ERROR, "%s", "failed to open SPI link \n"); 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(); main_talk_with_stm32();
BoozImuScaleGyro(booz_imu); ImuScaleGyro(imu);
RunOnceEvery(50, {DOWNLINK_SEND_BETH(gcs_com.udp_transport, 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, &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, /* 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) //&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, 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 //&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, 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) //&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, RunOnceEvery(50, {DOWNLINK_SEND_AHRS_EULER(gcs_com.udp_transport,
&estimator.tilt, &estimator.elevation, &estimator.azimuth );}); &estimator.tilt, &estimator.elevation, &estimator.azimuth );});
RunOnceEvery(50, {DOWNLINK_SEND_BOOZ2_ACCEL(DefaultChannel, 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 //&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()); RunOnceEvery(33, gcs_com_periodic());
@@ -293,12 +293,12 @@ static void main_talk_with_stm32() {
foo++; foo++;
booz_imu.gyro_unscaled.p = (msg_in.payload.msg_up.gyro.p&0xFFFF); 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); 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); 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); 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); 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.accel_unscaled.z = (msg_in.payload.msg_up.accel.z&0xFFFF);
} }
+31 -31
View File
@@ -31,7 +31,7 @@
#include "booz/booz2_commands.h" #include "booz/booz2_commands.h"
#include "booz/booz_actuators.h" #include "booz/booz_actuators.h"
//#include "booz/booz_radio_control.h" //#include "booz/booz_radio_control.h"
#include "booz/booz_imu.h" #include "imu.h"
#include "lisa/lisa_overo_link.h" #include "lisa/lisa_overo_link.h"
#include "beth/bench_sensors.h" #include "beth/bench_sensors.h"
@@ -65,7 +65,7 @@ static inline void main_init( void ) {
sys_time_init(); sys_time_init();
actuators_init(); actuators_init();
//radio_control_init(); //radio_control_init();
booz_imu_init(); imu_init();
overo_link_init(); overo_link_init();
bench_sensors_init(); bench_sensors_init();
booz2_commands[COMMAND_ROLL] = 0; booz2_commands[COMMAND_ROLL] = 0;
@@ -76,7 +76,7 @@ static inline void main_init( void ) {
static inline void main_periodic( void ) { static inline void main_periodic( void ) {
int8_t pitch_out,thrust_out; int8_t pitch_out,thrust_out;
booz_imu_periodic(); imu_periodic();
OveroLinkPeriodic(main_on_overo_link_lost) OveroLinkPeriodic(main_on_overo_link_lost)
@@ -114,7 +114,7 @@ static inline void main_periodic( void ) {
} }
static inline void main_event( 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); 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.y = bench_sensors.angle_2;
overo_link.up.msg.bench_sensor.z = bench_sensors.angle_3; 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.x = imu.accel_unscaled.x;
overo_link.up.msg.accel.y = booz_imu.accel_unscaled.y; overo_link.up.msg.accel.y = imu.accel_unscaled.y;
overo_link.up.msg.accel.z = booz_imu.accel_unscaled.z; 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.p = imu.gyro_unscaled.p;
overo_link.up.msg.gyro.q = booz_imu.gyro_unscaled.q; overo_link.up.msg.gyro.q = imu.gyro_unscaled.q;
overo_link.up.msg.gyro.r = booz_imu.gyro_unscaled.r; overo_link.up.msg.gyro.r = imu.gyro_unscaled.r;
//can_err_flags (uint16) represents the board number that is not communicating regularly //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 //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) { static inline void on_gyro_accel_event(void) {
BoozImuScaleGyro(booz_imu); ImuScaleGyro(imu);
BoozImuScaleAccel(booz_imu); ImuScaleAccel(imu);
//LED_TOGGLE(2); //LED_TOGGLE(2);
static uint8_t cnt; static uint8_t cnt;
@@ -161,46 +161,46 @@ static inline void on_gyro_accel_event(void) {
if (cnt == 0) { if (cnt == 0) {
DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel, DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel,
&booz_imu.gyro_unscaled.p, &imu.gyro_unscaled.p,
&booz_imu.gyro_unscaled.q, &imu.gyro_unscaled.q,
&booz_imu.gyro_unscaled.r); &imu.gyro_unscaled.r);
DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel, DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel,
&booz_imu.accel_unscaled.x, &imu.accel_unscaled.x,
&booz_imu.accel_unscaled.y, &imu.accel_unscaled.y,
&booz_imu.accel_unscaled.z); &imu.accel_unscaled.z);
} }
else if (cnt == 7) { else if (cnt == 7) {
DOWNLINK_SEND_BOOZ2_GYRO(DefaultChannel, DOWNLINK_SEND_BOOZ2_GYRO(DefaultChannel,
&booz_imu.gyro.p, &imu.gyro.p,
&booz_imu.gyro.q, &imu.gyro.q,
&booz_imu.gyro.r); &imu.gyro.r);
DOWNLINK_SEND_BOOZ2_ACCEL(DefaultChannel, DOWNLINK_SEND_BOOZ2_ACCEL(DefaultChannel,
&booz_imu.accel.x, &imu.accel.x,
&booz_imu.accel.y, &imu.accel.y,
&booz_imu.accel.z); &imu.accel.z);
} }
} }
static inline void on_mag_event(void) { static inline void on_mag_event(void) {
BoozImuScaleMag(booz_imu); ImuScaleMag(imu);
static uint8_t cnt; static uint8_t cnt;
cnt++; cnt++;
if (cnt > 1) cnt = 0; if (cnt > 1) cnt = 0;
if (cnt%2) { if (cnt%2) {
DOWNLINK_SEND_BOOZ2_MAG(DefaultChannel, DOWNLINK_SEND_BOOZ2_MAG(DefaultChannel,
&booz_imu.mag.x, &imu.mag.x,
&booz_imu.mag.y, &imu.mag.y,
&booz_imu.mag.z); &imu.mag.z);
} }
else { else {
DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel,
&booz_imu.mag_unscaled.x, &imu.mag_unscaled.x,
&booz_imu.mag_unscaled.y, &imu.mag_unscaled.y,
&booz_imu.mag_unscaled.z); &imu.mag_unscaled.z);
} }
} }
+6 -6
View File
@@ -1,6 +1,6 @@
#include "overo_estimator.h" #include "overo_estimator.h"
#include "booz/booz_imu.h" #include "imu.h"
#include <math.h> #include <math.h>
#include "messages2.h" #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); Bound(estimator.tilt,-89,89);
//low pass filter tilt gyro //low pass filter tilt gyro
estimator.tilt_dot = estimator.tilt_dot + 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 /* Second order filter yet to be tested
estimator.tilt_dot = estimator.tilt_dot * (2 - estimator.tilt_lp_coeff1 - estimator.tilt_lp_coeff2) + 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_dot_old * (1 - estimator.tilt_lp_coeff1 - estimator.tilt_lp_coeff2 +
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); Bound(estimator.elevation,-45,45);
//rotation compensation (mixing of two gyro values to generate a reading that reflects rate along beth axes //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) + float rotated_elev_dot = RATE_FLOAT_OF_BFP(imu.gyro.p) * cos(estimator.tilt) +
RATE_FLOAT_OF_BFP(booz_imu.gyro.r) * sin(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. //low pass filter -- should probably increase order and maybe move filtering to measured values.
estimator.elevation_dot = estimator.elevation_dot + estimator.elevation_dot = estimator.elevation_dot +
estimator.elevation_lp_coeff * (rotated_elev_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 //low pass filter azimuth gyro
//TODO: compensate rotation and increase order //TODO: compensate rotation and increase order
estimator.azimuth_dot = estimator.azimuth_dot + 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);
} }
+3 -3
View File
@@ -1,6 +1,6 @@
#include "overo_file_logger.h" #include "overo_file_logger.h"
#include "booz/booz_imu.h" #include "imu.h"
struct FileLogger file_logger; struct FileLogger file_logger;
@@ -15,8 +15,8 @@ void file_logger_init(char* filename) {
void file_logger_periodic(void) { void file_logger_periodic(void) {
static uint32_t foo = 0; static uint32_t foo = 0;
foo++; 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_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,booz_imu.gyro_unscaled.p,booz_imu.gyro_unscaled.q,booz_imu.gyro_unscaled.r); 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);
} }
+5 -5
View File
@@ -24,7 +24,7 @@
#include "booz_ahrs_aligner.h" #include "booz_ahrs_aligner.h"
#include <stdlib.h> /* for abs() */ #include <stdlib.h> /* for abs() */
#include "booz_imu.h" #include "imu.h"
#include "led.h" #include "led.h"
struct BoozAhrsAligner booz_ahrs_aligner; struct BoozAhrsAligner booz_ahrs_aligner;
@@ -52,11 +52,11 @@ void booz_ahrs_aligner_init(void) {
void booz_ahrs_aligner_run(void) { void booz_ahrs_aligner_run(void) {
RATES_ADD(gyro_sum, booz_imu.gyro); RATES_ADD(gyro_sum, imu.gyro);
VECT3_ADD(accel_sum, booz_imu.accel); VECT3_ADD(accel_sum, imu.accel);
VECT3_ADD(mag_sum, booz_imu.mag); 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++; samples_idx++;
#ifdef AHRS_ALIGNER_LED #ifdef AHRS_ALIGNER_LED
+17 -17
View File
@@ -23,7 +23,7 @@
#include "booz_ahrs_cmpl_euler.h" #include "booz_ahrs_cmpl_euler.h"
#include "booz_imu.h" #include "imu.h"
#include "booz_ahrs_aligner.h" #include "booz_ahrs_aligner.h"
#include "airframe.h" #include "airframe.h"
@@ -97,7 +97,7 @@ void booz_ahrs_propagate(void) {
/* unbias gyro */ /* unbias gyro */
struct Int32Rates uf_rate; 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 */ /* low pass rate */
RATES_ADD(booz_ahrs.imu_rate, uf_rate); RATES_ADD(booz_ahrs.imu_rate, uf_rate);
RATES_SDIV(booz_ahrs.imu_rate, booz_ahrs.imu_rate, 2); 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); INT32_RMAT_OF_EULERS(booz_ahrs.ltp_to_imu_rmat, booz_ahrs.ltp_to_imu_euler);
/* Compute LTP to BODY quaternion */ /* 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 */ /* 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 */ /* compute LTP to BODY eulers */
INT32_EULERS_OF_RMAT(booz_ahrs.ltp_to_body_euler, booz_ahrs.ltp_to_body_rmat); INT32_EULERS_OF_RMAT(booz_ahrs.ltp_to_body_euler, booz_ahrs.ltp_to_body_rmat);
/* compute body rates */ /* 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) { void booz_ahrs_update_accel(void) {
/* build a measurement assuming constant acceleration ?!! */ /* 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; int32_t cphi;
PPRZ_ITRIG_COS(cphi, measurement.phi); PPRZ_ITRIG_COS(cphi, measurement.phi);
int32_t cphi_ax = -INT_MULT_RSHIFT(cphi, booz_imu.accel.x, INT32_TRIG_FRAC); int32_t cphi_ax = -INT_MULT_RSHIFT(cphi, imu.accel.x, INT32_TRIG_FRAC);
INT32_ATAN2(measurement.theta, -cphi_ax, -booz_imu.accel.z); INT32_ATAN2(measurement.theta, -cphi_ax, -imu.accel.z);
measurement.phi *= F_UPDATE; measurement.phi *= F_UPDATE;
measurement.theta *= 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; //int32_t cphi_ctheta = (cphi*ctheta)>>INT32_TRIG_FRAC;
const int32_t mn = const int32_t mn =
ctheta * booz_imu.mag.x + ctheta * imu.mag.x +
sphi_stheta * booz_imu.mag.y + sphi_stheta * imu.mag.y +
cphi_stheta * booz_imu.mag.z; cphi_stheta * imu.mag.z;
const int32_t me = const int32_t me =
0 * booz_imu.mag.x + 0 * imu.mag.x +
cphi * booz_imu.mag.y + cphi * imu.mag.y +
-sphi * booz_imu.mag.z; -sphi * imu.mag.z;
//const int32_t md = //const int32_t md =
// -stheta * booz_imu.mag.x + // -stheta * imu.mag.x +
// sphi_ctheta * booz_imu.mag.y + // sphi_ctheta * imu.mag.y +
// cphi_ctheta * booz_imu.mag.z; // cphi_ctheta * imu.mag.z;
float m_psi = -atan2(me, mn); float m_psi = -atan2(me, mn);
measurement.psi = ((m_psi - RadOfDeg(booz_ahrs_mag_offset))*(float)(1<<(INT32_ANGLE_FRAC))*F_UPDATE); measurement.psi = ((m_psi - RadOfDeg(booz_ahrs_mag_offset))*(float)(1<<(INT32_ANGLE_FRAC))*F_UPDATE);
+1 -1
View File
@@ -121,7 +121,7 @@ extern void booz_ahrs_align(void);
void booz_ahrs_propagate(void) { void booz_ahrs_propagate(void) {
/* compute unbiased rates */ /* 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); RATES_SUB(bafe_rates, bafe_bias);
/* compute F /* compute F
+7 -7
View File
@@ -24,7 +24,7 @@
#include "booz_ahrs_float_lkf.h" #include "booz_ahrs_float_lkf.h"
#include "booz_imu.h" #include "imu.h"
#include "booz_ahrs_aligner.h" #include "booz_ahrs_aligner.h"
#include "airframe.h" #include "airframe.h"
@@ -200,13 +200,13 @@ float bafl_R_mag;
#define AHRS_LTP_TO_BODY() { \ #define AHRS_LTP_TO_BODY() { \
/* Compute LTP to BODY quaternion */ \ /* 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 */ \ /* 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 */ \ /* compute LTP to BODY eulers */ \
INT32_EULERS_OF_RMAT(booz_ahrs.ltp_to_body_euler, booz_ahrs.ltp_to_body_rmat); \ INT32_EULERS_OF_RMAT(booz_ahrs.ltp_to_body_euler, booz_ahrs.ltp_to_body_rmat); \
/* compute body rates */ \ /* 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) { static inline void booz_ahrs_lowpass_accel(void) {
// get latest measurement // 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 // low pass it
VECT3_ADD(bafl_accel_measure, bafl_accel_last); VECT3_ADD(bafl_accel_measure, bafl_accel_last);
VECT3_SDIV(bafl_accel_measure, bafl_accel_measure, 2); VECT3_SDIV(bafl_accel_measure, bafl_accel_measure, 2);
@@ -290,7 +290,7 @@ void booz_ahrs_propagate(void) {
booz_ahrs_lowpass_accel(); booz_ahrs_lowpass_accel();
/* compute unbiased rates */ /* 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); RATES_SUB(bafl_rates, bafl_bias);
@@ -647,7 +647,7 @@ static void booz_ahrs_do_update_mag(void) {
printf("Mag update.\n"); printf("Mag update.\n");
#endif #endif
MAGS_FLOAT_OF_BFP(bafl_mag, booz_imu.mag); MAGS_FLOAT_OF_BFP(bafl_mag, imu.mag);
/* P_prio = P */ /* P_prio = P */
for (i = 0; i < BAFL_SSIZE; i++) { for (i = 0; i < BAFL_SSIZE; i++) {
+2 -2
View File
@@ -24,7 +24,7 @@
#include "booz2_ins.h" #include "booz2_ins.h"
#include "booz_imu.h" #include "imu.h"
#include "firmwares/rotorcraft/baro.h" #include "firmwares/rotorcraft/baro.h"
#include "booz_gps.h" #include "booz_gps.h"
@@ -149,7 +149,7 @@ void booz_ins_realign_v(float z) {
void booz_ins_propagate() { void booz_ins_propagate() {
/* untilt accels */ /* untilt accels */
struct Int32Vect3 accel_body; 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; struct Int32Vect3 accel_ltp;
INT32_RMAT_TRANSP_VMULT(accel_ltp, booz_ahrs.ltp_to_body_rmat, accel_body); 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); float z_accel_float = ACCEL_FLOAT_OF_BFP(accel_ltp.z);
+26 -26
View File
@@ -46,7 +46,7 @@
#define PERIODIC_SEND_ALIVE(_chan) DOWNLINK_SEND_ALIVE(_chan, 16, MD5SUM) #define PERIODIC_SEND_ALIVE(_chan) DOWNLINK_SEND_ALIVE(_chan, 16, MD5SUM)
#include "booz2_battery.h" #include "booz2_battery.h"
#include "booz_imu.h" #include "imu.h"
#include "booz_gps.h" #include "booz_gps.h"
#include "booz2_ins.h" #include "booz2_ins.h"
#include "booz_ahrs.h" #include "booz_ahrs.h"
@@ -57,10 +57,10 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
#ifdef USE_GPS #ifdef USE_GPS
#define PERIODIC_SEND_BOOZ_STATUS(_chan) { \ #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 _twi_blmc_nb_err = 0; \
DOWNLINK_SEND_BOOZ_STATUS(_chan, \ DOWNLINK_SEND_BOOZ_STATUS(_chan, \
&booz_imu_nb_err, \ &imu_nb_err, \
&_twi_blmc_nb_err, \ &_twi_blmc_nb_err, \
&radio_control.status, \ &radio_control.status, \
&booz_gps_state.fix, \ &booz_gps_state.fix, \
@@ -75,11 +75,11 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
} }
#else /* !USE_GPS */ #else /* !USE_GPS */
#define PERIODIC_SEND_BOOZ_STATUS(_chan) { \ #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 twi_blmc_nb_err = 0; \
uint8_t fix = BOOZ2_GPS_FIX_NONE; \ uint8_t fix = BOOZ2_GPS_FIX_NONE; \
DOWNLINK_SEND_BOOZ_STATUS(_chan, \ DOWNLINK_SEND_BOOZ_STATUS(_chan, \
&booz_imu_nb_err, \ &imu_nb_err, \
&twi_blmc_nb_err, \ &twi_blmc_nb_err, \
&radio_control.status, \ &radio_control.status, \
&fix, \ &fix, \
@@ -130,44 +130,44 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
#define PERIODIC_SEND_BOOZ2_GYRO(_chan) { \ #define PERIODIC_SEND_BOOZ2_GYRO(_chan) { \
DOWNLINK_SEND_BOOZ2_GYRO(_chan, \ DOWNLINK_SEND_BOOZ2_GYRO(_chan, \
&booz_imu.gyro.p, \ &imu.gyro.p, \
&booz_imu.gyro.q, \ &imu.gyro.q, \
&booz_imu.gyro.r); \ &imu.gyro.r); \
} }
#define PERIODIC_SEND_BOOZ2_ACCEL(_chan) { \ #define PERIODIC_SEND_BOOZ2_ACCEL(_chan) { \
DOWNLINK_SEND_BOOZ2_ACCEL(_chan, \ DOWNLINK_SEND_BOOZ2_ACCEL(_chan, \
&booz_imu.accel.x, \ &imu.accel.x, \
&booz_imu.accel.y, \ &imu.accel.y, \
&booz_imu.accel.z); \ &imu.accel.z); \
} }
#define PERIODIC_SEND_BOOZ2_MAG(_chan) { \ #define PERIODIC_SEND_BOOZ2_MAG(_chan) { \
DOWNLINK_SEND_BOOZ2_MAG(_chan, \ DOWNLINK_SEND_BOOZ2_MAG(_chan, \
&booz_imu.mag.x, \ &imu.mag.x, \
&booz_imu.mag.y, \ &imu.mag.y, \
&booz_imu.mag.z); \ &imu.mag.z); \
} }
#define PERIODIC_SEND_IMU_GYRO_RAW(_chan) { \ #define PERIODIC_SEND_IMU_GYRO_RAW(_chan) { \
DOWNLINK_SEND_IMU_GYRO_RAW(_chan, \ DOWNLINK_SEND_IMU_GYRO_RAW(_chan, \
&booz_imu.gyro_unscaled.p, \ &imu.gyro_unscaled.p, \
&booz_imu.gyro_unscaled.q, \ &imu.gyro_unscaled.q, \
&booz_imu.gyro_unscaled.r); \ &imu.gyro_unscaled.r); \
} }
#define PERIODIC_SEND_IMU_ACCEL_RAW(_chan) { \ #define PERIODIC_SEND_IMU_ACCEL_RAW(_chan) { \
DOWNLINK_SEND_IMU_ACCEL_RAW(_chan, \ DOWNLINK_SEND_IMU_ACCEL_RAW(_chan, \
&booz_imu.accel_unscaled.x, \ &imu.accel_unscaled.x, \
&booz_imu.accel_unscaled.y, \ &imu.accel_unscaled.y, \
&booz_imu.accel_unscaled.z); \ &imu.accel_unscaled.z); \
} }
#define PERIODIC_SEND_IMU_MAG_RAW(_chan) { \ #define PERIODIC_SEND_IMU_MAG_RAW(_chan) { \
DOWNLINK_SEND_IMU_MAG_RAW(_chan, \ DOWNLINK_SEND_IMU_MAG_RAW(_chan, \
&booz_imu.mag_unscaled.x, \ &imu.mag_unscaled.x, \
&booz_imu.mag_unscaled.y, \ &imu.mag_unscaled.y, \
&booz_imu.mag_unscaled.z); \ &imu.mag_unscaled.z); \
} }
/* FIXME: make that depend on board */ /* 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.p, \
&booz_ahrs_aligner.lp_gyro.q, \ &booz_ahrs_aligner.lp_gyro.q, \
&booz_ahrs_aligner.lp_gyro.r, \ &booz_ahrs_aligner.lp_gyro.r, \
&booz_imu.gyro.p, \ &imu.gyro.p, \
&booz_imu.gyro.q, \ &imu.gyro.q, \
&booz_imu.gyro.r, \ &imu.gyro.r, \
&booz_ahrs_aligner.noise, \ &booz_ahrs_aligner.noise, \
&booz_ahrs_aligner.low_noise_cnt); \ &booz_ahrs_aligner.low_noise_cnt); \
} }
+1 -1
View File
@@ -22,7 +22,7 @@
*/ */
#include "booz_ahrs.h" #include "booz_ahrs.h"
#include "booz_imu.h" #include "imu.h"
struct BoozAhrs booz_ahrs; struct BoozAhrs booz_ahrs;
struct BoozAhrsFloat booz_ahrs_float; struct BoozAhrsFloat booz_ahrs_float;
+4 -4
View File
@@ -23,7 +23,7 @@
#include "booz_fms.h" #include "booz_fms.h"
#include "booz_imu.h" #include "imu.h"
#include "booz_gps.h" #include "booz_gps.h"
#include "booz_ahrs.h" #include "booz_ahrs.h"
@@ -71,9 +71,9 @@ void booz_fms_set_enabled(bool_t enabled) {
void booz_fms_update_info(void) { void booz_fms_update_info(void) {
// PPRZ_INT16_OF_INT32_RATES(booz_fms_info.imu.gyro, booz_imu.gyro); // PPRZ_INT16_OF_INT32_RATES(booz_fms_info.imu.gyro, imu.gyro);
// PPRZ_INT16_OF_INT32_VECT3(booz_fms_info.imu.accel, booz_imu.accel); // PPRZ_INT16_OF_INT32_VECT3(booz_fms_info.imu.accel, imu.accel);
//PPRZ_INT16_OF_INT32_VECT3(booz_fms_info.imu.mag, booz_imu.mag); //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_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); //PPRZ_INT16_OF_INT32_VECT3(booz_fms_info.gps.speed, booz_gps_state.ecef_speed);
+2 -2
View File
@@ -24,7 +24,7 @@
#include "booz2_hf_float.h" #include "booz2_hf_float.h"
#include "booz2_ins.h" #include "booz2_ins.h"
#include "booz_imu.h" #include "imu.h"
#include "booz_ahrs.h" #include "booz_ahrs.h"
#include "math/pprz_algebra_int.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))); VECT3_SDIV(scaled_biases, b2ins_accel_bias, (1<<(B2INS_ACCEL_BIAS_FRAC-B2INS_ACCEL_LTP_FRAC)));
struct Int32Vect3 accel_imu; struct Int32Vect3 accel_imu;
/* unbias accelerometers */ /* unbias accelerometers */
VECT3_DIFF(accel_imu, booz_imu.accel, scaled_biases); VECT3_DIFF(accel_imu, imu.accel, scaled_biases);
/* convert to LTP */ /* convert to LTP */
// BOOZ_IQUAT_VDIV(b2ins_accel_ltp, booz_ahrs.ltp_to_imu_quat, accel_imu); // 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); INT32_RMAT_TRANSP_VMULT(b2ins_accel_ltp, booz_ahrs.ltp_to_imu_rmat, accel_imu);
+2 -2
View File
@@ -24,7 +24,7 @@
#include "booz2_hf_float.h" #include "booz2_hf_float.h"
#include "booz2_ins.h" #include "booz2_ins.h"
#include "booz_imu.h" #include "imu.h"
#include "booz_ahrs.h" #include "booz_ahrs.h"
#include "booz_gps.h" #include "booz_gps.h"
#include <stdlib.h> #include <stdlib.h>
@@ -112,7 +112,7 @@ struct AccBuf acc_body;
struct Int32Vect3 acc_body_mean; struct Int32Vect3 acc_body_mean;
void b2_hff_store_accel_body(void) { 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; 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 */ /* once the buffer is full it always has the last acc_body.size accel measurements */
@@ -26,7 +26,7 @@
#include "booz_ahrs.h" #include "booz_ahrs.h"
#include "booz_imu.h" #include "imu.h"
#include "booz_radio_control.h" #include "booz_radio_control.h"
#include "airframe.h" #include "airframe.h"
+25 -25
View File
@@ -31,7 +31,7 @@
#include "messages.h" #include "messages.h"
#include "downlink.h" #include "downlink.h"
#include "booz_imu.h" #include "imu.h"
#include "interrupt_hw.h" #include "interrupt_hw.h"
@@ -58,7 +58,7 @@ static inline void main_init( void ) {
hw_init(); hw_init();
sys_time_init(); sys_time_init();
booz_imu_init(); imu_init();
DEBUG_SERVO1_INIT(); DEBUG_SERVO1_INIT();
DEBUG_SERVO2_INIT(); DEBUG_SERVO2_INIT();
@@ -86,19 +86,19 @@ static inline void main_periodic_task( void ) {
&i2c2_errors.last_unexpected_event); &i2c2_errors.last_unexpected_event);
}); });
#endif #endif
if (cpu_time_sec > 1) booz_imu_periodic(); if (cpu_time_sec > 1) imu_periodic();
RunOnceEvery(10, { LED_PERIODIC();}); RunOnceEvery(10, { LED_PERIODIC();});
} }
static inline void main_event_task( void ) { 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) { static inline void on_gyro_accel_event(void) {
BoozImuScaleGyro(booz_imu); ImuScaleGyro(imu);
BoozImuScaleAccel(booz_imu); ImuScaleAccel(imu);
LED_TOGGLE(2); LED_TOGGLE(2);
static uint8_t cnt; static uint8_t cnt;
@@ -107,45 +107,45 @@ static inline void on_gyro_accel_event(void) {
if (cnt == 0) { if (cnt == 0) {
DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel, DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel,
&booz_imu.gyro_unscaled.p, &imu.gyro_unscaled.p,
&booz_imu.gyro_unscaled.q, &imu.gyro_unscaled.q,
&booz_imu.gyro_unscaled.r); &imu.gyro_unscaled.r);
DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel, DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel,
&booz_imu.accel_unscaled.x, &imu.accel_unscaled.x,
&booz_imu.accel_unscaled.y, &imu.accel_unscaled.y,
&booz_imu.accel_unscaled.z); &imu.accel_unscaled.z);
} }
else if (cnt == 7) { else if (cnt == 7) {
DOWNLINK_SEND_BOOZ2_GYRO(DefaultChannel, DOWNLINK_SEND_BOOZ2_GYRO(DefaultChannel,
&booz_imu.gyro.p, &imu.gyro.p,
&booz_imu.gyro.q, &imu.gyro.q,
&booz_imu.gyro.r); &imu.gyro.r);
DOWNLINK_SEND_BOOZ2_ACCEL(DefaultChannel, DOWNLINK_SEND_BOOZ2_ACCEL(DefaultChannel,
&booz_imu.accel.x, &imu.accel.x,
&booz_imu.accel.y, &imu.accel.y,
&booz_imu.accel.z); &imu.accel.z);
} }
} }
static inline void on_mag_event(void) { static inline void on_mag_event(void) {
BoozImuScaleMag(booz_imu); ImuScaleMag(imu);
static uint8_t cnt; static uint8_t cnt;
cnt++; cnt++;
if (cnt > 1) cnt = 0; if (cnt > 1) cnt = 0;
if (cnt%2) { if (cnt%2) {
DOWNLINK_SEND_BOOZ2_MAG(DefaultChannel, DOWNLINK_SEND_BOOZ2_MAG(DefaultChannel,
&booz_imu.mag.x, &imu.mag.x,
&booz_imu.mag.y, &imu.mag.y,
&booz_imu.mag.z); &imu.mag.z);
} }
else { else {
DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel,
&booz_imu.mag_unscaled.x, &imu.mag_unscaled.x,
&booz_imu.mag_unscaled.y, &imu.mag_unscaled.y,
&booz_imu.mag_unscaled.z); &imu.mag_unscaled.z);
} }
} }
+2 -2
View File
@@ -1,3 +1,3 @@
#include "booz_imu.h" #include "imu.h"
void booz_imu_impl_init(void) {} void imu_impl_init(void) {}
+7 -7
View File
@@ -2,7 +2,7 @@
#include <string.h> #include <string.h>
#include "math/pprz_algebra_double.h" #include "math/pprz_algebra_double.h"
#include "booz_imu.h" #include "imu.h"
#include "booz_ahrs.h" #include "booz_ahrs.h"
#include "ahrs/booz_ahrs_mlkf.h" #include "ahrs/booz_ahrs_mlkf.h"
@@ -39,7 +39,7 @@ int main(int argc, char** argv) {
read_data(IN_FILE); read_data(IN_FILE);
booz_imu_init(); imu_init();
booz_ahrs_init(); booz_ahrs_init();
for (int i=0; i<nb_samples; i++) { for (int i=0; i<nb_samples; i++) {
@@ -79,14 +79,14 @@ static void read_data(const char* filename) {
static void feed_imu(int i) { static void feed_imu(int i) {
if (i>0) { if (i>0) {
RATES_COPY(booz_imu.gyro_prev, booz_imu.gyro); RATES_COPY(imu.gyro_prev, imu.gyro);
} }
else { 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); RATES_BFP_OF_REAL(imu.gyro, samples[i].gyro);
ACCELS_BFP_OF_REAL(booz_imu.accel, samples[i].accel); ACCELS_BFP_OF_REAL(imu.accel, samples[i].accel);
MAGS_BFP_OF_REAL(booz_imu.mag, samples[i].mag); MAGS_BFP_OF_REAL(imu.mag, samples[i].mag);
} }
+2 -2
View File
@@ -40,7 +40,7 @@
#include "csc_msg_def.h" #include "csc_msg_def.h"
#include ACTUATORS #include ACTUATORS
#include "booz/booz_imu.h" #include "imu.h"
#include "booz/ahrs/booz_ahrs_aligner.h" #include "booz/ahrs/booz_ahrs_aligner.h"
#include "booz/booz_ahrs.h" #include "booz/booz_ahrs.h"
#include "mercury_xsens.h" #include "mercury_xsens.h"
@@ -112,7 +112,7 @@ static inline void csc_main_init( void ) {
Uart0Init(); Uart0Init();
Uart1Init(); Uart1Init();
booz_imu_init(); imu_init();
booz_ahrs_aligner_init(); booz_ahrs_aligner_init();
booz_ahrs_init(); booz_ahrs_init();
+24 -24
View File
@@ -27,10 +27,10 @@
*/ */
#include "mercury_xsens.h" #include "mercury_xsens.h"
#include "booz/booz_imu.h" #include "imu.h"
#include "booz/booz_ahrs.h" #include "booz/booz_ahrs.h"
#include "booz/ahrs/booz_ahrs_aligner.h" #include "booz/ahrs/booz_ahrs_aligner.h"
#include "booz/booz_imu.h" #include "imu.h"
#include "csc_booz2_ins.h" #include "csc_booz2_ins.h"
#include <inttypes.h> #include <inttypes.h>
@@ -202,7 +202,7 @@ static uint8_t xsens_msg_idx[XSENS_COUNT];
static uint8_t ck[XSENS_COUNT]; static uint8_t ck[XSENS_COUNT];
static uint8_t send_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; xsens_msg_buf_ci[i] = 0;
FLOAT_RMAT_ZERO(xsens_rmat_neutral[i]); FLOAT_RMAT_ZERO(xsens_rmat_neutral[i]);
} }
booz_imu.accel_neutral.x = IMU_ACCEL_X_NEUTRAL; imu.accel_neutral.x = IMU_ACCEL_X_NEUTRAL;
booz_imu.accel_neutral.y = IMU_ACCEL_Y_NEUTRAL; imu.accel_neutral.y = IMU_ACCEL_Y_NEUTRAL;
booz_imu.accel_neutral.z = IMU_ACCEL_Z_NEUTRAL; imu.accel_neutral.z = IMU_ACCEL_Z_NEUTRAL;
booz_imu.gyro_neutral.p = IMU_GYRO_P_NEUTRAL; imu.gyro_neutral.p = IMU_GYRO_P_NEUTRAL;
booz_imu.gyro_neutral.q = IMU_GYRO_Q_NEUTRAL; imu.gyro_neutral.q = IMU_GYRO_Q_NEUTRAL;
booz_imu.gyro_neutral.r = IMU_GYRO_R_NEUTRAL; imu.gyro_neutral.r = IMU_GYRO_R_NEUTRAL;
booz_imu.mag_neutral.x = IMU_MAG_X_NEUTRAL; imu.mag_neutral.x = IMU_MAG_X_NEUTRAL;
booz_imu.mag_neutral.y = IMU_MAG_Y_NEUTRAL; imu.mag_neutral.y = IMU_MAG_Y_NEUTRAL;
booz_imu.mag_neutral.z = IMU_MAG_Z_NEUTRAL; imu.mag_neutral.z = IMU_MAG_Z_NEUTRAL;
// Also TODO: set scenario to aerospace // Also TODO: set scenario to aerospace
// set magnetic declination angle // set magnetic declination angle
// Probably quicker to just set everything once via MT Manager software // 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; uint8_t offset = 0;
// test RAW modes else calibrated modes // test RAW modes else calibrated modes
if (XSENS_MASK_RAWInertial(xsens_output_mode[xsens_id])){// || (XSENS_MASK_RAWGPS(xsens2_output_mode))) { 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); 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); 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); 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); 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); 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); 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); 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); 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); imu.mag_unscaled.z = XSENS_DATA_RAWInertial_magX(xsens_msg_buf[xsens_id][buf_slot],offset);
BoozImuScaleGyro(booz_imu); ImuScaleGyro(imu);
BoozImuScaleAccel(booz_imu); ImuScaleAccel(imu);
BoozImuScaleMag(booz_imu); ImuScaleMag(imu);
// Copied from booz2_main -- 5143134f060fcc57ce657e17d8b7fc2e72119fd7 // Copied from booz2_main -- 5143134f060fcc57ce657e17d8b7fc2e72119fd7
// mmt 6/15/09 // mmt 6/15/09
+9 -9
View File
@@ -65,21 +65,21 @@ extern int xsens_setzero;
#include "booz_ahrs.h" #include "booz_ahrs.h"
#define PERIODIC_SEND_BOOZ2_GYRO() { \ #define PERIODIC_SEND_BOOZ2_GYRO() { \
DOWNLINK_SEND_BOOZ2_GYRO(&booz_imu.gyro.p, \ DOWNLINK_SEND_BOOZ2_GYRO(&imu.gyro.p, \
&booz_imu.gyro.q, \ &imu.gyro.q, \
&booz_imu.gyro.r); \ &imu.gyro.r); \
} }
#define PERIODIC_SEND_BOOZ2_ACCEL() { \ #define PERIODIC_SEND_BOOZ2_ACCEL() { \
DOWNLINK_SEND_BOOZ2_ACCEL(&booz_imu.accel.x, \ DOWNLINK_SEND_BOOZ2_ACCEL(&imu.accel.x, \
&booz_imu.accel.y, \ &imu.accel.y, \
&booz_imu.accel.z); \ &imu.accel.z); \
} }
#define PERIODIC_SEND_BOOZ2_MAG() { \ #define PERIODIC_SEND_BOOZ2_MAG() { \
DOWNLINK_SEND_BOOZ2_MAG(&booz_imu.mag.x, \ DOWNLINK_SEND_BOOZ2_MAG(&imu.mag.x, \
&booz_imu.mag.y, \ &imu.mag.y, \
&booz_imu.mag.z); \ &imu.mag.z); \
} }
#define PERIODIC_SEND_BOOZ2_AHRS_EULER() { \ #define PERIODIC_SEND_BOOZ2_AHRS_EULER() { \
+10 -10
View File
@@ -21,18 +21,18 @@
* Boston, MA 02111-1307, USA. * Boston, MA 02111-1307, USA.
*/ */
#include "booz_imu.h" #include "imu.h"
#include "airframe.h" #include "airframe.h"
struct BoozImu booz_imu; struct Imu imu;
void booz_imu_init(void) { void imu_init(void) {
/* initialises neutrals */ /* initialises neutrals */
RATES_ASSIGN(booz_imu.gyro_neutral, IMU_GYRO_P_NEUTRAL, IMU_GYRO_Q_NEUTRAL, IMU_GYRO_R_NEUTRAL); RATES_ASSIGN(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(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); VECT3_ASSIGN(imu.mag_neutral, IMU_MAG_X_NEUTRAL, IMU_MAG_Y_NEUTRAL, IMU_MAG_Z_NEUTRAL);
/* /*
Compute quaternion and rotation matrix 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_PHI),
ANGLE_BFP_OF_REAL(IMU_BODY_TO_IMU_THETA), ANGLE_BFP_OF_REAL(IMU_BODY_TO_IMU_THETA),
ANGLE_BFP_OF_REAL(IMU_BODY_TO_IMU_PSI) }; 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_OF_EULERS(imu.body_to_imu_quat, body_to_imu_eulers);
INT32_QUAT_NORMALISE(booz_imu.body_to_imu_quat); INT32_QUAT_NORMALISE(imu.body_to_imu_quat);
INT32_RMAT_OF_EULERS(booz_imu.body_to_imu_rmat, body_to_imu_eulers); INT32_RMAT_OF_EULERS(imu.body_to_imu_rmat, body_to_imu_eulers);
#endif #endif
booz_imu_impl_init(); imu_impl_init();
} }
+10 -10
View File
@@ -28,10 +28,10 @@
#include "math/pprz_algebra_float.h" #include "math/pprz_algebra_float.h"
/* must be defined by underlying hardware */ /* must be defined by underlying hardware */
extern void booz_imu_impl_init(void); extern void imu_impl_init(void);
extern void booz_imu_periodic(void); extern void imu_periodic(void);
struct BoozImu { struct Imu {
struct Int32Rates gyro; struct Int32Rates gyro;
struct Int32Vect3 accel; struct Int32Vect3 accel;
struct Int32Vect3 mag; struct Int32Vect3 mag;
@@ -48,7 +48,7 @@ struct BoozImu {
}; };
/* abstract IMU interface providing floating point interface */ /* abstract IMU interface providing floating point interface */
struct BoozImuFloat { struct ImuFloat {
struct FloatRates gyro; struct FloatRates gyro;
struct FloatVect3 accel; struct FloatVect3 accel;
struct FloatVect3 mag; struct FloatVect3 mag;
@@ -64,11 +64,11 @@ struct BoozImuFloat {
#include BOOZ_IMU_TYPE_H #include BOOZ_IMU_TYPE_H
#endif #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); \ 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.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; \ _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); \ 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.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; \ _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 #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 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; \ 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; \ _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; \ _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 #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.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.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; \ _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; \
@@ -21,9 +21,9 @@
* Boston, MA 02111-1307, USA. * 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)); static void SSP_ISR(void) __attribute__((naked));
#if 0 #if 0
static inline bool_t isr_try_mag(void); 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 SSP_PINSEL1_MOSI (2<<6)
#define BoozImuSetSSP8bits() { \ #define ImuSetSSP8bits() { \
SSPCR0 = SSPCR0_VAL8; \ SSPCR0 = SSPCR0_VAL8; \
} }
#define BoozImuSetSSP16bits() { \ #define ImuSetSSP16bits() { \
SSPCR0 = SSPCR0_VAL16; \ 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) */ /* setup pins for SSP (SCK, MISO, MOSI) */
PINSEL1 |= SSP_PINSEL1_SCK | SSP_PINSEL1_MISO | SSP_PINSEL1_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 // 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 // setup 16 bits
BoozImuSetSSP16bits(); ImuSetSSP16bits();
// read adc // read adc
booz_imu_ssp_status = BOOZ_IMU_SSP_STA_BUSY_MAX1168; imu_ssp_status = IMU_SSP_STA_BUSY_MAX1168;
booz_max1168_read(); booz_max1168_read();
#if defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_AMI601 #if defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_AMI601
RunOnceEvery(10, { ami601_read(); }); RunOnceEvery(10, { ami601_read(); });
@@ -147,36 +147,36 @@ static void SSP_ISR(void) {
static void SSP_ISR(void) { static void SSP_ISR(void) {
ISR_ENTRY(); ISR_ENTRY();
switch (booz_imu_ssp_status) { switch (imu_ssp_status) {
case BOOZ_IMU_SSP_STA_BUSY_MAX1168: case IMU_SSP_STA_BUSY_MAX1168:
Max1168OnSpiInt(); Max1168OnSpiInt();
#if defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_MS2001 #if defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_MS2001
if (ms2001_status == MS2001_IDLE || ms2001_status == MS2001_GOT_EOC) { if (ms2001_status == MS2001_IDLE || ms2001_status == MS2001_GOT_EOC) {
BoozImuSetSSP8bits(); ImuSetSSP8bits();
if (ms2001_status == MS2001_IDLE) { if (ms2001_status == MS2001_IDLE) {
Ms2001SendReq(); Ms2001SendReq();
} }
else { /* MS2001_GOT_EOC */ else { /* MS2001_GOT_EOC */
Ms2001ReadRes(); Ms2001ReadRes();
} }
booz_imu_ssp_status = BOOZ_IMU_SSP_STA_BUSY_MS2100; imu_ssp_status = IMU_SSP_STA_BUSY_MS2100;
} }
else { else {
#endif #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 #if defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_MS2001
} }
#endif #endif
break; break;
#if defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_MS2001 #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(); Ms2001OnSpiIt();
if (ms2001_status == MS2001_IDLE) { if (ms2001_status == MS2001_IDLE) {
Ms2001SendReq(); Ms2001SendReq();
booz_imu_ssp_status = BOOZ_IMU_SSP_STA_BUSY_MS2100; imu_ssp_status = IMU_SSP_STA_BUSY_MS2100;
} }
else else
booz_imu_ssp_status = BOOZ_IMU_SSP_STA_IDLE; imu_ssp_status = IMU_SSP_STA_IDLE;
break; break;
#endif #endif
// default: // default:
@@ -21,8 +21,8 @@
* Boston, MA 02111-1307, USA. * Boston, MA 02111-1307, USA.
*/ */
#ifndef BOOZ_IMU_B2_ARCH_H #ifndef IMU_B2_ARCH_H
#define BOOZ_IMU_B2_ARCH_H #define IMU_B2_ARCH_H
/* /*
@@ -41,12 +41,12 @@
#include "LPC21xx.h" #include "LPC21xx.h"
#include "interrupt_hw.h" #include "interrupt_hw.h"
#define BOOZ_IMU_SSP_STA_IDLE 0 #define IMU_SSP_STA_IDLE 0
#define BOOZ_IMU_SSP_STA_BUSY_MAX1168 1 #define IMU_SSP_STA_BUSY_MAX1168 1
#define BOOZ_IMU_SSP_STA_BUSY_MS2100 2 #define IMU_SSP_STA_BUSY_MS2100 2
extern volatile uint8_t booz_imu_ssp_status; extern volatile uint8_t imu_ssp_status;
#endif /* BOOZ_IMU_B2_ARCH_H */ #endif /* IMU_B2_ARCH_H */
@@ -21,7 +21,7 @@
* Boston, MA 02111-1307, USA. * Boston, MA 02111-1307, USA.
*/ */
#include "booz_imu.h" #include "imu.h"
#include "LPC21xx.h" #include "LPC21xx.h"
#include "armVIC.h" #include "armVIC.h"
@@ -55,7 +55,7 @@
static void SPI1_ISR(void) __attribute__((naked)); static void SPI1_ISR(void) __attribute__((naked));
static uint8_t channel; static uint8_t channel;
void booz_imu_crista_arch_init(void) { void imu_crista_arch_init(void) {
channel = 0; channel = 0;
/* setup pins for SSP (SCK, MISO, MOSI) */ /* setup pins for SSP (SCK, MISO, MOSI) */
@@ -28,7 +28,7 @@
#define BoozImuCristaArchPeriodic() { \ #define ImuCristaArchPeriodic() { \
ADS8344_start(); \ ADS8344_start(); \
} }
@@ -21,21 +21,21 @@
* Boston, MA 02111-1307, USA. * Boston, MA 02111-1307, USA.
*/ */
#include "booz_imu.h" #include "imu.h"
#include "airframe.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" #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_P_CHAN] = sensors.gyro.value.x;
booz_max1168_values[IMU_GYRO_Q_CHAN] = sensors.gyro.value.y; booz_max1168_values[IMU_GYRO_Q_CHAN] = sensors.gyro.value.y;
booz_max1168_values[IMU_GYRO_R_CHAN] = sensors.gyro.value.z; 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 #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_X_CHAN] = sensors.mag.value.x;
ms2001_values[IMU_MAG_Y_CHAN] = sensors.mag.value.y; ms2001_values[IMU_MAG_Y_CHAN] = sensors.mag.value.y;
@@ -30,8 +30,8 @@
#define BOOZ2_IMU_B2_ARCH_H #define BOOZ2_IMU_B2_ARCH_H
extern void booz_imu_feed_gyro_accel(void); extern void imu_feed_gyro_accel(void);
extern void booz_imu_feed_mag(void); extern void imu_feed_mag(void);
#endif /* BOOZ2_IMU_B2_HW_H */ #endif /* BOOZ2_IMU_B2_HW_H */
@@ -2,18 +2,18 @@
* simulator ARCH for rotorcraft imu crista * simulator ARCH for rotorcraft imu crista
*/ */
#include "booz/booz_imu.h" #include "imu.h"
#include "airframe.h" #include "airframe.h"
void booz_imu_crista_arch_init(void) { void imu_crista_arch_init(void) {
} }
#include "nps_sensors.h" #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_P_CHAN] = sensors.gyro.value.x;
ADS8344_values[IMU_GYRO_Q_CHAN] = sensors.gyro.value.y; ADS8344_values[IMU_GYRO_Q_CHAN] = sensors.gyro.value.y;
ADS8344_values[IMU_GYRO_R_CHAN] = sensors.gyro.value.z; ADS8344_values[IMU_GYRO_R_CHAN] = sensors.gyro.value.z;
@@ -23,7 +23,7 @@ void booz_imu_feed_gyro_accel(void) {
ADS8344_available = TRUE; 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_X_CHAN] = sensors.mag.value.x;
ami601_values[IMU_MAG_Y_CHAN] = sensors.mag.value.y; ami601_values[IMU_MAG_Y_CHAN] = sensors.mag.value.y;
ami601_values[IMU_MAG_Z_CHAN] = sensors.mag.value.z; ami601_values[IMU_MAG_Z_CHAN] = sensors.mag.value.z;
@@ -29,13 +29,13 @@
#ifndef BOOZ_IMU_CRISTA_ARCH_H #ifndef BOOZ_IMU_CRISTA_ARCH_H
#define 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 imu_feed_gyro_accel(void);
extern void booz_imu_feed_mag(void); extern void imu_feed_mag(void);
#endif /* BOOZ_IMU_CRISTA_HW_H */ #endif /* BOOZ_IMU_CRISTA_HW_H */
@@ -1,4 +1,4 @@
#include "booz_imu.h" #include "imu.h"
#include <stm32/gpio.h> #include <stm32/gpio.h>
#include <stm32/misc.h> #include <stm32/misc.h>
@@ -21,7 +21,7 @@ void exti2_irq_handler(void);
/* accelerometer dma end of rx handler */ /* accelerometer dma end of rx handler */
void dma1_c4_irq_handler(void); void dma1_c4_irq_handler(void);
void booz_imu_aspirin_arch_init(void) { void imu_aspirin_arch_init(void) {
GPIO_InitTypeDef GPIO_InitStructure; GPIO_InitTypeDef GPIO_InitStructure;
EXTI_InitTypeDef EXTI_InitStructure; EXTI_InitTypeDef EXTI_InitStructure;
@@ -1,11 +1,11 @@
#ifndef BOOZ_IMU_ASPIRIN_ARCH_H #ifndef BOOZ_IMU_ASPIRIN_ARCH_H
#define BOOZ_IMU_ASPIRIN_ARCH_H #define BOOZ_IMU_ASPIRIN_ARCH_H
#include "booz_imu.h" #include "imu.h"
#include "led.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_write_to_reg(uint8_t addr, uint8_t val);
extern void adxl345_clear_rx_buf(void); extern void adxl345_clear_rx_buf(void);
extern void adxl345_start_reading_data(void); extern void adxl345_start_reading_data(void);
@@ -21,7 +21,7 @@
* Boston, MA 02111-1307, USA. * Boston, MA 02111-1307, USA.
*/ */
#include "booz_imu.h" #include "imu.h"
#include <stm32/gpio.h> #include <stm32/gpio.h>
#include <stm32/rcc.h> #include <stm32/rcc.h>
@@ -34,12 +34,12 @@
#define BOOZ_IMU_SSP_STA_BUSY_MAX1168 1 #define BOOZ_IMU_SSP_STA_BUSY_MAX1168 1
#define BOOZ_IMU_SSP_STA_BUSY_MS2100 2 #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 dma1_c4_irq_handler(void);
void spi2_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 -------------------------------------------------*/ /* Enable SPI2 Periph clock -------------------------------------------------*/
RCC_APB1PeriphClockCmd(RCC_APB1Periph_SPI2, ENABLE); RCC_APB1PeriphClockCmd(RCC_APB1Periph_SPI2, ENABLE);
@@ -70,13 +70,13 @@ void booz_imu_b2_arch_init(void) {
}; };
NVIC_Init(&NVIC_init_structure_spi); 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 // check ssp idle
// ASSERT((booz_imu_status == BOOZ_IMU_STA_IDLE), DEBUG_IMU, IMU_ERR_OVERUN); // ASSERT((imu_status == BOOZ_IMU_STA_IDLE), DEBUG_IMU, IMU_ERR_OVERUN);
booz_imu_ssp_status = BOOZ_IMU_SSP_STA_BUSY_MAX1168; imu_ssp_status = BOOZ_IMU_SSP_STA_BUSY_MAX1168;
Max1168ConfigureSPI(); Max1168ConfigureSPI();
SPI_Cmd(SPI2, ENABLE); SPI_Cmd(SPI2, ENABLE);
booz_max1168_read(); booz_max1168_read();
@@ -84,27 +84,27 @@ void booz_imu_periodic(void) {
} }
void dma1_c4_irq_handler(void) { void dma1_c4_irq_handler(void) {
switch (booz_imu_ssp_status) { switch (imu_ssp_status) {
case BOOZ_IMU_SSP_STA_BUSY_MAX1168: case BOOZ_IMU_SSP_STA_BUSY_MAX1168:
Max1168OnDmaIrq(); Max1168OnDmaIrq();
SPI_Cmd(SPI2, DISABLE); SPI_Cmd(SPI2, DISABLE);
if (ms2001_status == MS2001_IDLE) { if (ms2001_status == MS2001_IDLE) {
Ms2001SendReq(); 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()) { else if (ms2001_status == MS2001_WAITING_EOC && Ms2001HasEOC()) {
Ms2001ReadRes(); Ms2001ReadRes();
booz_imu_ssp_status = BOOZ_IMU_SSP_STA_BUSY_MS2100; imu_ssp_status = BOOZ_IMU_SSP_STA_BUSY_MS2100;
} }
else else
booz_imu_ssp_status = BOOZ_IMU_SSP_STA_IDLE; imu_ssp_status = BOOZ_IMU_SSP_STA_IDLE;
break; break;
case BOOZ_IMU_SSP_STA_BUSY_MS2100: case BOOZ_IMU_SSP_STA_BUSY_MS2100:
Ms2001OnDmaIrq(); Ms2001OnDmaIrq();
break; break;
default: default:
// POST_ERROR(DEBUG_IMU, IMU_ERR_SUPRIOUS_DMA1_C4_IRQ); // 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;
} }
} }
@@ -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 <poinix@gmail.com> * Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
* *
* This file is part of paparazzi. * This file is part of paparazzi.
@@ -21,7 +21,7 @@
* Boston, MA 02111-1307, USA. * Boston, MA 02111-1307, USA.
*/ */
#include "booz_imu.h" #include "imu.h"
#include <stm32/gpio.h> #include <stm32/gpio.h>
#include <stm32/rcc.h> #include <stm32/rcc.h>
@@ -44,7 +44,7 @@ static uint8_t buf_out[4];
extern void dma1_c4_irq_handler(void); extern void dma1_c4_irq_handler(void);
static void ADS8344_read_channel( void ); static void ADS8344_read_channel( void );
void booz_imu_crista_arch_init(void) { void imu_crista_arch_init(void) {
channel = 0; channel = 0;
/* Enable SPI2 Periph clock -------------------------------------------------*/ /* Enable SPI2 Periph clock -------------------------------------------------*/
@@ -24,7 +24,7 @@
#define BOOZ_IMU_CRISTA_ARCH_H #define BOOZ_IMU_CRISTA_ARCH_H
#define BoozImuCristaArchPeriodic() { \ #define ImuCristaArchPeriodic() { \
ADS8344_start(); \ ADS8344_start(); \
} }
@@ -1,8 +1,8 @@
#include "booz_imu.h" #include "imu.h"
#include "i2c.h" #include "i2c.h"
struct BoozImuAspirin imu_aspirin; struct ImuAspirin imu_aspirin;
/* initialize peripherals */ /* initialize peripherals */
static void configure_gyro(void); static void configure_gyro(void);
@@ -10,7 +10,7 @@ static void configure_mag(void);
static void configure_accel(void); static void configure_accel(void);
void booz_imu_impl_init(void) { void imu_impl_init(void) {
imu_aspirin.status = AspirinStatusUninit; imu_aspirin.status = AspirinStatusUninit;
imu_aspirin.gyro_available = FALSE; imu_aspirin.gyro_available = FALSE;
@@ -19,12 +19,12 @@ void booz_imu_impl_init(void) {
imu_aspirin.mag_available = FALSE; imu_aspirin.mag_available = FALSE;
imu_aspirin.accel_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) { if (imu_aspirin.status == AspirinStatusUninit) {
configure_gyro(); configure_gyro();
configure_mag(); configure_mag();
@@ -21,11 +21,11 @@
* Boston, MA 02111-1307, USA. * Boston, MA 02111-1307, USA.
*/ */
#ifndef BOOZ_IMU_ASPIRIN_H #ifndef IMU_ASPIRIN_H
#define BOOZ_IMU_ASPIRIN_H #define IMU_ASPIRIN_H
#include "airframe.h" #include "airframe.h"
#include "booz_imu.h" #include "imu.h"
#include "i2c.h" #include "i2c.h"
#include "booz/peripherals/booz_itg3200.h" #include "booz/peripherals/booz_itg3200.h"
@@ -56,7 +56,7 @@ enum AspirinStatus
AspirinStatusReadingMag AspirinStatusReadingMag
}; };
struct BoozImuAspirin { struct ImuAspirin {
volatile enum AspirinStatus status; volatile enum AspirinStatus status;
struct i2c_transaction i2c_trans_gyro; struct i2c_transaction i2c_trans_gyro;
struct i2c_transaction i2c_trans_mag; struct i2c_transaction i2c_trans_mag;
@@ -69,18 +69,18 @@ struct BoozImuAspirin {
volatile uint8_t accel_rx_buf[7]; 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 && \ 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 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 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]; \ 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 ) { \ if (imu_aspirin.mag_ready_for_read ) { \
/* read mag */ \ /* read mag */ \
imu_aspirin.i2c_trans_mag.type = I2CTransRx; \ imu_aspirin.i2c_trans_mag.type = I2CTransRx; \
@@ -91,15 +91,15 @@ extern struct BoozImuAspirin imu_aspirin;
imu_aspirin.status = AspirinStatusReadingMag; \ imu_aspirin.status = AspirinStatusReadingMag; \
} \ } \
else { \ else { \
imu_aspirin.status = AspirinStatusIdle; \ imu_aspirin.status = AspirinStatusIdle; \
} \ } \
} \ } \
if (imu_aspirin.status == AspirinStatusReadingMag && \ 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 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 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]; \ 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.mag_available = TRUE; \
imu_aspirin.status = AspirinStatusIdle; \ 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 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 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); \ 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(); \ _gyro_accel_handler(); \
} \ } \
} }
/* underlying architecture */ /* underlying architecture */
#include "imu/booz_imu_aspirin_arch.h" #include "imu_aspirin_arch.h"
/* must be implemented by underlying architecture */ /* 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 */
@@ -21,11 +21,11 @@
* Boston, MA 02111-1307, USA. * 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(); booz_max1168_init();
#if defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_MS2001 #if defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_MS2001
+23 -23
View File
@@ -21,10 +21,10 @@
* Boston, MA 02111-1307, USA. * Boston, MA 02111-1307, USA.
*/ */
#ifndef BOOZ_IMU_B2_H #ifndef IMU_B2_H
#define BOOZ_IMU_B2_H #define IMU_B2_H
#include "booz_imu.h" #include "imu.h"
#include "airframe.h" #include "airframe.h"
#include "peripherals/booz_max1168.h" #include "peripherals/booz_max1168.h"
@@ -112,11 +112,11 @@
#if defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_MS2001 #if defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_MS2001
#include "peripherals/booz_ms2001.h" #include "peripherals/booz_ms2001.h"
#define BoozImuMagEvent(_mag_handler) { \ #define ImuMagEvent(_mag_handler) { \
if (ms2001_status == MS2001_DATA_AVAILABLE) { \ if (ms2001_status == MS2001_DATA_AVAILABLE) { \
booz_imu.mag_unscaled.x = ms2001_values[IMU_MAG_X_CHAN]; \ imu.mag_unscaled.x = ms2001_values[IMU_MAG_X_CHAN]; \
booz_imu.mag_unscaled.y = ms2001_values[IMU_MAG_Y_CHAN]; \ imu.mag_unscaled.y = ms2001_values[IMU_MAG_Y_CHAN]; \
booz_imu.mag_unscaled.z = ms2001_values[IMU_MAG_Z_CHAN]; \ imu.mag_unscaled.z = ms2001_values[IMU_MAG_Z_CHAN]; \
ms2001_status = MS2001_IDLE; \ ms2001_status = MS2001_IDLE; \
_mag_handler(); \ _mag_handler(); \
} \ } \
@@ -124,40 +124,40 @@
#elif defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_AMI601 #elif defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_AMI601
#include "peripherals/booz_ami601.h" #include "peripherals/booz_ami601.h"
#define foo_handler() {} #define foo_handler() {}
#define BoozImuMagEvent(_mag_handler) { \ #define ImuMagEvent(_mag_handler) { \
AMI601Event(foo_handler); \ AMI601Event(foo_handler); \
if (ami601_status == AMI601_DATA_AVAILABLE) { \ if (ami601_status == AMI601_DATA_AVAILABLE) { \
booz_imu.mag_unscaled.x = ami601_values[IMU_MAG_X_CHAN]; \ imu.mag_unscaled.x = ami601_values[IMU_MAG_X_CHAN]; \
booz_imu.mag_unscaled.y = ami601_values[IMU_MAG_Y_CHAN]; \ imu.mag_unscaled.y = ami601_values[IMU_MAG_Y_CHAN]; \
booz_imu.mag_unscaled.z = ami601_values[IMU_MAG_Z_CHAN]; \ imu.mag_unscaled.z = ami601_values[IMU_MAG_Z_CHAN]; \
ami601_status = AMI601_IDLE; \ ami601_status = AMI601_IDLE; \
_mag_handler(); \ _mag_handler(); \
} \ } \
} }
#else #else
#define BoozImuMagEvent(_mag_handler) {} #define ImuMagEvent(_mag_handler) {}
#endif #endif
#define BoozImuEvent(_gyro_accel_handler, _mag_handler) { \ #define ImuEvent(_gyro_accel_handler, _mag_handler) { \
if (booz_max1168_status == STA_MAX1168_DATA_AVAILABLE) { \ if (booz_max1168_status == STA_MAX1168_DATA_AVAILABLE) { \
booz_imu.gyro_unscaled.p = booz_max1168_values[IMU_GYRO_P_CHAN]; \ imu.gyro_unscaled.p = booz_max1168_values[IMU_GYRO_P_CHAN]; \
booz_imu.gyro_unscaled.q = booz_max1168_values[IMU_GYRO_Q_CHAN]; \ imu.gyro_unscaled.q = booz_max1168_values[IMU_GYRO_Q_CHAN]; \
booz_imu.gyro_unscaled.r = booz_max1168_values[IMU_GYRO_R_CHAN]; \ imu.gyro_unscaled.r = booz_max1168_values[IMU_GYRO_R_CHAN]; \
booz_imu.accel_unscaled.x = booz_max1168_values[IMU_ACCEL_X_CHAN]; \ imu.accel_unscaled.x = booz_max1168_values[IMU_ACCEL_X_CHAN]; \
booz_imu.accel_unscaled.y = booz_max1168_values[IMU_ACCEL_Y_CHAN]; \ imu.accel_unscaled.y = booz_max1168_values[IMU_ACCEL_Y_CHAN]; \
booz_imu.accel_unscaled.z = booz_max1168_values[IMU_ACCEL_Z_CHAN]; \ imu.accel_unscaled.z = booz_max1168_values[IMU_ACCEL_Z_CHAN]; \
booz_max1168_status = STA_MAX1168_IDLE; \ booz_max1168_status = STA_MAX1168_IDLE; \
_gyro_accel_handler(); \ _gyro_accel_handler(); \
} \ } \
BoozImuMagEvent(_mag_handler); \ ImuMagEvent(_mag_handler); \
} }
/* underlying architecture */ /* underlying architecture */
#include "imu/booz_imu_b2_arch.h" #include "imu_b2_arch.h"
/* must be implemented by underlying architecture */ /* 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 */
@@ -21,16 +21,16 @@
* Boston, MA 02111-1307, USA. * Boston, MA 02111-1307, USA.
*/ */
#include "booz_imu.h" #include "imu.h"
volatile bool_t ADS8344_available; volatile bool_t ADS8344_available;
uint16_t ADS8344_values[ADS8344_NB_CHANNELS]; uint16_t ADS8344_values[ADS8344_NB_CHANNELS];
void booz_imu_impl_init(void) { void imu_impl_init(void) {
ADS8344_available = FALSE; ADS8344_available = FALSE;
booz_imu_crista_arch_init(); imu_crista_arch_init();
#ifdef USE_AMI601 #ifdef USE_AMI601
ami601_init(); 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 #ifdef USE_AMI601
RunOnceEvery(10, { ami601_read(); }); RunOnceEvery(10, { ami601_read(); });
#endif #endif
@@ -21,53 +21,53 @@
* Boston, MA 02111-1307, USA. * Boston, MA 02111-1307, USA.
*/ */
#ifndef BOOZ_IMU_CRISTA_H #ifndef IMU_CRISTA_H
#define BOOZ_IMU_CRISTA_H #define IMU_CRISTA_H
#include "booz_imu.h" #include "imu.h"
#include "airframe.h" #include "airframe.h"
#define ADS8344_NB_CHANNELS 8 #define ADS8344_NB_CHANNELS 8
extern uint16_t ADS8344_values[ADS8344_NB_CHANNELS]; extern uint16_t ADS8344_values[ADS8344_NB_CHANNELS];
extern volatile bool_t ADS8344_available; extern volatile bool_t ADS8344_available;
#define BoozImuEvent(_gyro_accel_handler, _mag_handler) { \ #define ImuEvent(_gyro_accel_handler, _mag_handler) { \
if (ADS8344_available) { \ if (ADS8344_available) { \
ADS8344_available = FALSE; \ ADS8344_available = FALSE; \
booz_imu.gyro_unscaled.p = ADS8344_values[IMU_GYRO_P_CHAN]; \ imu.gyro_unscaled.p = ADS8344_values[IMU_GYRO_P_CHAN]; \
booz_imu.gyro_unscaled.q = ADS8344_values[IMU_GYRO_Q_CHAN]; \ imu.gyro_unscaled.q = ADS8344_values[IMU_GYRO_Q_CHAN]; \
booz_imu.gyro_unscaled.r = ADS8344_values[IMU_GYRO_R_CHAN]; \ imu.gyro_unscaled.r = ADS8344_values[IMU_GYRO_R_CHAN]; \
booz_imu.accel_unscaled.x = ADS8344_values[IMU_ACCEL_X_CHAN]; \ imu.accel_unscaled.x = ADS8344_values[IMU_ACCEL_X_CHAN]; \
booz_imu.accel_unscaled.y = ADS8344_values[IMU_ACCEL_Y_CHAN]; \ imu.accel_unscaled.y = ADS8344_values[IMU_ACCEL_Y_CHAN]; \
booz_imu.accel_unscaled.z = ADS8344_values[IMU_ACCEL_Z_CHAN]; \ imu.accel_unscaled.z = ADS8344_values[IMU_ACCEL_Z_CHAN]; \
/* spare 3, temp 7 */ \ /* spare 3, temp 7 */ \
_gyro_accel_handler(); \ _gyro_accel_handler(); \
} \ } \
BoozImuMagEvent(_mag_handler); \ ImuMagEvent(_mag_handler); \
} }
#ifdef USE_AMI601 #ifdef USE_AMI601
#include "peripherals/booz_ami601.h" #include "peripherals/booz_ami601.h"
#define foo_handler() {} #define foo_handler() {}
#define BoozImuMagEvent(_mag_handler) { \ #define ImuMagEvent(_mag_handler) { \
AMI601Event(foo_handler); \ AMI601Event(foo_handler); \
if (ami601_status == AMI601_DATA_AVAILABLE) { \ if (ami601_status == AMI601_DATA_AVAILABLE) { \
booz_imu.mag_unscaled.x = ami601_values[IMU_MAG_X_CHAN]; \ imu.mag_unscaled.x = ami601_values[IMU_MAG_X_CHAN]; \
booz_imu.mag_unscaled.y = ami601_values[IMU_MAG_Y_CHAN]; \ imu.mag_unscaled.y = ami601_values[IMU_MAG_Y_CHAN]; \
booz_imu.mag_unscaled.z = ami601_values[IMU_MAG_Z_CHAN]; \ imu.mag_unscaled.z = ami601_values[IMU_MAG_Z_CHAN]; \
ami601_status = AMI601_IDLE; \ ami601_status = AMI601_IDLE; \
_mag_handler(); \ _mag_handler(); \
} \ } \
} }
#else #else
#define BoozImuMagEvent(_mag_handler) {} #define ImuMagEvent(_mag_handler) {}
#endif #endif
/* underlying architecture */ /* underlying architecture */
#include "imu/booz_imu_crista_arch.h" #include "imu_crista_arch.h"
/* must be defined by underlying architecture */ /* 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 */
+7 -7
View File
@@ -38,7 +38,7 @@
#include "booz_actuators.h" #include "booz_actuators.h"
#include "booz_radio_control.h" #include "booz_radio_control.h"
#include "booz_imu.h" #include "imu.h"
#include "booz_gps.h" #include "booz_gps.h"
#include "booz/booz2_analog.h" #include "booz/booz2_analog.h"
@@ -110,7 +110,7 @@ STATIC_INLINE void booz2_main_init( void ) {
#endif #endif
booz2_battery_init(); booz2_battery_init();
booz_imu_init(); imu_init();
booz_fms_init(); booz_fms_init();
autopilot_init(); autopilot_init();
@@ -137,7 +137,7 @@ STATIC_INLINE void booz2_main_init( void ) {
STATIC_INLINE void booz2_main_periodic( void ) { STATIC_INLINE void booz2_main_periodic( void ) {
booz_imu_periodic(); imu_periodic();
/* run control loops */ /* run control loops */
autopilot_periodic(); autopilot_periodic();
@@ -199,7 +199,7 @@ STATIC_INLINE void booz2_main_event( void ) {
RadioControlEvent(autopilot_on_rc_frame); 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); 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 ) { static inline void on_gyro_accel_event( void ) {
BoozImuScaleGyro(booz_imu); ImuScaleGyro(imu);
BoozImuScaleAccel(booz_imu); ImuScaleAccel(imu);
if (booz_ahrs.status == BOOZ_AHRS_UNINIT) { if (booz_ahrs.status == BOOZ_AHRS_UNINIT) {
booz_ahrs_aligner_run(); booz_ahrs_aligner_run();
@@ -248,7 +248,7 @@ static inline void on_gps_event(void) {
} }
static inline void on_mag_event(void) { static inline void on_mag_event(void) {
BoozImuScaleMag(booz_imu); ImuScaleMag(imu);
if (booz_ahrs.status == BOOZ_AHRS_RUNNING) if (booz_ahrs.status == BOOZ_AHRS_RUNNING)
booz_ahrs_update_mag(); booz_ahrs_update_mag();
} }
+3 -3
View File
@@ -42,11 +42,11 @@
#include "airframe.h" #include "airframe.h"
#include "actuators.h" #include "actuators.h"
#include "rdyb_booz_imu.h" #include "rdyb_imu.h"
#include "booz_radio_control.h" #include "booz_radio_control.h"
#include "rdyb_mahrs.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 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; 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); imu.mag.z = MAG_FLOAT_OF_BFP(msg_up->mag.z);
if (msg_up->valid.imu) if (msg_up->valid.imu)
rdyb_booz_imu_update(&imu); rdyb_imu_update(&imu);
} }
static void passthrough_down_fill(struct AutopilotMessagePTDown *msg_down) static void passthrough_down_fill(struct AutopilotMessagePTDown *msg_down)
+2 -2
View File
@@ -2,14 +2,14 @@
#define OVERO_TEST_PASSTHROUGH_H #define OVERO_TEST_PASSTHROUGH_H
#include "std.h" #include "std.h"
#include "booz/booz_imu.h" #include "imu.h"
struct OveroTestPassthrough { struct OveroTestPassthrough {
/* our network connection */ /* our network connection */
char* gs_gw; char* gs_gw;
/* our sensors */ /* our sensors */
struct BoozImuFloat imu; struct ImuFloat imu;
uint8_t rc_status; uint8_t rc_status;
int32_t baro_abs; int32_t baro_abs;
int32_t baro_diff; int32_t baro_diff;

Some files were not shown because too many files have changed in this diff Show More