fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-22 02:37:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Abs yaw and other bugs fixed

Browse Source
This commit is contained in:
Anton Babushkin
2013-04-20 12:33:02 +04:00
parent 5abae2c78d
commit 57cca9dbb4
2 changed files with 199 additions and 162 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/multirotor_att_control/multirotor_att_control_main.c
+112 -95
View File
@@ -241,123 +241,140 @@ mc_thread_main(int argc, char *argv[])
} else if (state.flag_control_manual_enabled) {
/* direct manual input */
if (state.flag_control_attitude_enabled) {
/* Control attitude, update attitude setpoint depending on SAS mode:
* VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS: roll, pitch, yaw, thrust
* VEHICLE_MANUAL_SAS_MODE_ALTITUDE: roll, pitch, yaw
* VEHICLE_MANUAL_SAS_MODE_SIMPLE: yaw
* */
if (state.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS) {
/* direct manual input */
if (state.flag_control_attitude_enabled) {
/* initialize to current yaw if switching to manual or att control */
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;
}
/* initialize to current yaw if switching to manual or att control */
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) {
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 (state.rc_signal_lost) {
if (state.manual_sas_mode != VEHICLE_MANUAL_SAS_MODE_SIMPLE) {
/* Don't reset attitude setpoint in SIMPLE SAS mode, it's handled by position controller. */
att_sp.roll_body = 0.0f;
att_sp.pitch_body = 0.0f;
if (state.manual_sas_mode != VEHICLE_MANUAL_SAS_MODE_ALTITUDE) {
/* 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
* high enough to create some lift to make hovering state likely.
*
* This is to prevent that someone landing, but not disarming his
* multicopter (throttle = 0) does not make it jump up in the air
* if shutting down his remote.
*/
if (isfinite(manual.throttle) && manual.throttle > 0.2f) {
/* 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;
} else {
att_sp.thrust = 0.0f;
}
}
}
/* keep current yaw, do not attempt to go to north orientation,
* since if the pilot regains RC control, he will be lost regarding
* the current orientation.
*/
if (rc_loss_first_time)
att_sp.yaw_body = att.yaw;
rc_loss_first_time = false;
} else {
rc_loss_first_time = true;
/* control yaw in all SAS modes */
/* set yaw if arming */
if (!flag_control_attitude_enabled && state.flag_system_armed) {
att_sp.yaw_body = att.yaw;
}
static bool rc_loss_first_time = true;
/* act if stabilization is active or if the (nonsense) direct pass through mode is set */
if (state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS ||
state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
/* if the RC signal is lost, try to stay level and go slowly back down to ground */
if (state.rc_signal_lost) {
/* 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.roll_body = 0.0f;
att_sp.pitch_body = 0.0f;
/*
* Only go to failsafe throttle if last known throttle was
* high enough to create some lift to make hovering state likely.
*
* This is to prevent that someone landing, but not disarming his
* multicopter (throttle = 0) does not make it jump up in the air
* if shutting down his remote.
*/
if (isfinite(manual.throttle) && manual.throttle > 0.2f) {
att_sp.thrust = failsafe_throttle;
if (state.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE) {
rates_sp.yaw = manual.yaw;
control_yaw_position = false;
} else {
att_sp.thrust = 0.0f;
}
/*
* This mode SHOULD be the default mode, which is:
* VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS
*
* However, we fall back to this setting for all other (nonsense)
* settings as well.
*/
/* keep current yaw, do not attempt to go to north orientation,
* since if the pilot regains RC control, he will be lost regarding
* the current orientation.
*/
if (rc_loss_first_time)
att_sp.yaw_body = att.yaw;
rc_loss_first_time = false;
} else {
rc_loss_first_time = true;
att_sp.roll_body = manual.roll;
att_sp.pitch_body = manual.pitch;
/* set attitude if arming */
if (!flag_control_attitude_enabled && state.flag_system_armed) {
att_sp.yaw_body = att.yaw;
}
/* act if stabilization is active or if the (nonsense) direct pass through mode is set */
if (state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS ||
state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
if (state.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE) {
/* only move setpoint if manual input is != 0 */
if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && (
state.manual_sas_mode != VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS ||
manual.throttle > 0.3f)) {
rates_sp.yaw = manual.yaw;
control_yaw_position = false;
first_time_after_yaw_speed_control = true;
} else {
/*
* This mode SHOULD be the default mode, which is:
* VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS
*
* However, we fall back to this setting for all other (nonsense)
* settings as well.
*/
/* only move setpoint if manual input is != 0 */
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 {
if (first_time_after_yaw_speed_control) {
att_sp.yaw_body = att.yaw;
first_time_after_yaw_speed_control = false;
}
control_yaw_position = true;
if (first_time_after_yaw_speed_control) {
att_sp.yaw_body = att.yaw;
first_time_after_yaw_speed_control = false;
}
control_yaw_position = true;
}
}
att_sp.thrust = manual.throttle;
att_sp.timestamp = hrt_absolute_time();
}
if (motor_test_mode) {
printf("testmode");
att_sp.roll_body = 0.0f;
att_sp.pitch_body = 0.0f;
att_sp.yaw_body = 0.0f;
att_sp.thrust = 0.1f;
att_sp.timestamp = hrt_absolute_time();
if (state.manual_sas_mode != VEHICLE_MANUAL_SAS_MODE_SIMPLE) {
/* don't update attitude setpoint in SIMPLE SAS mode */
att_sp.roll_body = manual.roll;
att_sp.pitch_body = manual.pitch;
if (state.manual_sas_mode != VEHICLE_MANUAL_SAS_MODE_ALTITUDE) {
/* don't set throttle in alt hold modes */
att_sp.thrust = manual.throttle;
}
}
/* STEP 2: publish the controller output */
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
att_sp.timestamp = hrt_absolute_time();
}
} else {
/* manual rate inputs, from RC control or joystick */
if (state.flag_control_rates_enabled &&
state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_RATES) {
rates_sp.roll = manual.roll;
if (motor_test_mode) {
printf("testmode");
att_sp.roll_body = 0.0f;
att_sp.pitch_body = 0.0f;
att_sp.yaw_body = 0.0f;
att_sp.thrust = 0.1f;
att_sp.timestamp = hrt_absolute_time();
}
rates_sp.pitch = manual.pitch;
rates_sp.yaw = manual.yaw;
rates_sp.thrust = manual.throttle;
rates_sp.timestamp = hrt_absolute_time();
}
/* STEP 2: publish the controller output */
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
} else {
/* manual rate inputs, from RC control or joystick */
if (state.flag_control_rates_enabled &&
state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_RATES) {
rates_sp.roll = manual.roll;
rates_sp.pitch = manual.pitch;
rates_sp.yaw = manual.yaw;
rates_sp.thrust = manual.throttle;
rates_sp.timestamp = hrt_absolute_time();
}
}
}
apps/multirotor_pos_control/multirotor_pos_control.c
+87 -67
View File
@@ -162,6 +162,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) {
int param_sub = orb_subscribe(ORB_ID(parameter_update));
int state_sub = orb_subscribe(ORB_ID(vehicle_status));
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
int local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
int local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
@@ -186,18 +187,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) {
int paramcheck_counter = 0;
while (!thread_should_exit) {
/* get a local copy of the vehicle state */
orb_copy(ORB_ID(vehicle_status), state_sub, &status);
/* get a local copy of manual setpoint */
orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual);
/* get a local copy of attitude */
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
/* get a local copy of local position */
orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos);
if (status.state_machine == SYSTEM_STATE_AUTO) {
/* get a local copy of local position setpoint */
orb_copy(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, &local_pos_sp);
}
/* check parameters at 1 Hz*/
paramcheck_counter++;
@@ -210,26 +200,58 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) {
paramcheck_counter = 0;
}
if (status.state_machine == SYSTEM_STATE_MANUAL) {
if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ALTITUDE || status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE) {
hrt_abstime t = hrt_absolute_time();
if (reset_sp_alt) {
reset_sp_alt = false;
local_pos_sp.z = local_pos.z;
alt_integral = manual.throttle;
char str[80];
sprintf(str, "reset alt setpoint: z = %.2f, throttle = %.2f", local_pos_sp.z, manual.throttle);
mavlink_log_info(mavlink_fd, str);
}
/* Check if controller should act */
bool act = status.flag_system_armed && (
/* SAS modes */
(
status.flag_control_manual_enabled &&
status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS && (
status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ALTITUDE ||
status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE
)
) ||
/* AUTO mode */
status.state_machine == SYSTEM_STATE_AUTO
);
float dt;
if (t_prev != 0) {
dt = (t - t_prev) * 0.000001f;
} else {
dt = 0.0f;
}
float err_linear_limit = params.alt_d / params.alt_p * params.alt_rate_max;
/* move altitude setpoint by manual controls */
if (act) {
orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual);
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp);
orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos);
if (status.state_machine == SYSTEM_STATE_AUTO) {
orb_copy(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, &local_pos_sp);
}
hrt_abstime t = hrt_absolute_time();
if (reset_sp_alt) {
reset_sp_alt = false;
local_pos_sp.z = local_pos.z;
alt_integral = manual.throttle;
char str[80];
sprintf(str, "reset alt setpoint: z = %.2f, throttle = %.2f", local_pos_sp.z, manual.throttle);
mavlink_log_info(mavlink_fd, str);
}
if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE && reset_sp_pos) {
reset_sp_pos = false;
local_pos_sp.x = local_pos.x;
local_pos_sp.y = local_pos.y;
char str[80];
sprintf(str, "reset pos setpoint: x = %.2f, y = %.2f", local_pos_sp.x, local_pos_sp.y);
mavlink_log_info(mavlink_fd, str);
}
float dt;
if (t_prev != 0) {
dt = (t - t_prev) * 0.000001f;
} else {
dt = 0.0f;
}
float err_linear_limit = params.alt_d / params.alt_p * params.alt_rate_max;
if (status.flag_control_manual_enabled) {
/* move altitude setpoint with manual controls */
float alt_ctl = manual.throttle - 0.5f;
if (fabs(alt_ctl) < alt_ctl_dz) {
alt_ctl = 0.0f;
@@ -245,48 +267,46 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) {
local_pos_sp.z = local_pos.z - err_linear_limit;
}
/* PID for altitude */
float alt_err = local_pos.z - local_pos_sp.z;
/* don't accelerate more than ALT_RATE_MAX, limit error to corresponding value */
if (alt_err > err_linear_limit) {
alt_err = err_linear_limit;
} else if (alt_err < -err_linear_limit) {
alt_err = -err_linear_limit;
}
/* PID for altitude rate */
float thrust_ctl_pd = alt_err * params.alt_p + local_pos.vz * params.alt_d;
float thrust_ctl = thrust_ctl_pd + alt_integral;
if (thrust_ctl < params.thr_min) {
thrust_ctl = params.thr_min;
} else if (thrust_ctl > params.thr_max) {
thrust_ctl = params.thr_max;
} else {
/* integrate only in linear area (with 20% gap) and not on min/max throttle */
if (fabs(thrust_ctl_pd) < err_linear_limit * params.alt_p * 0.8f)
alt_integral += thrust_ctl_pd / params.alt_p * params.alt_i * dt;
}
if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE) {
// TODO add position controller
att_sp.pitch_body = manual.pitch;
att_sp.roll_body = manual.roll;
att_sp.yaw_body = manual.yaw;
} else {
att_sp.pitch_body = manual.pitch;
att_sp.roll_body = manual.roll;
att_sp.yaw_body = manual.yaw;
// TODO move position setpoint with manual controls
}
att_sp.thrust = thrust_ctl;
att_sp.timestamp = hrt_absolute_time();
/* publish local position setpoint */
orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp);
/* publish new attitude setpoint */
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
t_prev = t;
} else {
reset_sp_alt = true;
}
/* PID for altitude */
float alt_err = local_pos.z - local_pos_sp.z;
/* don't accelerate more than ALT_RATE_MAX, limit error to corresponding value */
if (alt_err > err_linear_limit) {
alt_err = err_linear_limit;
} else if (alt_err < -err_linear_limit) {
alt_err = -err_linear_limit;
}
/* P and D components */
float thrust_ctl_pd = alt_err * params.alt_p + local_pos.vz * params.alt_d;
/* add I component */
float thrust_ctl = thrust_ctl_pd + alt_integral;
alt_integral += thrust_ctl_pd / params.alt_p * params.alt_i * dt;
if (thrust_ctl < params.thr_min) {
thrust_ctl = params.thr_min;
} else if (thrust_ctl > params.thr_max) {
thrust_ctl = params.thr_max;
}
if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE || status.state_machine == SYSTEM_STATE_AUTO) {
// TODO add position controller
} else {
reset_sp_pos = true;
}
att_sp.thrust = thrust_ctl;
att_sp.timestamp = hrt_absolute_time();
if (status.flag_control_manual_enabled) {
/* publish local position setpoint in manual mode */
orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp);
}
/* publish new attitude setpoint */
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
t_prev = t;
} else {
reset_sp_alt = true;
reset_sp_pos = true;
}
/* run at approximately 50 Hz */