diff options
author | Julian Oes <julian@oes.ch> | 2013-06-19 09:35:59 +0200 |
---|---|---|
committer | Julian Oes <julian@oes.ch> | 2013-06-19 09:35:59 +0200 |
commit | e2ff60b0a6dbcd714d57e781d9fe174b110a6237 (patch) | |
tree | b0fe3672dc180fcabead8a97b46f99db7cc82a94 /src/modules/multirotor_att_control | |
parent | f3ce61d7405a7edc1e5bb2aadf2ccec5bed90feb (diff) | |
download | px4-firmware-e2ff60b0a6dbcd714d57e781d9fe174b110a6237.tar.gz px4-firmware-e2ff60b0a6dbcd714d57e781d9fe174b110a6237.tar.bz2 px4-firmware-e2ff60b0a6dbcd714d57e781d9fe174b110a6237.zip |
Use rollacc and pitchacc from the estimator instead of differentiating in the controller
Diffstat (limited to 'src/modules/multirotor_att_control')
3 files changed, 19 insertions, 12 deletions
diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c index c3b2f5d2a..dd38242d3 100644 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c @@ -394,7 +394,8 @@ mc_thread_main(int argc, char *argv[]) /* measure in what intervals the controller runs */ perf_count(mc_interval_perf); - float gyro[3]; + float rates[3]; + float rates_acc[3]; /* get current rate setpoint */ bool rates_sp_valid = false; @@ -405,11 +406,17 @@ mc_thread_main(int argc, char *argv[]) } /* apply controller */ - gyro[0] = att.rollspeed; - gyro[1] = att.pitchspeed; - gyro[2] = att.yawspeed; + rates[0] = att.rollspeed; + rates[1] = att.pitchspeed; + rates[2] = att.yawspeed; - multirotor_control_rates(&rates_sp, gyro, &actuators, &control_debug_pub, &control_debug); + rates_acc[0] = att.rollacc; + rates_acc[1] = att.pitchacc; + rates_acc[2] = att.yawacc; + + + + multirotor_control_rates(&rates_sp, rates, rates_acc, &actuators, &control_debug_pub, &control_debug); orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); orb_publish(ORB_ID(vehicle_control_debug), control_debug_pub, &control_debug); diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c index a39d6f13d..e37ede3e0 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.c +++ b/src/modules/multirotor_att_control/multirotor_rate_control.c @@ -151,7 +151,7 @@ static int parameters_update(const struct mc_rate_control_param_handles *h, stru } void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, - const float rates[], struct actuator_controls_s *actuators, + const float rates[], const float rates_acc[], struct actuator_controls_s *actuators, const orb_advert_t *control_debug_pub, struct vehicle_control_debug_s *control_debug) { static uint64_t last_run = 0; @@ -186,8 +186,8 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, parameters_update(&h, &p); initialized = true; - pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f, PID_MODE_DERIVATIV_CALC); - pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f, PID_MODE_DERIVATIV_CALC); + pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f, PID_MODE_DERIVATIV_SET); + pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f, PID_MODE_DERIVATIV_SET); } @@ -201,11 +201,11 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, /* control pitch (forward) output */ float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch , - rates[1], 0.0f, deltaT, &control_debug->pitch_rate_p, &control_debug->pitch_rate_i, &control_debug->pitch_rate_d); + rates[1], rates_acc[1], deltaT, &control_debug->pitch_rate_p, &control_debug->pitch_rate_i, &control_debug->pitch_rate_d); /* control roll (left/right) output */ float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll , - rates[0], 0.0f, deltaT, &control_debug->roll_rate_p, &control_debug->roll_rate_i, &control_debug->roll_rate_d); + rates[0], rates_acc[0], deltaT, &control_debug->roll_rate_p, &control_debug->roll_rate_i, &control_debug->roll_rate_d); /* increase resilience to faulty control inputs */ if (isfinite(pitch_control)) { @@ -225,7 +225,7 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, warnx("rej. NaN ctrl roll"); } - /* control yaw rate */ + /* control yaw rate */ //XXX use library here and use rates_acc[2] float yaw_rate_control = p.yawrate_p * (rate_sp->yaw - rates[2]); /* increase resilience to faulty control inputs */ diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.h b/src/modules/multirotor_att_control/multirotor_rate_control.h index 465549540..fc741c378 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.h +++ b/src/modules/multirotor_att_control/multirotor_rate_control.h @@ -52,7 +52,7 @@ #include <uORB/topics/vehicle_control_debug.h> void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, - const float rates[], struct actuator_controls_s *actuators, + const float rates[], const float rates_acc[], struct actuator_controls_s *actuators, const orb_advert_t *control_debug_pub, struct vehicle_control_debug_s *control_debug); #endif /* MULTIROTOR_RATE_CONTROL_H_ */ |