Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-06 16:58:48 +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-31 14:10:48 +00:00
parent b5426ac4a5
commit 3815e40898
18 changed files with 141 additions and 152 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/autopilot/conf_booz.makefile
+1
View File
@@ -84,6 +84,7 @@ flt.CFLAGS += -DDISABLE_MAGNETOMETER
flt.srcs += micromag.c $(SRC_ARCH)/micromag_hw.c
flt.srcs += imu_v3.c $(SRC_ARCH)/imu_v3_hw.c
flt.CFLAGS += -DBOOZ_AHRS_TYPE=BOOZ_AHRS_MULTITILT
flt.srcs += multitilt.c
flt.srcs += booz_inter_mcu.c
conf/telemetry/booz.xml
+13 -6
View File
@@ -6,6 +6,7 @@
<process name="Controller">
<mode name="default">
<message name="BOOZ_STATUS" period="1."/>
<message name="RC" period="0.5"/>
<!-- <message name="BOOZ_FD" period="0.05"/> -->
<!-- <message name="BOOZ_DEBUG" period="0.25"/> -->
<message name="ACTUATORS" period="0.5"/>
@@ -26,6 +27,17 @@
<process name="Filter">
<mode name="default">
<message name="IMU_GYRO" period=".017"/>
<message name="IMU_GYRO_LP" period=".017"/>
<message name="IMU_GYRO_RAW" period=".25"/>
</mode>
<mode name="raw_sensors">
<message name="IMU_GYRO_RAW" period=".017"/>
</mode>
<mode name="scaled_sensors">
<message name="IMU_GYRO" period=".017"/>
</mode>
<mode name="simulation">
<message name="BOOZ_DEBUG" period=".1"/>
<message name="AHRS_STATE" period=".1"/>
<message name="AHRS_COV" period=".1"/>
@@ -37,12 +49,7 @@
<message name="IMU_MAG" period="1."/>
<message name="IMU_MAG_RAW" period="1."/>
</mode>
<mode name="raw_sensors">
<message name="IMU_GYRO_RAW" period=".017"/>
</mode>
<mode name="scaled_sensors">
<message name="IMU_GYRO" period=".017"/>
</mode>
</process>
sw/airborne/arm7/max1167_hw.c
+24 -8
View File
@@ -1,10 +1,16 @@
#include "max1167.h"
#include "led.h"
uint8_t max1167_error;
static void EXTINT0_ISR(void) __attribute__((naked));
extern void max1167_hw_init( void ) {
/* CS pin is output */
max1167_error = MAX1167_ERR_NONE;
/* SS pin is output */
SetBit(MAX1167_SS_IODIR, MAX1167_SS_PIN);
/* unselected max1167 */
Max1167Unselect();
@@ -23,11 +29,12 @@ extern void max1167_hw_init( void ) {
VICIntEnable = VIC_BIT( VIC_EINT0 ); // EXTINT0 interrupt enabled
VICVectCntl8 = VIC_ENABLE | VIC_EINT0;
VICVectAddr8 = (uint32_t)EXTINT0_ISR; // address of the ISR
}
#include "uart.h"
#include "messages.h"
#include "downlink.h"
void max1167_read( void ) {
if (max1167_status == STA_MAX1167_IDLE) {
/* select max1167 */
@@ -38,19 +45,22 @@ void max1167_read( void ) {
const uint8_t control_byte = 1 << 0 | 1 << 3 | 2 << 5;
SpiSend(control_byte);
/* enable timeout interrupt */
SpiEnableRti();
// SpiClearRti();
// SpiEnableRti();
max1167_status = STA_MAX1167_SENDING_REQ;
}
else {
/* report overrun error */
max1167_error = MAX1167_ERR_READ_OVERUN;
DOWNLINK_SEND_DEBUG_MCU_LINK(&max1167_status,&max1167_error, &max1167_status);
}
}
void EXTINT0_ISR(void) {
ISR_ENTRY();
if (max1167_status == STA_MAX1167_WAIT_EOC) {
/* Max1167Select(); maybe... */
// if (max1167_status == STA_MAX1167_WAIT_EOC) {
if (max1167_status == STA_MAX1167_SENDING_REQ) {
SpiEnable();
/* trigger 6 bytes read */
SpiSend(0);
@@ -67,6 +77,12 @@ void EXTINT0_ISR(void) {
}
else {
/* report error */
max1167_error = MAX1167_ERR_SPURIOUS_EOC;
DOWNLINK_SEND_DEBUG_MCU_LINK(&max1167_status,&max1167_error, &max1167_status);
/* reset */
max1167_status = STA_MAX1167_IDLE;
SpiDisable();
Max1167Unselect();
}
SetBit(EXTINT, MAX1167_EOC_EINT); /* clear extint0 */
sw/airborne/arm7/max1167_hw.h
+28 -21
View File
@@ -12,6 +12,10 @@
#include "interrupt_hw.h"
#include "spi_hw.h"
#define MAX1167_ERR_NONE 0
#define MAX1167_ERR_READ_OVERUN 1
#define MAX1167_ERR_SPURIOUS_EOC 2
extern uint8_t max1167_error;
#define MAX1167_SS_PIN 29
#define MAX1167_SS_IODIR IO1DIR
@@ -42,7 +46,6 @@
SpiClearRti(); \
SpiDisableRti(); \
SpiDisable(); \
/* Max1167Unselect(); maybe... */ \
/* here we might enable extint0 */ \
max1167_status = STA_MAX1167_WAIT_EOC; \
} \
@@ -52,25 +55,29 @@
/* should not happen */ \
break; \
\
case STA_MAX1167_READING_RES: \
/* store convertion result */ \
max1167_values[0] = SSPDR << 8; \
max1167_values[0] += SSPDR; \
max1167_values[1] = SSPDR << 8; \
max1167_values[1] += SSPDR; \
max1167_values[2] = SSPDR << 8; \
max1167_values[2] += SSPDR; \
SpiClearRti(); \
SpiDisableRti(); \
SpiDisable(); \
Max1167Unselect(); \
max1167_status = STA_MAX1167_DATA_AVAILABLE; \
break; \
case STA_MAX1167_DATA_AVAILABLE : \
/* should not happen */ \
break; \
} \
} \
}
case STA_MAX1167_READING_RES: { \
/* read dummy control byte reply */ \
uint8_t foo __attribute__ ((unused)); \
foo = SSPDR; \
/* store convertion result */ \
max1167_values[0] = SSPDR << 8; \
max1167_values[0] += SSPDR; \
max1167_values[1] = SSPDR << 8; \
max1167_values[1] += SSPDR; \
max1167_values[2] = SSPDR << 8; \
max1167_values[2] += SSPDR; \
SpiClearRti(); \
SpiDisableRti(); \
SpiDisable(); \
Max1167Unselect(); \
max1167_status = STA_MAX1167_DATA_AVAILABLE; \
} \
break; \
case STA_MAX1167_DATA_AVAILABLE : \
/* should not happen */ \
break; \
} \
} \
}
#endif /* MAX1167_WH */
sw/airborne/booz_ahrs.h
+40
View File
@@ -0,0 +1,40 @@
#ifndef BOOZ_AHRS_H
#define BOOZ_AHRS_H
#define BOOZ_AHRS_STATUS_UNINIT 0
#define BOOZ_AHRS_STATUS_RUNNING 1
#define BOOZ_AHRS_STATUS_CRASHED 2
extern uint8_t booz_ahrs_status;
extern float booz_ahrs_phi;
extern float booz_ahrs_theta;
extern float booz_ahrs_psi;
extern float booz_ahrs_p;
extern float booz_ahrs_q;
extern float booz_ahrs_r;
extern float booz_ahrs_bp;
extern float booz_ahrs_bq;
extern float booz_ahrs_br;
extern void booz_ahrs_init(void);
extern void booz_ahrs_start( const float* accel, const float* gyro, const int16_t* mag);
extern void booz_ahrs_predict( const float* gyro );
extern void booz_ahrs_update( const float* accel, const int16_t* mag );
//#define __AHRS_IMPL_HEADER(_typ, _ext) _t##_ext
//#define _AHRS_IMPL_HEADER(_typ, _ext) __AHRS_IMPL_HEADER(_typ, _ext)
//#define AHRS_IMPL_HEADER(_typ) _AHRS_IMPL_HEADER(_typ, ".h")
//#error BOOZ_AHRS_TYPE
//#include \"AHRS_IMPL_HEADER(BOOZ_AHRS_TYPE)\"
#define BOOZ_AHRS_MULTITILT 0
#define BOOZ_AHRS_EULER 1
#if defined BOOZ_AHRS_TYPE && BOOZ_AHRS_TYPE == BOOZ_AHRS_MULTITILT
#include "multitilt.h"
#endif
#endif /* BOOZ_AHRS_H */
sw/airborne/booz_autopilot.c
+1
View File
@@ -40,6 +40,7 @@ void booz_autopilot_on_rc_event(void) {
/* the ap gets a mode everytime - the rc filters it */
if (rc_values_contains_avg_channels) {
booz_autopilot_mode = BOOZ_AP_MODE_OF_PPRZ(rc_values[RADIO_MODE]);
rc_values_contains_avg_channels = FALSE;
}
switch (booz_autopilot_mode) {
case BOOZ_AP_MODE_RATE:
sw/airborne/booz_filter_main.c
+3 -1
View File
@@ -7,7 +7,9 @@
#include "adc.h"
#include "imu_v3.h"
#include "multitilt.h"
//#include "multitilt.h"
#include "booz_ahrs.h"
#include "uart.h"
#include "messages.h"
sw/airborne/booz_inter_mcu.c
+9
View File
@@ -7,10 +7,19 @@
struct booz_inter_mcu_state inter_mcu_state;
#ifdef BOOZ_FILTER_MCU
#define LP_GYROS
void inter_mcu_fill_state() {
#ifndef LP_GYROS
inter_mcu_state.r_rates[AXIS_P] = imu_vs_gyro_unbiased[AXIS_P] * RATE_PI_S/M_PI;
inter_mcu_state.r_rates[AXIS_Q] = imu_vs_gyro_unbiased[AXIS_Q] * RATE_PI_S/M_PI;
inter_mcu_state.r_rates[AXIS_R] = imu_vs_gyro_unbiased[AXIS_R] * RATE_PI_S/M_PI;
#else
inter_mcu_state.r_rates[AXIS_P] = imu_gyro_lp[AXIS_P] * RATE_PI_S/M_PI;
inter_mcu_state.r_rates[AXIS_Q] = imu_gyro_lp[AXIS_Q] * RATE_PI_S/M_PI;
inter_mcu_state.r_rates[AXIS_R] = imu_gyro_lp[AXIS_R] * RATE_PI_S/M_PI;
#endif
inter_mcu_state.f_rates[AXIS_P] = mtt_p * RATE_PI_S/M_PI;
inter_mcu_state.f_rates[AXIS_Q] = mtt_q * RATE_PI_S/M_PI;
inter_mcu_state.f_rates[AXIS_R] = mtt_r * RATE_PI_S/M_PI;
sw/airborne/booz_telemetry.h
+3
View File
@@ -29,6 +29,9 @@
&cpu_time_sec, \
&buss_twi_blmc_nb_err)
#define PERIODIC_SEND_RC() DOWNLINK_SEND_RC(PPM_NB_PULSES, rc_values)
#define PERIODIC_SEND_BOOZ_FD() \
DOWNLINK_SEND_BOOZ_FD(&booz_estimator_p, \
&booz_estimator_q, \
sw/airborne/control_grz.h
-93
View File
@@ -1,93 +0,0 @@
/*
* Paparazzi $Id$
*
* Copyright (C) 2006 Pascal Brisset, Antoine Drouin
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*
*/
#ifndef CONTROL_GRZ_H
#define CONTROL_GRZ_H
#include <inttypes.h>
struct pid {
float measure;
float set_point;
float p_gain;
float i_gain;
float d_gain;
float last_err;
float sum_err;
float saturation;
};
extern float max_roll_dot_sp;
extern float max_pitch_dot_sp;
extern float max_yaw_dot_sp;
extern float ctl_grz_throttle_level;
extern float ctl_grz_z_dot;
extern float ctl_grz_z;
extern float ctl_grz_z_dot_sum_err;
extern float ctl_grz_z_sum_err;
extern float ctl_grz_z_dot_pgain;
extern float ctl_grz_z_dot_igain;
extern float ctl_grz_z_dot_dgain;
extern float ctl_grz_z_dot_setpoint;
extern float ctl_grz_z_pgain;
extern float ctl_grz_z_igain;
extern float ctl_grz_z_dgain;
extern float ctl_grz_z_setpoint;
extern struct pid roll_pid;
extern struct pid pitch_pid;
extern struct pid yaw_pid;
extern struct pid roll_dot_pid;
extern struct pid pitch_dot_pid;
extern struct pid yaw_dot_pid;
extern void ctl_grz_init( void );
extern void ctl_grz_set_setpoints_rate( void );
extern void ctl_grz_set_setpoints_auto1( void );
extern void ctl_grz_set_setpoints_auto2( void );
extern void ctl_grz_set_measures( void );
extern void ctl_grz_rate_run ( void );
extern void ctl_grz_attitude_run ( void );
extern void ctl_grz_z_dot_run ( void );
extern void ctl_grz_alt_run ( void );
extern void ctl_grz_horiz_speed_run ( void );
extern float north_angle_set_point;
extern float east_angle_set_point;
extern struct pid vn_pid;
extern struct pid ve_pid;
extern bool_t flying;
void ctl_grz_reset( void );
extern float trim_roll, trim_yaw, trim_pitch;
#endif // CONTROL_GRZ_H
sw/airborne/imu_v3.h
+4 -4
View File
@@ -15,7 +15,7 @@ extern int16_t imu_mag[AXIS_NB]; /* magnetometer in arbitrary unit */
extern float imu_bat; /* battery in volts */
extern float imu_gyro_prev[AXIS_NB]; /* previous gyros in rad/s */
#define IMU_GYRO_LP_ALPHA 0.6
#define IMU_GYRO_LP_ALPHA 0.1
extern float imu_gyro_lp[AXIS_NB]; /* low passed calibrated gyros in rad/s */
/* raw sensors readings */
@@ -145,13 +145,13 @@ extern struct adc_buf buf_bat;
}
#define ImuPeriodic() { \
if (max1167_status != STA_MAX1167_IDLE) { \
/* if (max1167_status != STA_MAX1167_IDLE) { \
DOWNLINK_SEND_AHRS_OVERRUN(); \
} \
else { \
else { */ \
max1167_read(); \
micromag_read(); \
} \
/* } */ \
}
sw/airborne/max1167.c
+2 -2
View File
@@ -1,15 +1,15 @@
#include "max1167.h"
//volatile uint8_t max1167_data_available;
volatile uint8_t max1167_status;
uint16_t max1167_values[MAX1167_NB_CHAN];
extern void max1167_init( void ) {
max1167_hw_init();
uint8_t i;
for (i=0; i<MAX1167_NB_CHAN; i++)
max1167_values[i] = 0;
// max1167_data_available = FALSE;
max1167_status = STA_MAX1167_IDLE;
}
sw/airborne/max1167.h
+2 -1
View File
@@ -7,7 +7,6 @@
extern void max1167_init( void );
extern void max1167_read( void );
//extern volatile uint8_t max1167_data_available;
#define STA_MAX1167_IDLE 0
#define STA_MAX1167_SENDING_REQ 1
@@ -18,7 +17,9 @@ extern volatile uint8_t max1167_status;
extern uint16_t max1167_values[MAX1167_NB_CHAN];
extern void max1167_hw_init( void );
#include "max1167_hw.h"
#endif /* MAX1167_H */
sw/airborne/multitilt.c
-5
View File
@@ -66,7 +66,6 @@ static inline float psi_of_mag( const int16_t* mag) {
return -atan2( me, mn );
}
void multitilt_init(void) {
mtt_status = MT_STATUS_UNINIT;
mtt_phi = 0.;
@@ -157,8 +156,6 @@ void multitilt_predict( const float* gyro ) {
}
static void inline MttUpdateAxis(float _err, float _P[2][2], float* angle, float* bias) {
const float Pct_0 = _P[0][0];
const float Pct_1 = _P[1][0];
@@ -182,8 +179,6 @@ static void inline MttUpdateAxis(float _err, float _P[2][2], float* angle, float
}
void multitilt_update( const float* accel, const int16_t* mag ) {
const float measure_phi = phi_of_accel(accel);
sw/airborne/multitilt.h
+1 -1
View File
@@ -22,7 +22,7 @@ extern float mtt_P_theta[2][2];
extern float mtt_psi;
extern float mtt_r;
extern float mtt_br;
extern float mtt_P_psi[2][2];
extern void multitilt_init(void);
extern void multitilt_start( const float* accel, const float* gyro, const int16_t* mag);
sw/simulator/Makefile
+1 -1
View File
@@ -163,7 +163,7 @@ BOOZ_AB_SRCS += $(AB)/max1167.c $(AB_ARCH)/max1167_hw.c
BOOZ_AB_SRCS += $(AB)/micromag.c $(AB_ARCH)/micromag_hw.c
BOOZ_AB_SRCS += $(AB)/imu_v3.c $(AB_ARCH)/imu_v3_hw.c
CFLAGS += -DBOOZ_ATT_FILTER_TYPE=multitilt
CFLAGS += -DBOOZ_AHRS_TYPE=multitilt
BOOZ_AB_SRCS += $(AB)/multitilt.c
sw/simulator/booz_sensors_model.c
+6 -6
View File
@@ -105,14 +105,14 @@ static void booz_sensors_model_gyro_init(void) {
bsm.gyro_neutral->ve[AXIS_R] = (double)bsm.gyro_resolution * 0.6035156; /* ratio of full scale - nominal 0.5 */
bsm.gyro_noise_std_dev = v_get(AXIS_NB);
bsm.gyro_noise_std_dev->ve[AXIS_P] = RadOfDeg(1.);
bsm.gyro_noise_std_dev->ve[AXIS_Q] = RadOfDeg(1.);
bsm.gyro_noise_std_dev->ve[AXIS_R] = RadOfDeg(1.);
bsm.gyro_noise_std_dev->ve[AXIS_P] = RadOfDeg(.5);
bsm.gyro_noise_std_dev->ve[AXIS_Q] = RadOfDeg(.5);
bsm.gyro_noise_std_dev->ve[AXIS_R] = RadOfDeg(.5);
bsm.gyro_bias_initial = v_get(AXIS_NB);
bsm.gyro_bias_initial->ve[AXIS_P] = RadOfDeg( 1.0);
bsm.gyro_bias_initial->ve[AXIS_Q] = RadOfDeg(-0.25);
bsm.gyro_bias_initial->ve[AXIS_R] = RadOfDeg( 0.5);
bsm.gyro_bias_initial->ve[AXIS_P] = RadOfDeg( -0.2);
bsm.gyro_bias_initial->ve[AXIS_Q] = RadOfDeg( -0.5);
bsm.gyro_bias_initial->ve[AXIS_R] = RadOfDeg( 0.5);
bsm.gyro_bias_random_walk_std_dev = v_get(AXIS_NB);
bsm.gyro_bias_random_walk_std_dev->ve[AXIS_P] = RadOfDeg(5.e-1);
sw/simulator/main_booz_sim.c
+3 -3
View File
@@ -272,10 +272,10 @@ static void booz_sim_set_ppm_from_joystick( void ) {
// printf("joystick roll %f %d\n", booz_joystick_value[JS_ROLL], ppm_pulses[RADIO_ROLL]);
#else
ppm_pulses[RADIO_THROTTLE] = 1223 + 0.6 * (2050-1223);
BREAK_MTT();
WALK_OVAL();
//BREAK_MTT();
//WALK_OVAL();
// CIRCLE();
// STATIONARY();
STATIONARY();
// TOUPIE();
#endif
}