Browse Source

Cleanup: remove useless angular rates from attitude rate controller

sbg
Anton Babushkin 12 years ago
parent
commit
dec1fdbde0
  1. 8
      src/modules/multirotor_att_control/multirotor_att_control_main.c
  2. 6
      src/modules/multirotor_att_control/multirotor_rate_control.c
  3. 2
      src/modules/multirotor_att_control/multirotor_rate_control.h

8
src/modules/multirotor_att_control/multirotor_att_control_main.c

@ -395,13 +395,7 @@ mc_thread_main(int argc, char *argv[]) @@ -395,13 +395,7 @@ mc_thread_main(int argc, char *argv[])
rates[1] = att.pitchspeed;
rates[2] = att.yawspeed;
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);
multirotor_control_rates(&rates_sp, rates, &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);

6
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 @@ -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[], const float rates_acc[], struct actuator_controls_s *actuators,
const float rates[], 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;
@ -208,11 +208,11 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, @@ -208,11 +208,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], rates_acc[1], deltaT, &control_debug->pitch_rate_p, &control_debug->pitch_rate_i, &control_debug->pitch_rate_d);
rates[1], 0.0f, 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], rates_acc[0], deltaT, &control_debug->roll_rate_p, &control_debug->roll_rate_i, &control_debug->roll_rate_d);
rates[0], 0.0f, 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)) {

2
src/modules/multirotor_att_control/multirotor_rate_control.h

@ -52,7 +52,7 @@ @@ -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[], const float rates_acc[], struct actuator_controls_s *actuators,
const float rates[], struct actuator_controls_s *actuators,
const orb_advert_t *control_debug_pub, struct vehicle_control_debug_s *control_debug);
#endif /* MULTIROTOR_RATE_CONTROL_H_ */

Loading…
Cancel
Save