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

*** empty log message ***

Browse Source
This commit is contained in:
Pascal Brisset
2008-02-05 03:02:10 +00:00
parent 6f461bb27f
commit 03e59b19c3
10 changed files with 96 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/arm7/tl_imu.h
+9 -2
View File
@@ -29,13 +29,20 @@
#define HX_GAIN -1.
#define HX_NEUTRAL 0.
#define HX_CHAN 1
#define HY_GAIN -1.
#define HY_NEUTRAL 0
#define HY_GAIN -1.06
#define HY_NEUTRAL -75
#define HY_CHAN 0
#define HZ_GAIN -1.
#define HZ_NEUTRAL 0
#define HZ_CHAN 2
/* http://www.ngdc.noaa.gov/seg/geomag/declination.shtml
LA
*/
#define MAGNETIC_DECLINATION RadOfDeg(13)
#define TL_IMU_IDLE 0
#define TL_IMU_SENDING_BARO_REQ 1
sw/airborne/tl_autopilot.c
+9 -2
View File
@@ -27,10 +27,14 @@ void tl_autopilot_periodic_task(void) {
tl_control_agl_run();
SetCommands(tl_control_commands);
break;
case TL_AP_MODE_SPEED:
tl_control_speed_run();
tl_control_attitude_run();
SetCommands(tl_control_commands);
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();
SetCommands(tl_control_commands);
break;
@@ -52,6 +56,9 @@ void tl_autopilot_on_rc_event(void) {
case TL_AP_MODE_ATTITUDE:
tl_control_attitude_read_setpoints_from_rc();
break;
case TL_AP_MODE_SPEED:
tl_control_speed_read_setpoints_from_rc();
break;
case TL_AP_MODE_NAV:
tl_control_nav_read_setpoints_from_rc();
break;
sw/airborne/tl_autopilot.h
+2 -10
View File
@@ -8,6 +8,7 @@
#define TL_AP_MODE_RATE 2
#define TL_AP_MODE_ATTITUDE 3
#define TL_AP_MODE_NAV 4
#define TL_AP_MODE_SPEED 5
extern uint8_t tl_autopilot_mode;
@@ -27,15 +28,6 @@ extern void tl_autopilot_on_rc_event(void);
#define TL_AP_MODE_OF_PPRZ(mode) \
((mode) < TRESHOLD_RATE_PPRZ ? TL_AP_MODE_RATE : \
TL_AP_MODE_ATTITUDE )
#if 0
#define TL_AP_MODE_OF_PPRZ(mode) \
((mode) < TRESHOLD_RATE_PPRZ ? TL_AP_MODE_ATTITUDE : \
TL_AP_MODE_NAV )
#endif
TL_AP_MODE_AUTO )
#endif /* TL_AUTOPILOT_H */
sw/airborne/tl_control.c
+55 -1
View File
@@ -47,6 +47,10 @@ float tl_control_attitude_psi_sum_err;
#define TL_CONTROL_ATTITUDE_PSI_IGAIN -0.20
#define TL_CONTROL_ATTITUDE_PSI_TRIM 350
#define TL_CONTROL_SPEED_PGAIN -1000.
#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 */
@@ -66,6 +70,14 @@ float tl_control_agl_igain;
float tl_control_agl_sum_err;
#define TL_MAX_SPEED_ERROR 2
float tl_control_u_sp;
float tl_control_v_sp;
float tl_control_speed_pgain;
float tl_control_speed_dgain;
float tl_control_speed_igain;
float tl_control_speed_u_sum_err;
float tl_control_speed_v_sum_err;
void tl_control_init(void) {
@@ -95,12 +107,17 @@ void tl_control_init(void) {
tl_control_agl_igain = TL_CONTROL_AGL_IGAIN;
tl_control_agl_sum_err = 0;
tl_control_speed_pgain = TL_CONTROL_SPEED_PGAIN;
tl_control_speed_dgain = TL_CONTROL_SPEED_DGAIN;
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;
}
void tl_control_rate_read_setpoints_from_rc(void) {
tl_control_p_sp = -rc_values[RADIO_ROLL];
tl_control_q_sp = rc_values[RADIO_PITCH];
tl_control_r_sp = -rc_values[RADIO_YAW] * RadOfDeg(TL_CONTROL_RATE_R_MAX_SP)/MAX_PPRZ;
@@ -249,3 +266,40 @@ void tl_control_agl_run(void) {
}
}
}
void tl_control_speed_read_setpoints_from_rc(void) {
tl_control_v_sp = (2. * rc_values[RADIO_ROLL])/MAX_PPRZ;
tl_control_u_sp = (2. * -rc_values[RADIO_PITCH])/MAX_PPRZ;
if (!estimator_in_flight)
tl_control_attitude_psi_sp = estimator_psi;
else {
tl_control_attitude_psi_sp += -rc_values[RADIO_YAW] * RadOfDeg(TL_CONTROL_ATTITUDE_PSI_MAX_SP)/MAX_PPRZ;
NormRadAngle(tl_control_attitude_psi_sp);
}
tl_control_power_sp = rc_values[RADIO_THROTTLE];
}
void tl_control_speed_run(void) {
float err_u = tl_estimator_u - tl_control_u_sp;
Bound(err_u, -TL_MAX_SPEED_ERROR, TL_MAX_SPEED_ERROR);
float err_v = tl_estimator_v - tl_control_v_sp;
Bound(err_v, -TL_MAX_SPEED_ERROR, TL_MAX_SPEED_ERROR);
if(estimator_in_flight) {
tl_control_speed_u_sum_err += err_u;
tl_control_speed_v_sum_err += err_v;
} else {
tl_control_speed_u_sum_err = 0;
tl_control_speed_v_sum_err = 0;
}
tl_control_attitude_theta_sp = - (tl_control_speed_pgain * err_u +
tl_control_speed_dgain * tl_estimator_u_dot +
tl_control_speed_igain * tl_control_speed_u_sum_err );
tl_control_attitude_phi_sp = - (tl_control_speed_pgain * err_v +
tl_control_speed_dgain * tl_estimator_v_dot +
tl_control_speed_igain * tl_control_speed_v_sum_err );
}
sw/airborne/tl_control.h
+9
View File
@@ -12,7 +12,9 @@ extern void tl_control_rate_run(void);
extern void tl_control_attitude_read_setpoints_from_rc(void);
extern void tl_control_attitude_run(void);
extern void tl_control_agl_run(void);
extern void tl_control_speed_run(void);
extern void tl_control_nav_read_setpoints_from_rc(void);
extern void tl_control_speed_read_setpoints_from_rc(void);
extern bool_t kill_throttle;
@@ -51,6 +53,13 @@ extern float tl_control_agl_igain;
extern float tl_control_agl_sum_err;
extern float tl_control_agl_sp;
extern float tl_control_u_sp;
extern float tl_control_v_sp;
extern float tl_control_speed_pgain;
extern float tl_control_speed_dgain;
extern float tl_control_speed_igain;
extern float tl_control_speed_u_sum_err;
extern float tl_control_speed_v_sum_err;
extern int16_t tl_control_trim_r;
sw/airborne/tl_estimator.c
+5 -1
View File
@@ -12,6 +12,8 @@ uint16_t estimator_flight_time;
float tl_estimator_u;
float tl_estimator_v;
float tl_estimator_u_dot;
float tl_estimator_v_dot;
float estimator_x; /* m */
float estimator_y; /* m */
@@ -73,6 +75,8 @@ void tl_estimator_init(void) {
compute_dcm();
tl_estimator_u = 0.;
tl_estimator_v = 0.;
tl_estimator_u_dot = 0.;
tl_estimator_v_dot = 0.;
tl_estimator_cruise_power = TL_ESTIMATOR_CRUISE_POWER;
}
@@ -110,7 +114,7 @@ void tl_estimator_use_gyro(void) {
}
void tl_estimator_use_imu(void) {
float estimator_psi_measure = -atan2(tl_imu_hy, tl_imu_hx);
float estimator_psi_measure = -atan2(tl_imu_hx, -tl_imu_hy) + MAGNETIC_DECLINATION;
if (!estimator_in_flight)
estimator_ground_pressure = tl_imu_pressure;
sw/airborne/tl_estimator.h
+2
View File
@@ -8,6 +8,8 @@ extern uint16_t estimator_flight_time;
extern float tl_estimator_u;
extern float tl_estimator_v;
extern float tl_estimator_u_dot;
extern float tl_estimator_v_dot;
extern float estimator_x; /* m */
extern float estimator_y; /* m */
sw/airborne/tl_gps_configure.h
+1 -1
View File
@@ -1,7 +1,7 @@
static bool_t user_gps_configure(bool_t cpt) {
switch (cpt) {
case 0:
UbxSend_CFG_NAV(NAV_DYN_PEDESTRIAN, 3, 16, 24, 20, 5, 0, 0x3C, 0x3C, 0x14, 0x03E8 ,0x0000, 0x0, 0x17, 0x00FA, 0x00FA, 0x0064, 0x012C, 0x000F, 0x00, 0x00);
UbxSend_CFG_NAV(NAV_DYN_STATIONARY, 3, 16, 24, 20, 5, 0, 0x3C, 0x3C, 0x14, 0x03E8 ,0x0000, 0x0, 0x17, 0x00FA, 0x00FA, 0x0064, 0x012C, 0x000F, 0x00, 0x00);
break;
case 1:
UbxSend_CFG_MSG(UBX_NAV_ID, UBX_NAV_POSUTM_ID, 0, 1, 0, 0);
sw/airborne/tl_main.c
-1
View File
@@ -89,7 +89,6 @@ static inline void tl_main_periodic_task( void ) {
if (rc_status != RC_OK)
tl_autopilot_mode = TL_AP_MODE_FAILSAFE;
tl_telemetry_periodic_task();
SetActuatorsFromCommands(commands);
sw/airborne/tl_telemetry.h
+4 -2
View File
@@ -66,9 +66,11 @@
} \
}
#define PERIODIC_SEND_TL_VERTICAL_LOOP() DOWNLINK_SEND_TL_VERTICAL_LOOP(&tl_estimator_agl, &tl_control_agl_sp, &tl_control_agl_sum_err, &tl_estimator_agl_dot, &tl_estimator_cruise_power)
#define PERIODIC_SEND_TL_VERTICAL_LOOP() DOWNLINK_SEND_TL_VERTICAL_LOOP(&tl_estimator_agl, &tl_control_agl_sp, &tl_control_agl_sum_err, &tl_estimator_agl_dot)
#define PERIODIC_SEND_TL_DEBUG() DOWNLINK_SEND_TL_DEBUG(&x_unit_err_body, &y_unit_err_body)
#define PERIODIC_SEND_TL_SPEED_LOOP() DOWNLINK_SEND_TL_SPEED_LOOP(&tl_estimator_u, &tl_control_u_sp, &tl_control_speed_u_sum_err, &tl_estimator_v, &tl_control_v_sp, &tl_control_speed_v_sum_err)
#define PERIODIC_SEND_TL_DEBUG() DOWNLINK_SEND_TL_DEBUG(&tl_autopilot_mode)
#define PERIODIC_SEND_TL_KALM_PSI_STATE() { \