dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

multirotor_att_control: yaw setpoint reset fix

This commit is contained in:
Anton Babushkin 2013-08-22 17:31:59 +02:00
parent db950f7489
commit b5bb20995b
1 changed files with 52 additions and 49 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

75
src/modules/multirotor_att_control/multirotor_att_control_main.c
View File

@ -81,6 +81,8 @@ __EXPORT int multirotor_att_control_main(int argc, char *argv[]);
static bool thread_should_exit; static bool thread_should_exit;
static int mc_task; static int mc_task;
static bool motor_test_mode = false; static bool motor_test_mode = false;
static const float min_takeoff_throttle = 0.3f;
static const float yaw_deadzone = 0.01f;
static int static int
mc_thread_main(int argc, char *argv[]) mc_thread_main(int argc, char *argv[])
@ -147,14 +149,14 @@ mc_thread_main(int argc, char *argv[])
/* store last control mode to detect mode switches */ /* store last control mode to detect mode switches */
bool flag_control_manual_enabled = false; bool flag_control_manual_enabled = false;
bool flag_control_attitude_enabled = false; bool flag_control_attitude_enabled = false;
bool control_yaw_position = true; bool control_yaw_position = true;
bool reset_yaw_sp = true; bool reset_yaw_sp = true;
bool failsafe_first_time = true;
/* prepare the handle for the failsafe throttle */ /* prepare the handle for the failsafe throttle */
param_t failsafe_throttle_handle = param_find("MC_RCLOSS_THR"); param_t failsafe_throttle_handle = param_find("MC_RCLOSS_THR");
float failsafe_throttle = 0.0f; float failsafe_throttle = 0.0f;
param_get(failsafe_throttle_handle, &failsafe_throttle);
while (!thread_should_exit) { while (!thread_should_exit) {
@ -176,7 +178,7 @@ mc_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(parameter_update), param_sub, &update); orb_copy(ORB_ID(parameter_update), param_sub, &update);
/* update parameters */ /* update parameters */
// XXX no params here yet param_get(failsafe_throttle_handle, &failsafe_throttle);
} }
/* only run controller if attitude changed */ /* only run controller if attitude changed */
@ -208,6 +210,9 @@ mc_thread_main(int argc, char *argv[])
/* get a local copy of the current sensor values */ /* get a local copy of the current sensor values */
orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw); orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw);
/* set flag to safe value */
control_yaw_position = true;
/* define which input is the dominating control input */ /* define which input is the dominating control input */
if (control_mode.flag_control_offboard_enabled) { if (control_mode.flag_control_offboard_enabled) {
/* offboard inputs */ /* offboard inputs */
@ -225,31 +230,26 @@ mc_thread_main(int argc, char *argv[])
att_sp.yaw_body = offboard_sp.p3; att_sp.yaw_body = offboard_sp.p3;
att_sp.thrust = offboard_sp.p4; att_sp.thrust = offboard_sp.p4;
att_sp.timestamp = hrt_absolute_time(); att_sp.timestamp = hrt_absolute_time();
/* STEP 2: publish the result to the vehicle actuators */ /* publish the result to the vehicle actuators */
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
} }
} else if (control_mode.flag_control_manual_enabled) { } else if (control_mode.flag_control_manual_enabled) {
/* direct manual input */ /* manual input */
if (control_mode.flag_control_attitude_enabled) { if (control_mode.flag_control_attitude_enabled) {
/* control attitude, update attitude setpoint depending on mode */ /* control attitude, update attitude setpoint depending on mode */
/* initialize to current yaw if switching to manual or att control */
if (control_mode.flag_control_attitude_enabled != flag_control_attitude_enabled ||
control_mode.flag_control_manual_enabled != flag_control_manual_enabled) {
att_sp.yaw_body = att.yaw;
}
static bool rc_loss_first_time = true;
/* if the RC signal is lost, try to stay level and go slowly back down to ground */ /* if the RC signal is lost, try to stay level and go slowly back down to ground */
if (control_mode.failsave_highlevel) { if (control_mode.failsave_highlevel) {
failsafe_first_time = false;
if (!control_mode.flag_control_velocity_enabled) { if (!control_mode.flag_control_velocity_enabled) {
/* Don't reset attitude setpoint in position control mode, it's handled by position controller. */ /* don't reset attitude setpoint in position control mode, it's handled by position controller. */
att_sp.roll_body = 0.0f; att_sp.roll_body = 0.0f;
att_sp.pitch_body = 0.0f; att_sp.pitch_body = 0.0f;
}
if (!control_mode.flag_control_climb_rate_enabled) { if (!control_mode.flag_control_climb_rate_enabled) {
/* Don't touch throttle in modes with altitude hold, it's handled by position controller. /* don't touch throttle in modes with altitude hold, it's handled by position controller.
* *
* Only go to failsafe throttle if last known throttle was * Only go to failsafe throttle if last known throttle was
* high enough to create some lift to make hovering state likely. * high enough to create some lift to make hovering state likely.
@ -258,61 +258,61 @@ mc_thread_main(int argc, char *argv[])
* multicopter (throttle = 0) does not make it jump up in the air * multicopter (throttle = 0) does not make it jump up in the air
* if shutting down his remote. * if shutting down his remote.
*/ */
if (isfinite(manual.throttle) && manual.throttle > 0.2f) { // TODO use landed status instead of throttle if (isfinite(manual.throttle) && manual.throttle > min_takeoff_throttle) { // TODO use landed status instead of throttle
/* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */ /* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */
param_get(failsafe_throttle_handle, &failsafe_throttle);
att_sp.thrust = failsafe_throttle; att_sp.thrust = failsafe_throttle;
} else { } else {
att_sp.thrust = 0.0f; att_sp.thrust = 0.0f;
} }
} }
}
/* keep current yaw, do not attempt to go to north orientation, /* keep current yaw, do not attempt to go to north orientation,
* since if the pilot regains RC control, he will be lost regarding * since if the pilot regains RC control, he will be lost regarding
* the current orientation. * the current orientation.
*/ */
if (rc_loss_first_time) if (failsafe_first_time) {
att_sp.yaw_body = att.yaw; reset_yaw_sp = true;
}
rc_loss_first_time = false;
} else { } else {
rc_loss_first_time = true; failsafe_first_time = true;
/* control yaw in all manual / assisted modes */ /* control yaw in all manual / assisted modes */
/* set yaw if arming or switching to attitude stabilized mode */ /* set yaw if arming or switching to attitude stabilized mode */
if (!flag_control_attitude_enabled) { if (!flag_control_manual_enabled || !flag_control_attitude_enabled || !control_mode.flag_armed) {
reset_yaw_sp = true; reset_yaw_sp = true;
} }
/* only move setpoint if manual input is != 0 */ /* only move setpoint if manual input is != 0 */
if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) { // TODO use landed status instead of throttle // TODO review yaw restpoint reset
rates_sp.yaw = manual.yaw; if ((manual.yaw < -yaw_deadzone || yaw_deadzone < manual.yaw) && manual.throttle > min_takeoff_throttle) {
/* control yaw rate */
control_yaw_position = false; control_yaw_position = false;
reset_yaw_sp = true; rates_sp.yaw = manual.yaw;
reset_yaw_sp = true; // has no effect on control, just for beautiful log
} else { } else {
if (reset_yaw_sp) {
att_sp.yaw_body = att.yaw;
reset_yaw_sp = false;
}
control_yaw_position = true; control_yaw_position = true;
} }
if (!control_mode.flag_control_velocity_enabled) { if (!control_mode.flag_control_velocity_enabled) {
/* don't update attitude setpoint in position control mode */ /* update attitude setpoint if not in position control mode */
att_sp.roll_body = manual.roll; att_sp.roll_body = manual.roll;
att_sp.pitch_body = manual.pitch; att_sp.pitch_body = manual.pitch;
if (!control_mode.flag_control_climb_rate_enabled) { if (!control_mode.flag_control_climb_rate_enabled) {
/* don't set throttle in altitude hold modes */ /* pass throttle directly if not in altitude control mode */
att_sp.thrust = manual.throttle; att_sp.thrust = manual.throttle;
} }
} }
att_sp.timestamp = hrt_absolute_time(); }
/* reset yaw setpint to current position if needed */
if (reset_yaw_sp) {
att_sp.yaw_body = att.yaw;
reset_yaw_sp = false;
} }
if (motor_test_mode) { if (motor_test_mode) {
@ -321,10 +321,11 @@ mc_thread_main(int argc, char *argv[])
att_sp.pitch_body = 0.0f; att_sp.pitch_body = 0.0f;
att_sp.yaw_body = 0.0f; att_sp.yaw_body = 0.0f;
att_sp.thrust = 0.1f; att_sp.thrust = 0.1f;
att_sp.timestamp = hrt_absolute_time();
} }
/* STEP 2: publish the controller output */ att_sp.timestamp = hrt_absolute_time();
/* publish the controller output */
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
} else { } else {
@ -367,6 +368,7 @@ mc_thread_main(int argc, char *argv[])
rates[1] = att.pitchspeed; rates[1] = att.pitchspeed;
rates[2] = att.yawspeed; rates[2] = att.yawspeed;
multirotor_control_rates(&rates_sp, rates, &actuators, reset_integral); multirotor_control_rates(&rates_sp, rates, &actuators, reset_integral);
} else { } else {
/* rates controller disabled, set actuators to zero for safety */ /* rates controller disabled, set actuators to zero for safety */
actuators.control[0] = 0.0f; actuators.control[0] = 0.0f;
@ -374,6 +376,7 @@ mc_thread_main(int argc, char *argv[])
actuators.control[2] = 0.0f; actuators.control[2] = 0.0f;
actuators.control[3] = 0.0f; actuators.control[3] = 0.0f;
} }
actuators.timestamp = hrt_absolute_time(); actuators.timestamp = hrt_absolute_time();
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);