Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-20 11:06:26 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity

improved constants for better response

Browse Source
This commit is contained in:
Paul Cox
2010-08-06 18:01:48 +00:00
parent 78b9af0968
commit 2fac51c5d6
3 changed files with 12 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/beth/main_overo.c
+1 -1
View File
@@ -122,7 +122,7 @@ static void main_periodic(int my_sig_num) {
estimator_run(msg_in.bench_sensor.z,msg_in.bench_sensor.y,msg_in.bench_sensor.x);
controller.elevation_sp = ((int32_t)(foo/512.)%2) ? RadOfDeg(-15) : RadOfDeg(0);
controller.elevation_sp = ((int32_t)(foo/2048.)%2) ? RadOfDeg(-20) : RadOfDeg(10);
control_run();
sw/airborne/beth/overo_controller.c
+10 -8
View File
@@ -14,7 +14,7 @@ void control_init(void) {
controller.elevation_sp = 0.;
controller.omega_tilt_ref = RadOfDeg(200);
controller.omega_elevation_ref = RadOfDeg(50);
controller.omega_elevation_ref = RadOfDeg(120);
controller.xi_ref = 1.;
controller.tilt_ref = estimator.tilt;
@@ -25,9 +25,10 @@ void control_init(void) {
controller.elevation_dot_ref = estimator.elevation_dot;
controller.elevation_ddot_ref = 0.;
controller.one_over_J = 1.;
controller.one_over_J = 2.;
controller.mass = 5.;
controller.omega_cl = RadOfDeg(500);
controller.omega_cl = RadOfDeg(600);
controller.xi_cl = 1.;
controller.cmd_pitch_ff = 0.;
@@ -47,7 +48,7 @@ void control_run(void) {
/*
* propagate reference
*/
const float dt_ctl = 1./500.;
const float dt_ctl = 1./512.;
const float thrust_constant = 40.;
controller.tilt_ref = controller.tilt_ref + controller.tilt_dot_ref * dt_ctl;
@@ -72,17 +73,18 @@ void control_run(void) {
controller.cmd_pitch_fb = controller.one_over_J*(2*controller.xi_cl*controller.omega_cl*err_tilt_dot) +
controller.one_over_J*(controller.omega_cl*controller.omega_cl*err_tilt);
controller.cmd_thrust_ff = controller.one_over_J*controller.elevation_ddot_ref;
controller.cmd_thrust_fb = -controller.one_over_J*(2*controller.xi_cl*controller.omega_cl*err_elevation_dot) -
controller.one_over_J*(controller.omega_cl*controller.omega_cl*err_elevation);
controller.cmd_thrust_ff = controller.mass*controller.elevation_ddot_ref;
controller.cmd_thrust_fb = -controller.mass*(2*controller.xi_cl*controller.omega_cl*err_elevation_dot) -
controller.mass*(controller.omega_cl*controller.omega_cl*err_elevation);
controller.cmd_pitch = controller.cmd_pitch_ff + controller.cmd_pitch_fb;
controller.cmd_thrust = controller.cmd_thrust_ff + controller.cmd_thrust_fb + thrust_constant;
if (controller.cmd_thrust<0.) controller.cmd_thrust = 0;
if (!(foo%100)) {
//printf("pitch : ff:%f fb:%f (%f)\n",controller.cmd_pitch_ff, controller.cmd_pitch_fb,estimator.tilt_dot);
printf("thrust: ff:%f fb:%f (%f %f)\n",controller.cmd_thrust_ff, controller.cmd_thrust_fb,estimator.elevation,estimator.elevation_dot);
//printf("thrust: ff:%f fb:%f (%f %f)\n",controller.cmd_thrust_ff, controller.cmd_thrust_fb,estimator.elevation,estimator.elevation_dot);
//printf("%f %f %f\n",controller.tilt_ref,controller.tilt_dot_ref,controller.tilt_ddot_ref);
}
foo++;
sw/airborne/beth/overo_controller.h
+1
View File
@@ -23,6 +23,7 @@ struct OveroController {
/* invert control law parameter */
float one_over_J;
float mass;
/* closed loop parameters */
float omega_cl;