Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-06 07:53:43 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

*** empty log message ***

Browse Source
This commit is contained in:
Antoine Drouin
2007-10-16 17:08:03 +00:00
parent 4ba7c225b6
commit b09f24a9f7
12 changed files with 102 additions and 52 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/booz.xml
+5 -1
View File
@@ -63,7 +63,7 @@ ctl.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B57600
ctl.srcs += $(SRC_ARCH)/uart_hw.c
ctl.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart1
ctl.srcs += downlink.c pprz_transport.c
ctl.srcs += downlink.c pprz_transport.c
ctl.CFLAGS += -DDATALINK=PPRZ -DPPRZ_UART=Uart1
@@ -76,6 +76,10 @@ ctl.srcs += booz_estimator.c booz_control.c
ctl.CFLAGS += -DRADIO_CONTROL -DRADIO_CONTROL_TYPE=RC_FUTABA
ctl.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
ctl.srcs += commands.c
ctl.srcs += booz_telemetry.c
#
# AHRS CPU
#
conf/messages.xml
+3
View File
@@ -432,6 +432,9 @@
<message name="BOOZ_STATUS" ID="215">
<field name="link_imu_nb_err" type="uint32"/>
<field name="imu_status" type="uint8"/>
<field name="rc_status" type="uint8" values="OK|LOST|REALLY_LOST"/>
<!-- <field name="mode" type="uint8" values="MANUAL|AUTO1|AUTO2|FAILSAFE"/> -->
<!-- <field name="vsupply" type="uint8" unit="decivolt"/> -->
</message>
<message name="ENOSE_STATUS" ID="218">
conf/telemetry/booz.xml
+5 -7
View File
@@ -1,16 +1,14 @@
<?xml version="1.0"?>
<!DOCTYPE telemetry SYSTEM "telemetry.dtd">
<telemetry>
<process name="Ap">
<mode name="default">
</mode>
<mode name="debug">
</mode>
</process>
<process name="Fbw">
<process name="Main">
<mode name="default">
<message name="BOOZ_STATUS" period="1."/>
</mode>
<mode name="debug">
</mode>
</process>
</telemetry>
sw/airborne/booz_control.c
+1
View File
@@ -2,6 +2,7 @@
#include "booz_estimator.h"
#include "commands.h"
#include "radio_control.h"
float booz_control_p_sp;
float booz_control_q_sp;
sw/airborne/booz_downlink.h
+2 -2
View File
@@ -2,10 +2,10 @@
#define BOOZ_DOWNLINK_H
#include "std.h"
#include "periodic.h"
static inline void booz_downlink_periodic_task(void) {
PeriodicSendMain()
}
sw/airborne/imu_v3.c
+2
View File
@@ -27,6 +27,7 @@ struct adc_buf buf_bat;
#define IMU_DETECT_STILL_LEN 128
bool_t imu_vehicle_still;
float imu_vs_gyro_initial_bias[AXIS_NB];
uint16_t imu_vs_accel_raw[AXIS_NB][IMU_DETECT_STILL_LEN];
uint32_t imu_vs_accel_raw_sum[AXIS_NB];
@@ -64,6 +65,7 @@ void imu_init(void) {
imu_vs_accel_raw_sum[i] = 0;
imu_vs_gyro_raw_sum[i] = 0;
imu_vs_mag_raw_sum[i] = 0;
imu_vs_gyro_initial_bias[i] = 0.;
for (j=0; j<IMU_DETECT_STILL_LEN; j++) {
imu_vs_accel_raw[i][j] = 0;
imu_vs_gyro_raw[i][j] = 0;
sw/airborne/imu_v3.h
+25 -21
View File
@@ -20,6 +20,7 @@ extern int16_t imu_mag_raw[AXIS_NB];
extern void imu_init(void);
extern void imu_detect_vehicle_still(void);
extern bool_t imu_vehicle_still;
extern float imu_vs_gyro_initial_bias[AXIS_NB];
/* sliding average for still vehicle detection */
extern uint32_t imu_vs_accel_raw_avg[AXIS_NB];
@@ -51,13 +52,13 @@ extern struct adc_buf buf_bat;
imu_accel[AXIS_Z]= IMU_ACCEL_Y_GAIN * (buf_az.sum - IMU_ACCEL_Z_NEUTRAL); \
}
#define IMU_GYRO_X_NEUTRAL 40885
#define IMU_GYRO_Y_NEUTRAL 40910
#define IMU_GYRO_Z_NEUTRAL 39552
#define IMU_GYRO_P_NEUTRAL 40885
#define IMU_GYRO_Q_NEUTRAL 40910
#define IMU_GYRO_R_NEUTRAL 39552
#define IMU_GYRO_X_GAIN -0.0002202
#define IMU_GYRO_Y_GAIN -0.0002160
#define IMU_GYRO_Z_GAIN 0.0002104
#define IMU_GYRO_P_GAIN -0.0002202
#define IMU_GYRO_Q_GAIN -0.0002150
#define IMU_GYRO_R_GAIN 0.0002104
#define ImuUpdateGyros() { \
imu_gyro_prev[AXIS_X] = imu_gyro[AXIS_X]; \
@@ -66,12 +67,12 @@ extern struct adc_buf buf_bat;
imu_gyro_raw[AXIS_X] = max1167_values[0]; \
imu_gyro_raw[AXIS_Y] = max1167_values[1]; \
imu_gyro_raw[AXIS_Z] = max1167_values[2]; \
imu_gyro[AXIS_X] = (float)((int32_t)max1167_values[0] - IMU_GYRO_X_NEUTRAL) \
* IMU_GYRO_X_GAIN; \
imu_gyro[AXIS_Y] = (float)((int32_t)max1167_values[1] - IMU_GYRO_Y_NEUTRAL) \
* IMU_GYRO_Y_GAIN; \
imu_gyro[AXIS_Z] = (float)((int32_t)max1167_values[2] - IMU_GYRO_Z_NEUTRAL) \
* IMU_GYRO_Z_GAIN; \
imu_gyro[AXIS_X] = (float)((int32_t)max1167_values[0] - IMU_GYRO_P_NEUTRAL) \
* IMU_GYRO_P_GAIN; \
imu_gyro[AXIS_Y] = (float)((int32_t)max1167_values[1] - IMU_GYRO_Q_NEUTRAL) \
* IMU_GYRO_Q_GAIN; \
imu_gyro[AXIS_Z] = (float)((int32_t)max1167_values[2] - IMU_GYRO_R_NEUTRAL) \
* IMU_GYRO_R_GAIN; \
}
#define ImuUpdateMag() { \
@@ -98,15 +99,18 @@ extern struct adc_buf buf_bat;
imu_accel[AXIS_X] = IMU_ACCEL_X_GAIN *((int32_t)imu_vs_accel_raw_avg[AXIS_X] - IMU_ACCEL_X_NEUTRAL); \
imu_accel[AXIS_Y] = IMU_ACCEL_Y_GAIN *((int32_t)imu_vs_accel_raw_avg[AXIS_Y] - IMU_ACCEL_Y_NEUTRAL); \
imu_accel[AXIS_Z] = IMU_ACCEL_Z_GAIN *((int32_t)imu_vs_accel_raw_avg[AXIS_Z] - IMU_ACCEL_Z_NEUTRAL); \
imu_gyro[AXIS_X] = (float)((int32_t)imu_vs_gyro_raw_avg[AXIS_X] - IMU_GYRO_X_NEUTRAL) \
* IMU_GYRO_X_GAIN; \
imu_gyro[AXIS_Y] = (float)((int32_t)imu_vs_gyro_raw_avg[AXIS_Y] - IMU_GYRO_Y_NEUTRAL) \
* IMU_GYRO_Y_GAIN; \
imu_gyro[AXIS_Z] = (float)((int32_t)imu_vs_gyro_raw_avg[AXIS_Z] - IMU_GYRO_Z_NEUTRAL) \
* IMU_GYRO_Z_GAIN; \
imu_gyro_prev[AXIS_X] = imu_gyro[AXIS_X]; \
imu_gyro_prev[AXIS_Y] = imu_gyro[AXIS_Y]; \
imu_gyro_prev[AXIS_Z] = imu_gyro[AXIS_Z]; \
imu_gyro[AXIS_X] = (float)((int32_t)imu_vs_gyro_raw_avg[AXIS_P] - IMU_GYRO_P_NEUTRAL) \
* IMU_GYRO_P_GAIN; \
imu_gyro[AXIS_Y] = (float)((int32_t)imu_vs_gyro_raw_avg[AXIS_Q] - IMU_GYRO_Q_NEUTRAL) \
* IMU_GYRO_Q_GAIN; \
imu_gyro[AXIS_Z] = (float)((int32_t)imu_vs_gyro_raw_avg[AXIS_R] - IMU_GYRO_R_NEUTRAL) \
* IMU_GYRO_R_GAIN; \
imu_gyro_prev[AXIS_P] = imu_gyro[AXIS_P]; \
imu_gyro_prev[AXIS_Q] = imu_gyro[AXIS_Q]; \
imu_gyro_prev[AXIS_R] = imu_gyro[AXIS_R]; \
imu_vs_gyro_initial_bias[AXIS_P] = imu_gyro[AXIS_P]; \
imu_vs_gyro_initial_bias[AXIS_Q] = imu_gyro[AXIS_Q]; \
imu_vs_gyro_initial_bias[AXIS_R] = imu_gyro[AXIS_R]; \
}
sw/airborne/main_booz.c
+25 -4
View File
@@ -10,7 +10,7 @@
#include "uart.h"
#include "messages.h"
#include "downlink.h"
#include "booz_downlink.h"
#include "booz_telemetry.h"
#include "spi.h"
#include "link_imu.h"
@@ -35,7 +35,13 @@ static inline void main_init( void ) {
hw_init();
led_init();
sys_time_init();
// actuators_init();
// SetCommands(commands_failsafe);
ppm_init();
radio_control_init();
spi_init();
link_imu_init();
uart1_init_tx();
@@ -44,16 +50,21 @@ static inline void main_init( void ) {
static inline void main_periodic_task( void ) {
radio_control_periodic_task();
booz_downlink_periodic_task();
booz_telemetry_periodic_task();
static uint8_t my_cnt = 0;
my_cnt++;
if (!(my_cnt%10)) {
LED_TOGGLE(1);
DOWNLINK_SEND_BOOZ_STATUS(&link_imu_nb_err, &(link_imu_state.status));
}
// DOWNLINK_SEND_BOOZ_STATUS(&link_imu_nb_err, &(link_imu_state.status));
radio_control_periodic_task();
// if (fbw_mode == FBW_MODE_MANUAL && rc_status == RC_REALLY_LOST) {
// fbw_mode = FBW_MODE_AUTO;
// }
}
}
static inline void main_event_task( void ) {
@@ -76,6 +87,16 @@ static inline void main_event_task( void ) {
DOWNLINK_SEND_BOOZ_FD(&link_imu_state.rates[AXIS_P], &link_imu_state.rates[AXIS_Q], &link_imu_state.rates[AXIS_R], &link_imu_state.eulers[AXIS_X], &link_imu_state.eulers[AXIS_Y], &link_imu_state.eulers[AXIS_Z]);
}
if (ppm_valid) {
ppm_valid = FALSE;
radio_control_event_task();
// if (rc_values_contains_avg_channels) {
// fbw_mode = FBW_MODE_OF_PPRZ(rc_values[RADIO_MODE]);
// }
/* setpoints */
}
}
bool_t dl_msg_available;
sw/airborne/main_imu.c
+12 -12
View File
@@ -21,15 +21,15 @@
//#define SEND_ACCEL 1
//#define SEND_MAG 1
//#define SEND_GYRO 1
#define SEND_ACCEL_RAW 1
#define SEND_MAG_RAW 1
#define SEND_GYRO 1
//#define SEND_ACCEL_RAW 1
//#define SEND_MAG_RAW 1
#define SEND_GYRO_RAW 1
#define SEND_ACCEL_RAW_AVG 1
#define SEND_MAG_RAW_AVG 1
#define SEND_GYRO_RAW_AVG 1
#define SEND_AHRS_STATE 1
#define SEND_AHRS_COV 1
//#define SEND_ACCEL_RAW_AVG 1
//#define SEND_MAG_RAW_AVG 1
//#define SEND_GYRO_RAW_AVG 1
//#define SEND_AHRS_STATE 1
//#define SEND_AHRS_COV 1
static inline void main_init( void );
static inline void main_periodic_task( void );
@@ -155,10 +155,10 @@ static inline void ahrs_task( void ) {
DOWNLINK_SEND_IMU_MAG_RAW_AVG(&imu_vs_mag_raw_avg[AXIS_X], &imu_vs_mag_raw_avg[AXIS_Y], &imu_vs_mag_raw_avg[AXIS_Z], \
&imu_vs_mag_raw_var[AXIS_X], &imu_vs_mag_raw_var[AXIS_Y], &imu_vs_mag_raw_var[AXIS_Z]);
#endif
if (imu_vehicle_still) {
ahrs_step = AHRS_STEP_ROLL;
afe_init(imu_mag, imu_accel, imu_gyro);
}
// if (imu_vehicle_still) {
// ahrs_step = AHRS_STEP_ROLL;
// afe_init(imu_mag, imu_accel, imu_gyro);
//}
}
else {
afe_predict(imu_gyro);
sw/airborne/main_mtt.c
+9 -5
View File
@@ -15,6 +15,8 @@
#include "messages.h"
#include "downlink.h"
#include "link_imu.h"
//#define SEND_ACCEL 1
//#define SEND_GYRO 1
//#define SEND_ACCEL_RAW 1
@@ -58,6 +60,7 @@ static inline void main_init( void ) {
main_init_spi1();
max1167_init();
multitilt_init();
link_imu_init();
int_enable();
}
@@ -83,7 +86,8 @@ static inline void main_event_task( void ) {
DOWNLINK_SEND_IMU_ACCEL_RAW(&imu_accel_raw[AXIS_X], &imu_accel_raw[AXIS_Y], &imu_accel_raw[AXIS_Z]);
#endif
ahrs_task();
ahrs_task();
link_imu_send();
}
}
@@ -118,15 +122,15 @@ static inline void ahrs_task( void ) {
break;
case MT_STATUS_RUNNING : {
t0 = T0TC;
// t0 = T0TC;
multitilt_predict(imu_gyro_prev);
multitilt_update(imu_accel);
t1 = T0TC;
uint32_t dif = t1 - t0;
// t1 = T0TC;
// uint32_t dif = t1 - t0;
static uint32_t foo_cnt = 0;
foo_cnt++;
if (!(foo_cnt % 10)) {
DOWNLINK_SEND_TIME(&dif);
// DOWNLINK_SEND_TIME(&dif);
#ifdef SEND_AHRS_STATE
const float foo = 0.;
DOWNLINK_SEND_AHRS_STATE(&mtt_phi, &mtt_theta, &foo, &foo, &mtt_bp, &mtt_bq, &mtt_br);
sw/airborne/multitilt.c
+11
View File
@@ -34,6 +34,17 @@ static const float R_accel = 0.3;
void multitilt_init(void) {
mtt_status = MT_STATUS_UNINIT;
mtt_phi = 0.;
mtt_theta = 0.;
mtt_psi = 0.;
mtt_p = 0.;
mtt_q = 0.;
mtt_r = 0.;
mtt_bp = 0.;
mtt_bq = 0.;
mtt_br = 0.;
}
void multitilt_start(const float* accel, const float* gyro) {
sw/airborne/multitilt.h
+2
View File
@@ -19,9 +19,11 @@ extern float mtt_q;
extern float mtt_bq;
extern float mtt_P_theta[2][2];
extern float mtt_psi;
extern float mtt_r;
extern float mtt_br;
extern void multitilt_init(void);
extern void multitilt_start( const float* accel, const float* gyro);
extern void multitilt_predict( const float* gyro );