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

rename booz_imu to imu, fix imu makefiles

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

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