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:
Pascal Brisset
2008-02-26 10:37:31 +00:00
parent a3fc49981f
commit 414ba8d153
6 changed files with 47 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/tl_autopilot.c
+2 -1
View File
@@ -34,8 +34,9 @@ void tl_autopilot_periodic_task(void) {
break;
case TL_AP_MODE_NAV:
/* 4Hz */
// RunOnceEvery(15, { common_nav_periodic_task_4Hz(); tl_nav_periodic_task(); });
RunOnceEvery(15, { common_nav_periodic_task_4Hz(); tl_nav_periodic_task(); });
tl_control_attitude_run();
tl_control_agl_run();
SetCommands(tl_control_commands);
break;
}
sw/airborne/tl_control.c
+22 -41
View File
@@ -24,8 +24,8 @@ float tl_control_agl_sp;
pprz_t tl_control_commands[COMMANDS_NB];
#define TL_CONTROL_RATE_R_PGAIN -120.
#define TL_CONTROL_RATE_R_DGAIN 5.
#define TL_CONTROL_RATE_R_PGAIN -100.
#define TL_CONTROL_RATE_R_DGAIN 4.
#define TL_CONTROL_RATE_R_IGAIN 0.2
/* setpoints for max stick throw in degres per second */
@@ -51,9 +51,9 @@ float tl_control_attitude_psi_sum_err;
#define TL_CONTROL_SPEED_DGAIN 0.
#define TL_CONTROL_SPEED_IGAIN -0.20
#define TL_CONTROL_AGL_PGAIN -1050 /* PPRZ / m */
#define TL_CONTROL_AGL_DGAIN -1150 /* PPRZ / m */
#define TL_CONTROL_AGL_IGAIN -2 /* PPRZ / m */
#define TL_CONTROL_AGL_PGAIN -400 /* PPRZ / m */
#define TL_CONTROL_AGL_DGAIN -550 /* PPRZ / m */
#define TL_CONTROL_AGL_IGAIN -1 /* PPRZ / m */
#define TL_MAX_AGL_ERROR 1
@@ -63,7 +63,6 @@ float tl_control_attitude_psi_sum_err;
#define TL_CONTROL_TRIM_R 300
int16_t tl_control_trim_r;
uint8_t tl_z_mode;
float tl_control_agl_pgain;
float tl_control_agl_dgain;
float tl_control_agl_igain;
@@ -112,8 +111,6 @@ void tl_control_init(void) {
tl_control_speed_igain = TL_CONTROL_SPEED_IGAIN;
tl_control_speed_u_sum_err = 0;
tl_control_speed_v_sum_err = 0;
tl_z_mode = TL_Z_MODE_RM;
}
@@ -157,29 +154,20 @@ void tl_control_rate_run(void) {
}
// #define DO_STEPS
// #define DO_VERTICAL_STEPS
#define DO_VERTICAL_STEPS
void tl_control_attitude_read_setpoints_from_rc(void) {
tl_control_attitude_phi_sp = -rc_values[RADIO_ROLL];
tl_control_attitude_theta_sp = rc_values[RADIO_PITCH];
switch (tl_z_mode) {
case TL_Z_MODE_RC:
tl_control_power_sp = rc_values[RADIO_THROTTLE];
break;
case TL_Z_MODE_RM:
//tl_control_agl_sp = Max (0, 3. * (rc_values[RADIO_THROTTLE]/MAX_PPRZ - MAX_PPRZ/10)); /* 2m max */
#ifdef DO_VERTICAL_STEPS
if (cpu_time_sec % 30 < 15)
tl_control_agl_sp = -1.5;
if (cpu_time_sec % 30 < 15)
tl_control_agl_sp = -3;
else
tl_control_agl_sp = -2.0;
tl_control_agl_sp = -3.0;
#else
tl_control_agl_sp = -3. * rc_values[RADIO_THROTTLE]/MAX_PPRZ;
#endif
break;
}
#ifndef DO_STEPS
if (!estimator_in_flight)
@@ -245,26 +233,19 @@ void tl_control_nav_read_setpoints_from_rc(void) {
}
void tl_control_agl_run(void) {
switch(tl_z_mode) {
case TL_Z_MODE_RC:
/* Do nothing */
break;
case TL_Z_MODE_RM: {
float agl_err = tl_estimator_agl - tl_control_agl_sp;
Bound(agl_err, -TL_MAX_AGL_ERROR, TL_MAX_AGL_ERROR);
if (estimator_in_flight)
tl_control_agl_sum_err += agl_err;
else
tl_control_agl_sum_err = 0;
tl_control_power_sp = tl_estimator_cruise_power - /* minus because Z pointing downward */
( tl_control_agl_pgain * agl_err +
tl_control_agl_dgain * tl_estimator_agl_dot +
tl_control_agl_igain * tl_control_agl_sum_err);
if (rc_values[RADIO_THROTTLE] < MAX_PPRZ/10) /* FIXME */
tl_control_power_sp = 0;
}
}
float agl_err = tl_estimator_agl - tl_control_agl_sp;
Bound(agl_err, -TL_MAX_AGL_ERROR, TL_MAX_AGL_ERROR);
if (estimator_in_flight)
tl_control_agl_sum_err += agl_err;
else
tl_control_agl_sum_err = 0;
tl_control_power_sp = tl_estimator_cruise_power - /* minus because Z pointing downward */
( tl_control_agl_pgain * agl_err +
tl_control_agl_dgain * tl_estimator_agl_dot +
tl_control_agl_igain * tl_control_agl_sum_err);
if (rc_values[RADIO_THROTTLE] < MAX_PPRZ/10 || kill_throttle) /* FIXME */
tl_control_power_sp = 0;
}
sw/airborne/tl_estimator.c
+15 -1
View File
@@ -6,8 +6,10 @@
#include "tl_vfilter.h"
#include "tl_telemetry.h"
#include "led.h"
bool_t estimator_in_flight;
bool_t tl_estimator_set_ground_pressure;
uint16_t estimator_flight_time;
float tl_estimator_u;
@@ -47,6 +49,10 @@ static const float R_accel = 0.5;
#define TL_PSI_KALM_UNINIT 0
#define TL_PSI_KALM_RUNNING 1
#define BARO_ALPHA 1
float tl_baro_alpha;
uint8_t tl_psi_kalm_status;
float tl_psi_kalm_psi;
float tl_psi_kalm_r;
@@ -78,6 +84,8 @@ void tl_estimator_init(void) {
tl_estimator_u_dot = 0.;
tl_estimator_v_dot = 0.;
tl_estimator_cruise_power = TL_ESTIMATOR_CRUISE_POWER;
tl_estimator_set_ground_pressure = TRUE;
tl_baro_alpha = BARO_ALPHA;
}
void tl_estimator_use_gps(void) {
@@ -109,6 +117,8 @@ void tl_estimator_use_gyro(void) {
}
tl_vf_predict(tl_imu_accel);
DOWNLINK_SEND_VF_PREDICT(&tl_imu_accel);
tl_estimator_agl = tl_vf_z;
tl_estimator_agl_dot = tl_vf_zdot;
}
@@ -116,8 +126,11 @@ void tl_estimator_use_gyro(void) {
void tl_estimator_use_imu(void) {
float estimator_psi_measure = -atan2(tl_imu_hx, -tl_imu_hy) + MAGNETIC_DECLINATION;
if (!estimator_in_flight)
if (tl_estimator_set_ground_pressure) {
estimator_ground_pressure = tl_imu_pressure;
estimator_z_baro = 0;
}
estimator_z_baro = (estimator_ground_pressure - (float)tl_imu_pressure)*0.084;
if (tl_psi_kalm_status == TL_PSI_KALM_UNINIT) {
@@ -131,6 +144,7 @@ void tl_estimator_use_imu(void) {
tl_vf_update(-tl_imu_rm);
#else
tl_vf_update(-estimator_z_baro);
DOWNLINK_SEND_VF_UPDATE(&estimator_z_baro, &tl_imu_rm);
#endif
}
sw/airborne/tl_estimator.h
+2
View File
@@ -4,6 +4,7 @@
#include "std.h"
extern bool_t estimator_in_flight;
extern bool_t tl_estimator_set_ground_pressure;
extern uint16_t estimator_flight_time;
extern float tl_estimator_u;
@@ -37,6 +38,7 @@ void tl_estimator_use_gyro(void);
void tl_estimator_use_imu(void);
void tl_estimator_periodic_task(void);
extern float tl_baro_alpha;
extern float tl_psi_kalm_psi;
extern float tl_psi_kalm_bias;
extern float tl_psi_kalm_P[2][2];
sw/airborne/tl_main.c
+5
View File
@@ -61,8 +61,13 @@ static inline void tl_main_init( void ) {
uart0_init_tx();
uart1_init_tx();
// gps_init();
int_enable();
// sys_time_usleep(500000);
// gps_configure_uart();
DOWNLINK_SEND_BOOT(&cpu_time_sec);
}
sw/airborne/tl_nav.c
+1
View File
@@ -4,6 +4,7 @@
#include "tl_estimator.h"
#include "tl_control.h"
#include "gps.h"
#include "radio_control.h"
float tl_nav_goto_h_pgain;
float tl_nav_goto_h_dgain;