diff options
author | daregger <daregger@student.ethz.ch> | 2012-10-16 16:49:45 +0200 |
---|---|---|
committer | daregger <daregger@student.ethz.ch> | 2012-10-16 16:49:45 +0200 |
commit | b50bc7798ac463de3e0c3147df46a3f7227df8c3 (patch) | |
tree | 7a7823cf52632136814214509f8f1d07192b8915 /apps/multirotor_att_control | |
parent | 0b26ca84d451adfdf80e956fc1b199def17aafd9 (diff) | |
download | px4-firmware-b50bc7798ac463de3e0c3147df46a3f7227df8c3.tar.gz px4-firmware-b50bc7798ac463de3e0c3147df46a3f7227df8c3.tar.bz2 px4-firmware-b50bc7798ac463de3e0c3147df46a3f7227df8c3.zip |
Wip on inner rate loop
Diffstat (limited to 'apps/multirotor_att_control')
-rw-r--r--[-rwxr-xr-x] | apps/multirotor_att_control/multirotor_rate_control.c | 41 |
1 files changed, 28 insertions, 13 deletions
diff --git a/apps/multirotor_att_control/multirotor_rate_control.c b/apps/multirotor_att_control/multirotor_rate_control.c index 2809c4533..c132665d2 100755..100644 --- a/apps/multirotor_att_control/multirotor_rate_control.c +++ b/apps/multirotor_att_control/multirotor_rate_control.c @@ -1,8 +1,8 @@ /**************************************************************************** * * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Tobias Naegeli <naegelit@student.ethz.ch> - * @author Lorenz Meier <lm@inf.ethz.ch> + * Author: Tobias Naegeli <naegelit@student.ethz.ch> + * Lorenz Meier <lm@inf.ethz.ch> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,7 +35,11 @@ /** * @file multirotor_rate_control.c + * * Implementation of rate controller + * + * @author Tobias Naegeli <naegelit@student.ethz.ch> + * @author Lorenz Meier <lm@inf.ethz.ch> */ #include "multirotor_rate_control.h" @@ -50,10 +54,11 @@ #include <systemlib/param/param.h> #include <arch/board/up_hrt.h> - PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 20.0f); /* same on Flamewheel */ - PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f); - PARAM_DEFINE_FLOAT(MC_YAWRATE_AWU, 0.0f); - PARAM_DEFINE_FLOAT(MC_YAWRATE_LIM, 0.f); +PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 20.0f); /* 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, 40.0f); PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 40.0f); /* 0.15 F405 Flamewheel */ PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.05f); @@ -144,8 +149,8 @@ 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) { - static float roll_control_last=0; - static float pitch_control_last=0; + static float roll_control_last = 0; + static float pitch_control_last = 0; static uint64_t last_run = 0; const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; last_run = hrt_absolute_time(); @@ -174,15 +179,25 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, /* calculate current control outputs */ /* control pitch (forward) output */ - float pitch_control = p.attrate_p * deltaT *((rate_sp->pitch)*p.attrate_lim-rates[1])-p.attrate_d*(pitch_control_last); - pitch_control_last=pitch_control; - /* control roll (left/right) output */ + /* increase resilience to faulty control inputs */ + if (isfinite(pitch_control)) { + pitch_control_last = pitch_control; + } else { + pitch_control = 0.0f; + } + /* control roll (left/right) output */ float roll_control = p.attrate_p * deltaT * ((rate_sp->roll)*p.attrate_lim-rates[0])-p.attrate_d*(roll_control_last); - roll_control_last=roll_control; + /* increase resilience to faulty control inputs */ + if (isfinite(roll_control)) { + roll_control_last = roll_control; + } else { + roll_control = 0.0f; + } + /* control yaw rate */ - float yaw_rate_control = p.yawrate_p * deltaT * ((rate_sp->yaw)*p.attrate_lim-rates[2] ); + float yaw_rate_control = p.yawrate_p * deltaT * ((rate_sp->yaw)*p.attrate_lim-rates[2]); actuators->control[0] = roll_control; actuators->control[1] = pitch_control; |