[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[paparazzi-commits] [5316] added the imu structure as a parameter to Boo
|
From: |
antoine drouin |
|
Subject: |
[paparazzi-commits] [5316] added the imu structure as a parameter to BoozImuScalexxx macros |
|
Date: |
Wed, 11 Aug 2010 19:07:27 +0000 |
Revision: 5316
http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=5316
Author: poine
Date: 2010-08-11 19:07:26 +0000 (Wed, 11 Aug 2010)
Log Message:
-----------
added the imu structure as a parameter to BoozImuScalexxx macros
Modified Paths:
--------------
paparazzi3/trunk/sw/airborne/beth/main_overo.c
paparazzi3/trunk/sw/airborne/beth/main_stm32.c
paparazzi3/trunk/sw/airborne/booz/booz2_main.c
paparazzi3/trunk/sw/airborne/booz/booz_imu.h
paparazzi3/trunk/sw/airborne/booz/test/booz_test_imu.c
paparazzi3/trunk/sw/airborne/csc/mercury_xsens.c
paparazzi3/trunk/sw/airborne/lisa/lisa_stm_passthrough_main.c
Modified: paparazzi3/trunk/sw/airborne/beth/main_overo.c
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/main_overo.c 2010-08-11 18:46:33 UTC
(rev 5315)
+++ paparazzi3/trunk/sw/airborne/beth/main_overo.c 2010-08-11 19:07:26 UTC
(rev 5316)
@@ -115,7 +115,7 @@
main_talk_with_stm32();
- BoozImuScaleGyro();
+ BoozImuScaleGyro(booz_imu);
RunOnceEvery(50,
{DOWNLINK_SEND_BETH(gcs_com.udp_transport,&msg_in.bench_sensor.x,&msg_in.bench_sensor.y,
&msg_in.bench_sensor.z,&foo);});
Modified: paparazzi3/trunk/sw/airborne/beth/main_stm32.c
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/main_stm32.c 2010-08-11 18:46:33 UTC
(rev 5315)
+++ paparazzi3/trunk/sw/airborne/beth/main_stm32.c 2010-08-11 19:07:26 UTC
(rev 5316)
@@ -154,8 +154,8 @@
static inline void on_gyro_accel_event(void) {
- BoozImuScaleGyro();
- BoozImuScaleAccel();
+ BoozImuScaleGyro(booz_imu);
+ BoozImuScaleAccel(booz_imu);
LED_TOGGLE(2);
static uint8_t cnt;
Modified: paparazzi3/trunk/sw/airborne/booz/booz2_main.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_main.c 2010-08-11 18:46:33 UTC
(rev 5315)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_main.c 2010-08-11 19:07:26 UTC
(rev 5316)
@@ -223,8 +223,8 @@
static inline void on_gyro_accel_event( void ) {
- BoozImuScaleGyro();
- BoozImuScaleAccel();
+ BoozImuScaleGyro(booz_imu);
+ BoozImuScaleAccel(booz_imu);
if (booz_ahrs.status == BOOZ_AHRS_UNINIT) {
booz_ahrs_aligner_run();
@@ -251,7 +251,7 @@
}
static inline void on_mag_event(void) {
- BoozImuScaleMag();
+ BoozImuScaleMag(booz_imu);
if (booz_ahrs.status == BOOZ_AHRS_RUNNING)
booz_ahrs_update_mag();
}
Modified: paparazzi3/trunk/sw/airborne/booz/booz_imu.h
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz_imu.h 2010-08-11 18:46:33 UTC
(rev 5315)
+++ paparazzi3/trunk/sw/airborne/booz/booz_imu.h 2010-08-11 19:07:26 UTC
(rev 5316)
@@ -68,34 +68,34 @@
extern void booz_imu_init(void);
-#define BoozImuScaleGyro() { \
- RATES_COPY(booz_imu.gyro_prev, booz_imu.gyro); \
- booz_imu.gyro.p = ((booz_imu.gyro_unscaled.p -
booz_imu.gyro_neutral.p)*IMU_GYRO_P_SIGN*IMU_GYRO_P_SENS_NUM)/IMU_GYRO_P_SENS_DEN;
\
- booz_imu.gyro.q = ((booz_imu.gyro_unscaled.q -
booz_imu.gyro_neutral.q)*IMU_GYRO_Q_SIGN*IMU_GYRO_Q_SENS_NUM)/IMU_GYRO_Q_SENS_DEN;
\
- booz_imu.gyro.r = ((booz_imu.gyro_unscaled.r -
booz_imu.gyro_neutral.r)*IMU_GYRO_R_SIGN*IMU_GYRO_R_SENS_NUM)/IMU_GYRO_R_SENS_DEN;
\
+#define BoozImuScaleGyro(_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; \
+ _imu.gyro.r = ((_imu.gyro_unscaled.r -
_imu.gyro_neutral.r)*IMU_GYRO_R_SIGN*IMU_GYRO_R_SENS_NUM)/IMU_GYRO_R_SENS_DEN; \
}
-#define BoozImuScaleAccel() { \
- VECT3_COPY(booz_imu.accel_prev, booz_imu.accel); \
- booz_imu.accel.x = ((booz_imu.accel_unscaled.x -
booz_imu.accel_neutral.x)*IMU_ACCEL_X_SIGN*IMU_ACCEL_X_SENS_NUM)/IMU_ACCEL_X_SENS_DEN;
\
- booz_imu.accel.y = ((booz_imu.accel_unscaled.y -
booz_imu.accel_neutral.y)*IMU_ACCEL_Y_SIGN*IMU_ACCEL_Y_SENS_NUM)/IMU_ACCEL_Y_SENS_DEN;
\
- booz_imu.accel.z = ((booz_imu.accel_unscaled.z -
booz_imu.accel_neutral.z)*IMU_ACCEL_Z_SIGN*IMU_ACCEL_Z_SENS_NUM)/IMU_ACCEL_Z_SENS_DEN;
\
+#define BoozImuScaleAccel(_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;
\
+ _imu.accel.z = ((_imu.accel_unscaled.z -
_imu.accel_neutral.z)*IMU_ACCEL_Z_SIGN*IMU_ACCEL_Z_SENS_NUM)/IMU_ACCEL_Z_SENS_DEN;
\
}
#if defined IMU_MAG_45_HACK
-#define BoozImuScaleMag() { \
- int32_t msx = ((booz_imu.mag_unscaled.x - booz_imu.mag_neutral.x) *
IMU_MAG_X_SIGN * IMU_MAG_X_SENS_NUM) / IMU_MAG_X_SENS_DEN; \
- int32_t msy = ((booz_imu.mag_unscaled.y - booz_imu.mag_neutral.y) *
IMU_MAG_Y_SIGN * IMU_MAG_Y_SENS_NUM) / IMU_MAG_Y_SENS_DEN; \
- booz_imu.mag.x = msx - msy; \
- booz_imu.mag.y = msx + msy; \
- booz_imu.mag.z = ((booz_imu.mag_unscaled.z - booz_imu.mag_neutral.z) *
IMU_MAG_Z_SIGN * IMU_MAG_Z_SENS_NUM) / IMU_MAG_Z_SENS_DEN; \
+#define BoozImuScaleMag(_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; \
+ _imu.mag.y = msx + msy; \
+ _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() { \
- booz_imu.mag.x = ((booz_imu.mag_unscaled.x - booz_imu.mag_neutral.x) *
IMU_MAG_X_SIGN * IMU_MAG_X_SENS_NUM) / IMU_MAG_X_SENS_DEN; \
- booz_imu.mag.y = ((booz_imu.mag_unscaled.y - booz_imu.mag_neutral.y) *
IMU_MAG_Y_SIGN * IMU_MAG_Y_SENS_NUM) / IMU_MAG_Y_SENS_DEN; \
- booz_imu.mag.z = ((booz_imu.mag_unscaled.z - booz_imu.mag_neutral.z) *
IMU_MAG_Z_SIGN * IMU_MAG_Z_SENS_NUM) / IMU_MAG_Z_SENS_DEN; \
+#define BoozImuScaleMag(_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; \
}
#endif
Modified: paparazzi3/trunk/sw/airborne/booz/test/booz_test_imu.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/test/booz_test_imu.c 2010-08-11
18:46:33 UTC (rev 5315)
+++ paparazzi3/trunk/sw/airborne/booz/test/booz_test_imu.c 2010-08-11
19:07:26 UTC (rev 5316)
@@ -35,6 +35,7 @@
#include "interrupt_hw.h"
+#include "my_debug_servo.h"
static inline void main_init( void );
static inline void main_periodic_task( void );
@@ -59,6 +60,10 @@
sys_time_init();
booz_imu_init();
+ DEBUG_SERVO1_INIT();
+ DEBUG_SERVO2_INIT();
+
+
int_enable();
}
@@ -67,14 +72,20 @@
LED_TOGGLE(3);
DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM);
});
+#ifdef USE_I2C2
RunOnceEvery(111, {
DOWNLINK_SEND_I2C_ERRORS(DefaultChannel,
- &i2c2.got_unexpected_event,
- &i2c2.errc_ack_fail, &i2c2.errc_miss_start_stop,
- &i2c2.errc_arb_lost, &i2c2.errc_over_under,
- &i2c2.errc_pec_recep, &i2c2.errc_timeout_tlow,
- &i2c2.errc_smbus_alert);
+ &i2c2_errors.ack_fail_cnt,
+ &i2c2_errors.miss_start_stop_cnt,
+ &i2c2_errors.arb_lost_cnt,
+ &i2c2_errors.over_under_cnt,
+ &i2c2_errors.pec_recep_cnt,
+ &i2c2_errors.timeout_tlow_cnt,
+ &i2c2_errors.smbus_alert_cnt,
+ &i2c2_errors.unexpected_event_cnt,
+ &i2c2_errors.last_unexpected_event);
});
+#endif
booz_imu_periodic();
RunOnceEvery(10, { LED_PERIODIC();});
}
@@ -86,8 +97,8 @@
}
static inline void on_gyro_accel_event(void) {
- BoozImuScaleGyro();
- BoozImuScaleAccel();
+ BoozImuScaleGyro(booz_imu);
+ BoozImuScaleAccel(booz_imu);
LED_TOGGLE(2);
static uint8_t cnt;
Modified: paparazzi3/trunk/sw/airborne/csc/mercury_xsens.c
===================================================================
--- paparazzi3/trunk/sw/airborne/csc/mercury_xsens.c 2010-08-11 18:46:33 UTC
(rev 5315)
+++ paparazzi3/trunk/sw/airborne/csc/mercury_xsens.c 2010-08-11 19:07:26 UTC
(rev 5316)
@@ -310,9 +310,9 @@
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();
- BoozImuScaleAccel();
- BoozImuScaleMag();
+ BoozImuScaleGyro(booz_imu);
+ BoozImuScaleAccel(booz_imu);
+ BoozImuScaleMag(booz_imu);
// Copied from booz2_main -- 5143134f060fcc57ce657e17d8b7fc2e72119fd7
// mmt 6/15/09
Modified: paparazzi3/trunk/sw/airborne/lisa/lisa_stm_passthrough_main.c
===================================================================
--- paparazzi3/trunk/sw/airborne/lisa/lisa_stm_passthrough_main.c
2010-08-11 18:46:33 UTC (rev 5315)
+++ paparazzi3/trunk/sw/airborne/lisa/lisa_stm_passthrough_main.c
2010-08-11 19:07:26 UTC (rev 5316)
@@ -29,7 +29,7 @@
#include "booz/booz_imu.h"
#include "booz_radio_control.h"
#include "actuators/booz_actuators_pwm.h"
-#include "lisa/lisa_overo_link_crc.h"
+#include "lisa/lisa_overo_link.h"
static inline void main_init(void);
static inline void main_periodic(void);
@@ -38,28 +38,27 @@
static inline void on_gyro_accel_event(void);
static inline void on_mag_event(void);
-static inline void main_on_overo_msg_received(void);
-static inline void main_on_overo_link_lost(void);
+static inline void on_overo_link_msg_received(void);
+static inline void on_overo_link_lost(void);
+static inline void on_overo_link_crc_failed(void);
+static inline void on_rc_message(void);
+
static bool_t new_radio_msg;
int main(void) {
- main_init();
+ main_init();
- while (1) {
- if (sys_time_periodic())
- main_periodic();
- main_event();
- }
+ while (1) {
+ if (sys_time_periodic())
+ main_periodic();
+ main_event();
+ }
- return 0;
+ return 0;
}
-static void on_rc_message(void) {
- new_radio_msg = TRUE;
-}
-
static inline void main_init(void) {
hw_init();
@@ -68,24 +67,35 @@
radio_control_init();
booz_actuators_pwm_hw_init();
overo_link_init();
+ new_radio_msg = FALSE;
}
static inline void main_periodic(void) {
- booz_imu_periodic();
- OveroLinkPeriodic(main_on_overo_link_lost);
- RunOnceEvery(10, {LED_PERIODIC(); DOWNLINK_SEND_ALIVE(DefaultChannel,
16, MD5SUM);radio_control_periodic();});
+ booz_imu_periodic();
+ OveroLinkPeriodic(on_overo_link_lost);
+ RunOnceEvery(10, {
+ LED_PERIODIC();
+ DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM);
+ radio_control_periodic();
+ });
+
}
static inline void main_event(void) {
BoozImuEvent(on_gyro_accel_event, on_mag_event);
- OveroLinkEvent(main_on_overo_msg_received, main_on_overo_link_lost);
+ OveroLinkEvent(on_overo_link_msg_received, on_overo_link_crc_failed);
RadioControlEvent(on_rc_message);
+
}
-static inline void main_on_overo_msg_received(void) {
+static inline void on_rc_message(void) {
+ new_radio_msg = TRUE;
+}
+static inline void on_overo_link_msg_received(void) {
+
if (new_radio_msg) overo_link.up.msg.valid.rc = 1;
else overo_link.up.msg.valid.rc = 0;
new_radio_msg = FALSE;
@@ -114,59 +124,21 @@
booz_actuators_pwm_commit();
}
-static inline void main_on_overo_link_lost(void) {
+static inline void on_overo_link_lost(void) {
+
+}
+static inline void on_overo_link_crc_failed(void) {
+
}
-static inline void on_gyro_accel_event(void) {
- static uint8_t cnt;
- BoozImuScaleGyro();
- BoozImuScaleAccel();
-
- LED_TOGGLE(2);
- cnt++;
- if (cnt > 15) cnt = 0;
-
- if (cnt == 0) {
- DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel,
- &booz_imu.gyro_unscaled.p,
- &booz_imu.gyro_unscaled.q,
- &booz_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);
- } else if (cnt == 7) {
- DOWNLINK_SEND_BOOZ2_GYRO(DefaultChannel,
- &booz_imu.gyro.p,
- &booz_imu.gyro.q,
- &booz_imu.gyro.r);
-
- DOWNLINK_SEND_BOOZ2_ACCEL(DefaultChannel,
- &booz_imu.accel.x,
- &booz_imu.accel.y,
- &booz_imu.accel.z);
- }
+static inline void on_gyro_accel_event(void) {
+ BoozImuScaleGyro(booz_imu);
+ BoozImuScaleAccel(booz_imu);
}
static inline void on_mag_event(void) {
- static uint8_t cnt;
-
- BoozImuScaleMag();
-
- cnt++;
- if (cnt % 2) {
- DOWNLINK_SEND_BOOZ2_MAG(DefaultChannel,
- &booz_imu.mag.x,
- &booz_imu.mag.y,
- &booz_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);
- }
+ BoozImuScaleMag(booz_imu);
}
| [Prev in Thread] |
Current Thread |
[Next in Thread] |
- [paparazzi-commits] [5316] added the imu structure as a parameter to BoozImuScalexxx macros,
antoine drouin <=