From 291f4f3a33e6428b23624b1ffe12fec1015816cd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 19 Sep 2012 18:53:29 +0200 Subject: Reworked control interface, needs testing / validation --- .../multirotor_att_control_main.c | 31 +++++++++++++++------- .../multirotor_attitude_control.c | 2 +- .../multirotor_rate_control.c | 8 +++--- .../multirotor_rate_control.h | 4 +-- 4 files changed, 29 insertions(+), 16 deletions(-) (limited to 'apps/multirotor_att_control') diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index 530033701..3f0d56245 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -58,13 +58,14 @@ #include #include #include -#include +#include #include #include #include #include "multirotor_attitude_control.h" +#include "multirotor_rate_control.h" __EXPORT int multirotor_att_control_main(int argc, char *argv[]); @@ -93,18 +94,18 @@ mc_thread_main(int argc, char *argv[]) memset(&manual, 0, sizeof(manual)); struct sensor_combined_s raw; memset(&raw, 0, sizeof(raw)); - struct ardrone_motors_setpoint_s setpoint; - memset(&setpoint, 0, sizeof(setpoint)); + struct vehicle_rates_setpoint_s rates_sp; + memset(&rates_sp, 0, sizeof(rates_sp)); struct actuator_controls_s actuators; /* subscribe to attitude, motor setpoints and system state */ int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); + int setpoint_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); int state_sub = orb_subscribe(ORB_ID(vehicle_status)); int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); - int setpoint_sub = orb_subscribe(ORB_ID(ardrone_motors_setpoint)); /* * Do not rate-limit the loop to prevent aliasing @@ -145,6 +146,12 @@ mc_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); /* get a local copy of attitude setpoint */ orb_copy(ORB_ID(vehicle_attitude_setpoint), att_setpoint_sub, &att_sp); + /* get a local copy of rates setpoint */ + bool updated; + orb_check(setpoint_sub, &updated); + if (updated) { + orb_copy(ORB_ID(vehicle_rates_setpoint), setpoint_sub, &rates_sp); + } /* get a local copy of the current sensor values */ orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw); @@ -155,16 +162,15 @@ mc_thread_main(int argc, char *argv[]) /* manual inputs, from RC control or joystick */ att_sp.roll_body = manual.roll; att_sp.pitch_body = manual.pitch; - att_sp.yaw_rate_body = manual.yaw; + att_sp.yaw_body = manual.yaw; // XXX Hack, remove, switch to yaw rate controller + /* set yaw rate */ + rates_sp.yaw = manual.yaw; att_sp.timestamp = hrt_absolute_time(); if (motor_test_mode) { att_sp.roll_body = 0.0f; att_sp.pitch_body = 0.0f; att_sp.yaw_body = 0.0f; - att_sp.roll_rate_body = 0.0f; - att_sp.pitch_rate_body = 0.0f; - att_sp.yaw_rate_body = 0.0f; att_sp.thrust = 0.1f; } else { att_sp.thrust = manual.throttle; @@ -184,7 +190,14 @@ mc_thread_main(int argc, char *argv[]) /* XXX could be also run in an independent loop */ if (state.flag_control_rates_enabled) { - multirotor_control_rates(&att_sp, &raw.gyro_rad_s, &actuators); + /* lowpass gyros */ + // XXX + static float gyro_lp[3] = {0, 0, 0}; + gyro_lp[0] = raw.gyro_rad_s[0]; + gyro_lp[1] = raw.gyro_rad_s[1]; + gyro_lp[2] = raw.gyro_rad_s[2]; + + multirotor_control_rates(&rates_sp, gyro_lp, &actuators); } /* STEP 3: publish the result to the vehicle actuators */ diff --git a/apps/multirotor_att_control/multirotor_attitude_control.c b/apps/multirotor_att_control/multirotor_attitude_control.c index 997aa0c10..d7413913b 100644 --- a/apps/multirotor_att_control/multirotor_attitude_control.c +++ b/apps/multirotor_att_control/multirotor_attitude_control.c @@ -243,7 +243,7 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s float roll_control = pid_calculate(&roll_controller, att_sp->roll_body + p.att_yoff, att->roll, att->rollspeed, deltaT); /* control yaw rate */ - float yaw_rate_control = pid_calculate(&yaw_speed_controller, att_sp->yaw_rate_body, att->yawspeed, 0.0f, deltaT); + float yaw_rate_control = pid_calculate(&yaw_speed_controller, att_sp->yaw_body, att->yawspeed, 0.0f, deltaT); /* * compensate the vertical loss of thrust diff --git a/apps/multirotor_att_control/multirotor_rate_control.c b/apps/multirotor_att_control/multirotor_rate_control.c index 8a3ac78cd..fefd1f767 100644 --- a/apps/multirotor_att_control/multirotor_rate_control.c +++ b/apps/multirotor_att_control/multirotor_rate_control.c @@ -132,7 +132,7 @@ static int parameters_update(const struct mc_rate_control_param_handles *h, stru return OK; } -void multirotor_control_rates(const struct vehicle_attitude_setpoint_s *rate_sp, +void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, const float rates[], struct actuator_controls_s *actuators) { static uint64_t last_run = 0; @@ -179,13 +179,13 @@ void multirotor_control_rates(const struct vehicle_attitude_setpoint_s *rate_sp, /* calculate current control outputs */ /* control pitch (forward) output */ - float pitch_control = pid_calculate(&pitch_controller, rate_sp->pitch_rate_body, + float pitch_control = pid_calculate(&pitch_controller, rate_sp->pitch, rates[1], 0.0f, deltaT); /* control roll (left/right) output */ - float roll_control = pid_calculate(&roll_controller, rate_sp->roll_rate_body, + float roll_control = pid_calculate(&roll_controller, rate_sp->roll, rates[0], 0.0f, deltaT); /* control yaw rate */ - float yaw_rate_control = pid_calculate(&yaw_speed_controller, rate_sp->yaw_rate_body, rates[2], 0.0f, deltaT); + float yaw_rate_control = pid_calculate(&yaw_speed_controller, rate_sp->yaw, rates[2], 0.0f, deltaT); /* * compensate the vertical loss of thrust diff --git a/apps/multirotor_att_control/multirotor_rate_control.h b/apps/multirotor_att_control/multirotor_rate_control.h index 6f34d99e7..3c3c50801 100644 --- a/apps/multirotor_att_control/multirotor_rate_control.h +++ b/apps/multirotor_att_control/multirotor_rate_control.h @@ -47,10 +47,10 @@ #include #include -#include +#include #include -void multirotor_control_rates(const struct vehicle_attitude_setpoint_s *rate_sp, +void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, const float rates[], struct actuator_controls_s *actuators); #endif /* MULTIROTOR_RATE_CONTROL_H_ */ -- cgit v1.2.3