diff options
author | Tobias Naegeli <naegelit@student.ethz.ch> | 2012-11-01 16:50:52 +0100 |
---|---|---|
committer | Tobias Naegeli <naegelit@student.ethz.ch> | 2012-11-01 16:50:52 +0100 |
commit | 80ac43e21dadf727133201663169e6e931bc5a35 (patch) | |
tree | c865806b1b329e84314bdbeeb0d15007ea742d62 | |
parent | 6af2ea9fbc019b023e2b1201b788c9185690e193 (diff) | |
download | px4-firmware-80ac43e21dadf727133201663169e6e931bc5a35.tar.gz px4-firmware-80ac43e21dadf727133201663169e6e931bc5a35.tar.bz2 px4-firmware-80ac43e21dadf727133201663169e6e931bc5a35.zip |
Fine tuning of manual control
-rw-r--r-- | apps/multirotor_att_control/multirotor_attitude_control.c | 76 | ||||
-rw-r--r-- | apps/multirotor_att_control/multirotor_rate_control.c | 40 | ||||
-rw-r--r-- | apps/sensors/sensor_params.c | 14 | ||||
-rw-r--r-- | apps/systemcmds/param/param.c | 3 | ||||
-rw-r--r-- | apps/systemlib/pid/pid.c | 4 |
5 files changed, 70 insertions, 67 deletions
diff --git a/apps/multirotor_att_control/multirotor_attitude_control.c b/apps/multirotor_att_control/multirotor_attitude_control.c index 003180c18..839d56d29 100644 --- a/apps/multirotor_att_control/multirotor_attitude_control.c +++ b/apps/multirotor_att_control/multirotor_attitude_control.c @@ -57,50 +57,50 @@ PARAM_DEFINE_FLOAT(MC_YAWPOS_P, 0.3f); PARAM_DEFINE_FLOAT(MC_YAWPOS_I, 0.15f); PARAM_DEFINE_FLOAT(MC_YAWPOS_D, 0.0f); -PARAM_DEFINE_FLOAT(MC_YAWPOS_AWU, 1.0f); -PARAM_DEFINE_FLOAT(MC_YAWPOS_LIM, 3.0f); +//PARAM_DEFINE_FLOAT(MC_YAWPOS_AWU, 1.0f); +//PARAM_DEFINE_FLOAT(MC_YAWPOS_LIM, 3.0f); PARAM_DEFINE_FLOAT(MC_ATT_P, 0.2f); PARAM_DEFINE_FLOAT(MC_ATT_I, 0.0f); PARAM_DEFINE_FLOAT(MC_ATT_D, 0.05f); -PARAM_DEFINE_FLOAT(MC_ATT_AWU, 0.05f); -PARAM_DEFINE_FLOAT(MC_ATT_LIM, 0.4f); +//PARAM_DEFINE_FLOAT(MC_ATT_AWU, 0.05f); +//PARAM_DEFINE_FLOAT(MC_ATT_LIM, 0.4f); -PARAM_DEFINE_FLOAT(MC_ATT_XOFF, 0.0f); -PARAM_DEFINE_FLOAT(MC_ATT_YOFF, 0.0f); +//PARAM_DEFINE_FLOAT(MC_ATT_XOFF, 0.0f); +//PARAM_DEFINE_FLOAT(MC_ATT_YOFF, 0.0f); struct mc_att_control_params { float yaw_p; float yaw_i; float yaw_d; - float yaw_awu; - float yaw_lim; + //float yaw_awu; + //float yaw_lim; float att_p; float att_i; float att_d; - float att_awu; - float att_lim; + //float att_awu; + //float att_lim; - float att_xoff; - float att_yoff; + //float att_xoff; + //float att_yoff; }; struct mc_att_control_param_handles { param_t yaw_p; param_t yaw_i; param_t yaw_d; - param_t yaw_awu; - param_t yaw_lim; + //param_t yaw_awu; + //param_t yaw_lim; param_t att_p; param_t att_i; param_t att_d; - param_t att_awu; - param_t att_lim; + //param_t att_awu; + //param_t att_lim; - param_t att_xoff; - param_t att_yoff; + //param_t att_xoff; + //param_t att_yoff; }; /** @@ -122,17 +122,17 @@ static int parameters_init(struct mc_att_control_param_handles *h) h->yaw_p = param_find("MC_YAWPOS_P"); h->yaw_i = param_find("MC_YAWPOS_I"); h->yaw_d = param_find("MC_YAWPOS_D"); - h->yaw_awu = param_find("MC_YAWPOS_AWU"); - h->yaw_lim = param_find("MC_YAWPOS_LIM"); + //h->yaw_awu = param_find("MC_YAWPOS_AWU"); + //h->yaw_lim = param_find("MC_YAWPOS_LIM"); h->att_p = param_find("MC_ATT_P"); h->att_i = param_find("MC_ATT_I"); h->att_d = param_find("MC_ATT_D"); - h->att_awu = param_find("MC_ATT_AWU"); - h->att_lim = param_find("MC_ATT_LIM"); + //h->att_awu = param_find("MC_ATT_AWU"); + //h->att_lim = param_find("MC_ATT_LIM"); - h->att_xoff = param_find("MC_ATT_XOFF"); - h->att_yoff = param_find("MC_ATT_YOFF"); + //h->att_xoff = param_find("MC_ATT_XOFF"); + //h->att_yoff = param_find("MC_ATT_YOFF"); return OK; } @@ -142,17 +142,17 @@ static int parameters_update(const struct mc_att_control_param_handles *h, struc param_get(h->yaw_p, &(p->yaw_p)); param_get(h->yaw_i, &(p->yaw_i)); param_get(h->yaw_d, &(p->yaw_d)); - param_get(h->yaw_awu, &(p->yaw_awu)); - param_get(h->yaw_lim, &(p->yaw_lim)); + //param_get(h->yaw_awu, &(p->yaw_awu)); + //param_get(h->yaw_lim, &(p->yaw_lim)); param_get(h->att_p, &(p->att_p)); param_get(h->att_i, &(p->att_i)); param_get(h->att_d, &(p->att_d)); - param_get(h->att_awu, &(p->att_awu)); - param_get(h->att_lim, &(p->att_lim)); + //param_get(h->att_awu, &(p->att_awu)); + //param_get(h->att_lim, &(p->att_lim)); - param_get(h->att_xoff, &(p->att_xoff)); - param_get(h->att_yoff, &(p->att_yoff)); + //param_get(h->att_xoff, &(p->att_xoff)); + //param_get(h->att_yoff, &(p->att_yoff)); return OK; } @@ -186,10 +186,10 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s parameters_init(&h); parameters_update(&h, &p); - pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, p.att_awu, - p.att_lim, PID_MODE_DERIVATIV_SET); - pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, p.att_awu, - p.att_lim, PID_MODE_DERIVATIV_SET); + pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, + 1000.0f, PID_MODE_DERIVATIV_SET); + pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, + 1000.0f, PID_MODE_DERIVATIV_SET); initialized = true; } @@ -202,18 +202,18 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s /* apply parameters */ printf("att ctrl: delays: %d us sens->ctrl, rate: %d Hz, input: %d Hz\n", sensor_delay, (int)(1.0f/deltaT), (int)(1.0f/dT_input)); - pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, p.att_awu, p.att_lim); - pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, p.att_awu, p.att_lim); + pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f); + pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f); } /* calculate current control outputs */ /* control pitch (forward) output */ - rates_sp->pitch = pid_calculate(&pitch_controller, att_sp->pitch_body + p.att_xoff, + rates_sp->pitch = pid_calculate(&pitch_controller, att_sp->pitch_body , att->pitch, att->pitchspeed, deltaT); /* control roll (left/right) output */ - rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body + p.att_yoff, + rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body , att->roll, att->rollspeed, deltaT); /* control yaw rate */ diff --git a/apps/multirotor_att_control/multirotor_rate_control.c b/apps/multirotor_att_control/multirotor_rate_control.c index a5bff97e6..d5e20312b 100644 --- a/apps/multirotor_att_control/multirotor_rate_control.c +++ b/apps/multirotor_att_control/multirotor_rate_control.c @@ -58,28 +58,28 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.1f); /* same on Flamewheel */ PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f); -PARAM_DEFINE_FLOAT(MC_YAWRATE_AWU, 0.0f); -PARAM_DEFINE_FLOAT(MC_YAWRATE_LIM, 1.0f); +//PARAM_DEFINE_FLOAT(MC_YAWRATE_AWU, 0.0f); +//PARAM_DEFINE_FLOAT(MC_YAWRATE_LIM, 1.0f); PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 0.2f); /* 0.15 F405 Flamewheel */ PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.05f); PARAM_DEFINE_FLOAT(MC_ATTRATE_I, 0.0f); -PARAM_DEFINE_FLOAT(MC_ATTRATE_AWU, 0.05f); -PARAM_DEFINE_FLOAT(MC_ATTRATE_LIM, 1.0f); /**< roughly < 500 deg/s limit */ +//PARAM_DEFINE_FLOAT(MC_ATTRATE_AWU, 0.05f); +//PARAM_DEFINE_FLOAT(MC_ATTRATE_LIM, 1.0f); /**< roughly < 500 deg/s limit */ struct mc_rate_control_params { float yawrate_p; float yawrate_d; float yawrate_i; - float yawrate_awu; - float yawrate_lim; + //float yawrate_awu; + //float yawrate_lim; float attrate_p; float attrate_d; float attrate_i; - float attrate_awu; - float attrate_lim; + //float attrate_awu; + //float attrate_lim; float rate_lim; }; @@ -89,14 +89,14 @@ struct mc_rate_control_param_handles { param_t yawrate_p; param_t yawrate_i; param_t yawrate_d; - param_t yawrate_awu; - param_t yawrate_lim; + //param_t yawrate_awu; + //param_t yawrate_lim; param_t attrate_p; param_t attrate_i; param_t attrate_d; - param_t attrate_awu; - param_t attrate_lim; + //param_t attrate_awu; + //param_t attrate_lim; }; /** @@ -118,14 +118,14 @@ static int parameters_init(struct mc_rate_control_param_handles *h) h->yawrate_p = param_find("MC_YAWRATE_P"); h->yawrate_i = param_find("MC_YAWRATE_I"); h->yawrate_d = param_find("MC_YAWRATE_D"); - h->yawrate_awu = param_find("MC_YAWRATE_AWU"); - h->yawrate_lim = param_find("MC_YAWRATE_LIM"); + //h->yawrate_awu = param_find("MC_YAWRATE_AWU"); + //h->yawrate_lim = param_find("MC_YAWRATE_LIM"); h->attrate_p = param_find("MC_ATTRATE_P"); h->attrate_i = param_find("MC_ATTRATE_I"); h->attrate_d = param_find("MC_ATTRATE_D"); - h->attrate_awu = param_find("MC_ATTRATE_AWU"); - h->attrate_lim = param_find("MC_ATTRATE_LIM"); + //h->attrate_awu = param_find("MC_ATTRATE_AWU"); + //h->attrate_lim = param_find("MC_ATTRATE_LIM"); return OK; } @@ -135,14 +135,14 @@ static int parameters_update(const struct mc_rate_control_param_handles *h, stru param_get(h->yawrate_p, &(p->yawrate_p)); param_get(h->yawrate_i, &(p->yawrate_i)); param_get(h->yawrate_d, &(p->yawrate_d)); - param_get(h->yawrate_awu, &(p->yawrate_awu)); - param_get(h->yawrate_lim, &(p->yawrate_lim)); + //param_get(h->yawrate_awu, &(p->yawrate_awu)); + //param_get(h->yawrate_lim, &(p->yawrate_lim)); param_get(h->attrate_p, &(p->attrate_p)); param_get(h->attrate_i, &(p->attrate_i)); param_get(h->attrate_d, &(p->attrate_d)); - param_get(h->attrate_awu, &(p->attrate_awu)); - param_get(h->attrate_lim, &(p->attrate_lim)); + //param_get(h->attrate_awu, &(p->attrate_awu)); + //param_get(h->attrate_lim, &(p->attrate_lim)); return OK; } diff --git a/apps/sensors/sensor_params.c b/apps/sensors/sensor_params.c index 892ec975a..fd8a6602a 100644 --- a/apps/sensors/sensor_params.c +++ b/apps/sensors/sensor_params.c @@ -68,28 +68,28 @@ PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f); PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f); PARAM_DEFINE_FLOAT(RC1_MAX, 2000.0f); PARAM_DEFINE_FLOAT(RC1_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC1_DZ, 30.0f); +PARAM_DEFINE_FLOAT(RC1_DZ, 0.0f); PARAM_DEFINE_FLOAT(RC1_EXP, 0.0f); PARAM_DEFINE_FLOAT(RC2_MIN, 1000); PARAM_DEFINE_FLOAT(RC2_TRIM, 1500); PARAM_DEFINE_FLOAT(RC2_MAX, 2000); PARAM_DEFINE_FLOAT(RC2_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC2_DZ, 30.0f); +PARAM_DEFINE_FLOAT(RC2_DZ, 0.0f); PARAM_DEFINE_FLOAT(RC2_EXP, 0.0f); PARAM_DEFINE_FLOAT(RC3_MIN, 1000); PARAM_DEFINE_FLOAT(RC3_TRIM, 1500); PARAM_DEFINE_FLOAT(RC3_MAX, 2000); PARAM_DEFINE_FLOAT(RC3_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC3_DZ, 10.0f); +PARAM_DEFINE_FLOAT(RC3_DZ, 0.0f); PARAM_DEFINE_FLOAT(RC3_EXP, 0.0f); PARAM_DEFINE_FLOAT(RC4_MIN, 1000); PARAM_DEFINE_FLOAT(RC4_TRIM, 1500); PARAM_DEFINE_FLOAT(RC4_MAX, 2000); PARAM_DEFINE_FLOAT(RC4_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC4_DZ, 30.0f); +PARAM_DEFINE_FLOAT(RC4_DZ, 0.0f); PARAM_DEFINE_FLOAT(RC4_EXP, 0.0f); PARAM_DEFINE_FLOAT(RC5_MIN, 1000); @@ -103,21 +103,21 @@ PARAM_DEFINE_FLOAT(RC6_MIN, 1000); PARAM_DEFINE_FLOAT(RC6_TRIM, 1500); PARAM_DEFINE_FLOAT(RC6_MAX, 2000); PARAM_DEFINE_FLOAT(RC6_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC6_DZ, 0.1f); +PARAM_DEFINE_FLOAT(RC6_DZ, 0.0f); PARAM_DEFINE_FLOAT(RC6_EXP, 0.0f); PARAM_DEFINE_FLOAT(RC7_MIN, 1000); PARAM_DEFINE_FLOAT(RC7_TRIM, 1500); PARAM_DEFINE_FLOAT(RC7_MAX, 2000); PARAM_DEFINE_FLOAT(RC7_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC7_DZ, 0.1f); +PARAM_DEFINE_FLOAT(RC7_DZ, 0.0f); PARAM_DEFINE_FLOAT(RC7_EXP, 0.0f); PARAM_DEFINE_FLOAT(RC8_MIN, 1000); PARAM_DEFINE_FLOAT(RC8_TRIM, 1500); PARAM_DEFINE_FLOAT(RC8_MAX, 2000); PARAM_DEFINE_FLOAT(RC8_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC8_DZ, 0.1f); +PARAM_DEFINE_FLOAT(RC8_DZ, 0.0f); PARAM_DEFINE_FLOAT(RC8_EXP, 0.0f); PARAM_DEFINE_INT32(RC_TYPE, 1); // 1 = FUTABA diff --git a/apps/systemcmds/param/param.c b/apps/systemcmds/param/param.c index 9cb280933..d06eabae0 100644 --- a/apps/systemcmds/param/param.c +++ b/apps/systemcmds/param/param.c @@ -132,6 +132,9 @@ do_load(const char* param_file_name) if (fd < 0) err(1, "open '%s'", param_file_name); + /* set new default file name */ + param_set_default_file(param_file_name); + int result = param_load(fd); close(fd); diff --git a/apps/systemlib/pid/pid.c b/apps/systemlib/pid/pid.c index 0358caa25..dd8b6adc6 100644 --- a/apps/systemlib/pid/pid.c +++ b/apps/systemlib/pid/pid.c @@ -172,9 +172,9 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo // Calculate the output. Limit output magnitude to pid->limit float output = (pid->error_previous * pid->kp) + (i * pid->ki) + (d * pid->kd); - if (output > pid->limit) output = pid->limit; + //if (output > pid->limit) output = pid->limit; - if (output < -pid->limit) output = -pid->limit; + //if (output < -pid->limit) output = -pid->limit; if (isfinite(output)) { pid->last_output = output; |