fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-24 04:53:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Changed yaw position control to yaw speed control for multirotors, tested with ardrone

Browse Source
This commit is contained in:
Julian Oes
2012-11-19 16:17:52 -08:00
parent 2652b16d37
commit 129e6d73de
4 changed files with 43 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/multirotor_att_control/multirotor_att_control_main.c
+10 -4
View File
@@ -155,6 +155,9 @@ mc_thread_main(int argc, char *argv[])
/* store if yaw position or yaw speed has been changed */
bool control_yaw_position = true;
/* store if we stopped a yaw movement */
bool first_time_after_yaw_speed_control = true;
/* prepare the handle for the failsafe throttle */
param_t failsafe_throttle_handle = param_find("MC_RCLOSS_THR");
float failsafe_throttle = 0.0f;
@@ -249,7 +252,7 @@ mc_thread_main(int argc, char *argv[])
if (state.flag_control_attitude_enabled != flag_control_attitude_enabled ||
state.flag_control_manual_enabled != flag_control_manual_enabled ||
state.flag_system_armed != flag_system_armed) {
att_sp.yaw_body = att.yaw;
att_sp.yaw_tait_bryan = att.yaw;
}
static bool rc_loss_first_time = true;
@@ -294,16 +297,19 @@ mc_thread_main(int argc, char *argv[])
if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) {
rates_sp.yaw = manual.yaw;
control_yaw_position = false;
first_time_after_yaw_speed_control = true;
} else {
att_sp.yaw_body = 0.0f;
rates_sp.yaw = 0.0f;
if(first_time_after_yaw_speed_control) {
att_sp.yaw_tait_bryan = att.yaw;
first_time_after_yaw_speed_control = false;
}
control_yaw_position = true;
}
}
att_sp.thrust = manual.throttle;
att_sp.timestamp = hrt_absolute_time();
//rates_sp.yaw = manual.yaw;
}
}
/* STEP 2: publish the result to the vehicle actuators */
apps/multirotor_att_control/multirotor_attitude_control.c
+11 -1
View File
@@ -181,6 +181,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
static bool initialized = false;
static float yaw_error;
/* initialize the pid controllers when the function is called for the first time */
if (initialized == false) {
parameters_init(&h);
@@ -218,7 +220,15 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
if(control_yaw_position) {
/* control yaw rate */
rates_sp->yaw = p.yaw_p * atan2f(cosf(att->yaw - att_sp->yaw_body), sinf(att->yaw - att_sp->yaw_body)) - (p.yaw_d * att->yawspeed);
//rates_sp->yaw = p.yaw_p * atan2f(cosf(att->yaw - att_sp->yaw_tait_bryan), sinf(att->yaw - att_sp->yaw_tait_bryan)) - (p.yaw_d * att->yawspeed);
yaw_error = att->yaw - att_sp->yaw_tait_bryan;
if ((double)yaw_error > M_PI) {
yaw_error -= M_PI;
} else if ((double)yaw_error < -M_PI) {
yaw_error += M_PI;
}
rates_sp->yaw = - p.yaw_p * (yaw_error) - (p.yaw_d * att->yawspeed);
}
rates_sp->thrust = att_sp->thrust;