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 29 Packages Projects Releases Wiki Activity

*** empty log message ***

Browse Source
This commit is contained in:
Antoine Drouin
2007-11-06 21:08:19 +00:00
parent 1f537bfaaa
commit 09b631c80d
7 changed files with 145 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/booz_control.c
+2 -1
View File
@@ -137,7 +137,8 @@ void booz_control_attitude_run(void) {
#ifndef DISABLE_MAGNETOMETER
const float att_err_psi = booz_estimator_psi - booz_control_attitude_psi_sp;
float att_err_psi = booz_estimator_psi - booz_control_attitude_psi_sp;
NormRadAngle(att_err_psi);
const float cmd_r = booz_control_attitude_psi_pgain * att_err_psi +
booz_control_attitude_psi_dgain * booz_estimator_r;
#else
sw/airborne/booz_nav.c
+30 -2
View File
@@ -7,6 +7,32 @@
#include "radio_control.h"
#ifndef DISABLE_NAV
#define Block(x) case x: nav_block=x;
#define InitBlock() { nav_stage = 0; block_time = 0; InitStage(); }
#define NextBlock() { nav_block++; InitBlock(); }
#define GotoBlock(b) { nav_block=b; InitBlock(); }
#define Stage(s) case s: nav_stage=s;
#define NextStage() { nav_stage++; InitStage() }
#define NextStageFrom(wp) { last_wp = wp; NextStage() }
#define GotoStage(s) { nav_stage = s; InitStage() }
#define NavGotoWaypoint(_wp) {}
#define NavVerticalAutoThrottleMode(_foo) {}
#define NavVerticalAltitudeMode(_foo, _bar) {}
#define NAV_C
#include "flight_plan.h"
const uint8_t nb_waypoint;
struct point waypoints[NB_WAYPOINT] = WAYPOINTS;
uint8_t nav_stage, nav_block;
uint16_t stage_time, block_time;
#include "booz_nav_hover.h"
#define BoozNavRunOnceEvery(_prescaler, _code) { \
@@ -21,17 +47,19 @@
void booz_nav_init(void) {
nav_block = 0;
nav_stage = 0;
booz_nav_hover_init();
}
void booz_nav_run(void) {
#ifndef DISABLE_NAV
BoozNavRunOnceEvery( 10, \
BoozNavRunOnceEvery( 62, \
{ \
booz_nav_hover_run(); \
BoozControlAttitudeSetSetPoints(booz_nav_hover_phi_command, booz_nav_hover_theta_command, \
booz_nav_hover_psi_sp, booz_nav_hover_power_command); \
booz_nav_hover_psi_sp, booz_nav_hover_power_command); \
});
booz_control_attitude_run();
sw/airborne/booz_nav.h
+23 -3
View File
@@ -1,15 +1,35 @@
#ifndef BOOZ_NAV_H
#define BOOZ_NAV_H
#include "std.h"
#include "booz_nav_hover.h"
#ifndef DISABLE_NAV
#endif
extern void booz_nav_init(void);
extern void booz_nav_run(void);
extern void booz_nav_read_setpoints_from_rc(void);
struct point {
float x;
float y;
float a;
};
#define WaypointX(_wp) (waypoints[_wp].x)
#define WaypointY(_wp) (waypoints[_wp].y)
#define WaypointAlt(_wp) (waypoints[_wp].a)
extern const uint8_t nb_waypoint;
extern struct point waypoints[];
extern uint16_t stage_time, block_time;
extern uint8_t nav_stage, nav_block;
extern void nav_init_stage( void );
#define InitStage() { nav_init_stage(); return; }
#endif /* BOOZ_NAV_H */
sw/airborne/booz_nav_hover.c
+2 -5
View File
@@ -79,17 +79,12 @@ void booz_nav_hover_run(void) {
void booz_nav_hover_read_setpoints_from_rc(void) {
#ifndef DISABLE_NAV
booz_nav_hover_z_sp = -1. - 10. / MAX_PPRZ * (float)rc_values[RADIO_THROTTLE];
booz_nav_hover_psi_sp = -RadOfDeg(30.) / MAX_PPRZ * (float)rc_values[RADIO_YAW];
#if defined STICK_MODE && STICK_MODE == STICK_ABSOLUTE_POS
booz_nav_horizontal_x_sp = -20. / MAX_PPRZ * (float)rc_values[RADIO_PITCH]; /* +/- 20m */
booz_nav_horizontal_y_sp = -20. / MAX_PPRZ * (float)rc_values[RADIO_ROLL]; /* warning RC roll negativ to the right !! +/- 20m */
booz_nav_horizontal_u_sp = 0.;
booz_nav_horizontal_v_sp = 0.;
#elif defined STICK_MODE && STICK_MODE == STICK_ABSOLUTE_SPEED
booz_nav_horizontal_x_sp += -5. * DT_READ_SETPOINTS / MAX_PPRZ * (float)rc_values[RADIO_PITCH]; /* +/-5 m/s */
booz_nav_horizontal_y_sp += 5. * DT_READ_SETPOINTS / MAX_PPRZ * (float)rc_values[RADIO_ROLL]; /* +/-5 m/s */
booz_nav_horizontal_u_sp = 0.;
booz_nav_horizontal_v_sp = 0.;
#elif defined STICK_MODE && STICK_MODE == STICK_RELATIVE_SPEED
float booz_nav_hover_dx_bod = -5. / MAX_PPRZ * (float)rc_values[RADIO_PITCH];
float booz_nav_hover_dy_bod = -5. / MAX_PPRZ * (float)rc_values[RADIO_ROLL]; /* warning RC roll negativ to the right !! +/- 5m/s */
@@ -100,6 +95,8 @@ void booz_nav_hover_read_setpoints_from_rc(void) {
booz_nav_hover_dx_bod * booz_estimator_dcm[AXIS_Y][AXIS_Y];
booz_nav_hover_x_sp += (booz_nav_hover_dx_earth * DT_READ_SETPOINTS);
booz_nav_hover_y_sp += (booz_nav_hover_dy_earth * DT_READ_SETPOINTS);
booz_nav_hover_psi_sp += (-RadOfDeg(60.) * DT_READ_SETPOINTS / MAX_PPRZ * (float)rc_values[RADIO_YAW]);
NormRadAngle(booz_nav_hover_psi_sp);
#endif /* STICK_MODE */
#endif /* DISABLE_NAV */
}
sw/simulator/booz_sensors_model.c
+30 -35
View File
@@ -1,4 +1,5 @@
#include "booz_sensors_model.h"
#include "booz_sensors_model_params.h"
#include <math.h>
@@ -61,28 +62,28 @@ static void booz_sensors_model_accel_init(void) {
bsm.accel->ve[AXIS_X] = 0.;
bsm.accel->ve[AXIS_Y] = 0.;
bsm.accel->ve[AXIS_Z] = 0.;
bsm.accel_resolution = 1024 * 32;
bsm.accel_resolution = BSM_ACCEL_RESOLUTION;
bsm.accel_sensitivity = m_get(AXIS_NB, AXIS_NB);
m_zero(bsm.accel_sensitivity);
bsm.accel_sensitivity->me[AXIS_X][AXIS_X] = -(double)(bsm.accel_resolution) / (6. * 9.81);
bsm.accel_sensitivity->me[AXIS_Y][AXIS_Y] = (double)(bsm.accel_resolution) / (6. * 9.81);
bsm.accel_sensitivity->me[AXIS_Z][AXIS_Z] = (double)(bsm.accel_resolution) / (6. * 9.81);
bsm.accel_sensitivity->me[AXIS_X][AXIS_X] = BSM_ACCEL_SENSITIVITY_XX;
bsm.accel_sensitivity->me[AXIS_Y][AXIS_Y] = BSM_ACCEL_SENSITIVITY_YY;
bsm.accel_sensitivity->me[AXIS_Z][AXIS_Z] = BSM_ACCEL_SENSITIVITY_ZZ;
bsm.accel_neutral = v_get(AXIS_NB);
bsm.accel_neutral->ve[AXIS_X] = 538 * 32;
bsm.accel_neutral->ve[AXIS_Y] = 506 * 32;
bsm.accel_neutral->ve[AXIS_Z] = 506 * 32;
bsm.accel_neutral->ve[AXIS_X] = BSM_ACCEL_NEUTRAL_X;
bsm.accel_neutral->ve[AXIS_Y] = BSM_ACCEL_NEUTRAL_Y;
bsm.accel_neutral->ve[AXIS_Z] = BSM_ACCEL_NEUTRAL_Z;
bsm.accel_noise_std_dev = v_get(AXIS_NB);
bsm.accel_noise_std_dev->ve[AXIS_X] = 2e-1; /* m2s-4 */
bsm.accel_noise_std_dev->ve[AXIS_Y] = 2e-1; /* m2s-4 */
bsm.accel_noise_std_dev->ve[AXIS_Z] = 2e-1; /* m2s-4 */
bsm.accel_noise_std_dev->ve[AXIS_X] = BSM_ACCEL_NOISE_STD_DEV_X;
bsm.accel_noise_std_dev->ve[AXIS_Y] = BSM_ACCEL_NOISE_STD_DEV_Y;
bsm.accel_noise_std_dev->ve[AXIS_Z] = BSM_ACCEL_NOISE_STD_DEV_Z;
bsm.accel_bias = v_get(AXIS_NB);
bsm.accel_bias->ve[AXIS_P] = 1e-3; /* ms-2 */
bsm.accel_bias->ve[AXIS_Q] = 1e-3; /* ms-2 */
bsm.accel_bias->ve[AXIS_R] = 1e-3; /* ms-2 */
bsm.accel_bias->ve[AXIS_P] = BSM_ACCEL_BIAS_X;
bsm.accel_bias->ve[AXIS_Q] = BSM_ACCEL_BIAS_Y;
bsm.accel_bias->ve[AXIS_R] = BSM_ACCEL_BIAS_Z;
}
@@ -93,39 +94,33 @@ static void booz_sensors_model_gyro_init(void) {
bsm.gyro->ve[AXIS_P] = 0.;
bsm.gyro->ve[AXIS_Q] = 0.;
bsm.gyro->ve[AXIS_R] = 0.;
bsm.gyro_resolution = 65536;
bsm.gyro_resolution = BSM_GYRO_RESOLUTION;
bsm.gyro_sensitivity = m_get(AXIS_NB, AXIS_NB);
m_zero(bsm.gyro_sensitivity);
bsm.gyro_sensitivity->me[AXIS_P][AXIS_P] =
(double)bsm.gyro_resolution / (2.*RadOfDeg(-413.41848)); /* degres/s - nominal 300 */
bsm.gyro_sensitivity->me[AXIS_Q][AXIS_Q] =
(double)bsm.gyro_resolution / (2.*RadOfDeg(-403.65564)); /* degres/s - nominal 300 */
bsm.gyro_sensitivity->me[AXIS_R][AXIS_R] =
(double)bsm.gyro_resolution / (2.*RadOfDeg( 395.01929)); /* degres/s - nominal 300 */
bsm.gyro_sensitivity->me[AXIS_P][AXIS_P] = BSM_GYRO_SENSITIVITY_PP;
bsm.gyro_sensitivity->me[AXIS_Q][AXIS_Q] = BSM_GYRO_SENSITIVITY_QQ;
bsm.gyro_sensitivity->me[AXIS_R][AXIS_R] = BSM_GYRO_SENSITIVITY_RR;
bsm.gyro_neutral = v_get(AXIS_NB);
bsm.gyro_neutral->ve[AXIS_P] =
(double)bsm.gyro_resolution * 0.6238556; /* ratio of full scale - nominal 0.5 */
bsm.gyro_neutral->ve[AXIS_Q] =
(double)bsm.gyro_resolution * 0.6242371; /* ratio of full scale - nominal 0.5 */
bsm.gyro_neutral->ve[AXIS_R] =
(double)bsm.gyro_resolution * 0.6035156; /* ratio of full scale - nominal 0.5 */
bsm.gyro_neutral->ve[AXIS_P] = BSM_GYRO_NEUTRAL_P;
bsm.gyro_neutral->ve[AXIS_Q] = BSM_GYRO_NEUTRAL_Q;
bsm.gyro_neutral->ve[AXIS_R] = BSM_GYRO_NEUTRAL_R;
bsm.gyro_noise_std_dev = v_get(AXIS_NB);
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_noise_std_dev->ve[AXIS_P] = BSM_GYRO_NOISE_STD_DEV_P;
bsm.gyro_noise_std_dev->ve[AXIS_Q] = BSM_GYRO_NOISE_STD_DEV_Q;
bsm.gyro_noise_std_dev->ve[AXIS_R] = BSM_GYRO_NOISE_STD_DEV_R;
bsm.gyro_bias_initial = v_get(AXIS_NB);
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_initial->ve[AXIS_P] = BSM_GYRO_BIAS_INITIAL_P;
bsm.gyro_bias_initial->ve[AXIS_Q] = BSM_GYRO_BIAS_INITIAL_Q;
bsm.gyro_bias_initial->ve[AXIS_R] = BSM_GYRO_BIAS_INITIAL_R;
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);
bsm.gyro_bias_random_walk_std_dev->ve[AXIS_Q] = RadOfDeg(5.e-1);
bsm.gyro_bias_random_walk_std_dev->ve[AXIS_R] = RadOfDeg(5.e-1);
bsm.gyro_bias_random_walk_std_dev->ve[AXIS_P] = BSM_GYRO_BIAS_RANDOM_WALK_STD_DEV_P;
bsm.gyro_bias_random_walk_std_dev->ve[AXIS_Q] = BSM_GYRO_BIAS_RANDOM_WALK_STD_DEV_Q;
bsm.gyro_bias_random_walk_std_dev->ve[AXIS_R] = BSM_GYRO_BIAS_RANDOM_WALK_STD_DEV_R;
bsm.gyro_bias_random_walk_value = v_get(AXIS_NB);
bsm.gyro_bias_random_walk_value->ve[AXIS_P] = bsm.gyro_bias_initial->ve[AXIS_P];
sw/simulator/booz_sensors_model_params.h
+56
View File
@@ -0,0 +1,56 @@
#ifndef BOOZ_SENSORS_MODEL_PARAMS_H
#define BOOZ_SENSORS_MODEL_PARAMS_H
/*
* Accelerometer
*/
#define BSM_ACCEL_RESOLUTION (1024 * 32)
/* ms-2 */
#define BSM_ACCEL_SENSITIVITY_XX -(1024. * 32.)/(2 * 6. * 9.81)
#define BSM_ACCEL_SENSITIVITY_YY (1024. * 32.)/(2 * 6. * 9.81)
#define BSM_ACCEL_SENSITIVITY_ZZ (1024. * 32.)/(2 * 6. * 9.81)
#define BSM_ACCEL_NEUTRAL_X (538. * 32.)
#define BSM_ACCEL_NEUTRAL_Y (506. * 32.)
#define BSM_ACCEL_NEUTRAL_Z (506. * 32.)
/* m2s-4 */
#define BSM_ACCEL_NOISE_STD_DEV_X 2e-1
#define BSM_ACCEL_NOISE_STD_DEV_Y 2e-1
#define BSM_ACCEL_NOISE_STD_DEV_Z 2e-1
/* ms-2 */
#define BSM_ACCEL_BIAS_X 1e-3
#define BSM_ACCEL_BIAS_Y 1e-3
#define BSM_ACCEL_BIAS_Z 1e-3
/*
* Gyrometer
*/
#define BSM_GYRO_RESOLUTION 65536
/* degres/s - nominal 300 */
#define BSM_GYRO_SENSITIVITY_PP 65536. / (2.*RadOfDeg(-413.41848));
#define BSM_GYRO_SENSITIVITY_QQ 65536. / (2.*RadOfDeg(-403.65564));
#define BSM_GYRO_SENSITIVITY_RR 65536. / (2.*RadOfDeg( 395.01929));
#define BSM_GYRO_NEUTRAL_P 65536. * 0.6238556;
#define BSM_GYRO_NEUTRAL_Q 65536. * 0.6242371;
#define BSM_GYRO_NEUTRAL_R 65536. * 0.6035156;
#define BSM_GYRO_NOISE_STD_DEV_P RadOfDeg(.5)
#define BSM_GYRO_NOISE_STD_DEV_Q RadOfDeg(.5)
#define BSM_GYRO_NOISE_STD_DEV_R RadOfDeg(.5)
#define BSM_GYRO_BIAS_INITIAL_P RadOfDeg( .5)
#define BSM_GYRO_BIAS_INITIAL_Q RadOfDeg(-0.5)
#define BSM_GYRO_BIAS_INITIAL_R RadOfDeg( .25)
#define BSM_GYRO_BIAS_RANDOM_WALK_STD_DEV_P RadOfDeg(5.e-1)
#define BSM_GYRO_BIAS_RANDOM_WALK_STD_DEV_Q RadOfDeg(5.e-1)
#define BSM_GYRO_BIAS_RANDOM_WALK_STD_DEV_R RadOfDeg(5.e-1)
#endif /* BOOZ_SENSORS_MODEL_PARAMS_H */
sw/simulator/main_booz_sim.c
+2 -2
View File
@@ -201,9 +201,9 @@ static void booz_sim_set_ppm_from_joystick( void ) {
#else
ppm_pulses[RADIO_THROTTLE] = 1223 + 0.6 * (2050-1223);
//BREAK_MTT();
//WALK_OVAL();
WALK_OVAL();
// CIRCLE();
HOVER();
//HOVER();
// TOUPIE();
// ATTITUDE_ROLL_STEPS();
//ATTITUDE_PITCH_STEPS();