mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-06-05 23:49:00 +08:00
rename booz_imu to imu, fix imu makefiles
This commit is contained in:
@@ -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
|
||||||
|
|||||||
@@ -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 ) {}
|
||||||
|
|
||||||
|
|||||||
@@ -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,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);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -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);
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -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);
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -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
|
||||||
|
|||||||
@@ -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);
|
||||||
|
|
||||||
|
|||||||
@@ -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
|
||||||
|
|||||||
@@ -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++) {
|
||||||
|
|||||||
@@ -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);
|
||||||
|
|||||||
@@ -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); \
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -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;
|
||||||
|
|||||||
@@ -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);
|
||||||
|
|||||||
@@ -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);
|
||||||
|
|||||||
@@ -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"
|
||||||
|
|
||||||
|
|||||||
@@ -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);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -1,3 +1,3 @@
|
|||||||
#include "booz_imu.h"
|
#include "imu.h"
|
||||||
|
|
||||||
void booz_imu_impl_init(void) {}
|
void imu_impl_init(void) {}
|
||||||
|
|||||||
@@ -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);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
@@ -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();
|
||||||
|
|||||||
@@ -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
|
||||||
|
|||||||
@@ -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() { \
|
||||||
|
|||||||
@@ -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();
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -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
|
||||||
|
|||||||
@@ -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 */
|
||||||
|
|||||||
@@ -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();
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -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,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
Reference in New Issue
Block a user