// auto_spline_run - runs the auto spline controller
// called by auto_run at 100hz or more
void Sub::auto_spline_run()
{
// if not armed set throttle to zero and exit immediately
if (!motors.armed()) {
// To-Do: reset waypoint origin to current location because vehicle is probably on the ground so we don't want it lurching left or right on take-off
// (of course it would be better if people just used take-off)
// Sub vehicles do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
return;
}
// process pilot's yaw input
float target_yaw_rate = 0;
if (!failsafe.pilot_input) {
// get pilot's desired yaw rat
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
if (!is_zero(target_yaw_rate)) {
set_auto_yaw_mode(AUTO_YAW_HOLD);
}
}
// set motors to full range
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// run waypoint controller
wp_nav.update_spline();
float lateral_out, forward_out;
translate_wpnav_rp(lateral_out, forward_out);
// Send to forward/lateral outputs
motors.set_lateral(lateral_out);
motors.set_forward(forward_out);
// call z-axis position controller (wpnav should have already updated it's alt target)
pos_control.update_z_controller();
// get pilot desired lean angles
float target_roll, target_pitch;
get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, aparm.angle_max);
// call attitude controller
if (auto_yaw_mode == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
} else {
// roll, pitch from waypoint controller, yaw heading from auto_heading()
attitude_control.input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, get_auto_heading(), true, get_smoothing_gain());
}
}
// auto_spline_run - runs the auto spline controller
// called by auto_run at 100hz or more
void Copter::auto_spline_run()
{
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) {
// To-Do: reset waypoint origin to current location because copter is probably on the ground so we don't want it lurching left or right on take-off
// (of course it would be better if people just used take-off)
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else // multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
#endif
// clear i term when we're taking off
set_throttle_takeoff();
return;
}
// process pilot's yaw input
float target_yaw_rate = 0;
if (!failsafe.radio) {
// get pilot's desired yaw rat
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
if (!is_zero(target_yaw_rate)) {
set_auto_yaw_mode(AUTO_YAW_HOLD);
}
}
// set motors to full range
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// run waypoint controller
wp_nav.update_spline();
// call z-axis position controller (wpnav should have already updated it's alt target)
pos_control.update_z_controller();
// call attitude controller
if (auto_yaw_mode == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
}else{
// roll, pitch from waypoint controller, yaw heading from auto_heading()
attitude_control.input_euler_angle_roll_pitch_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(), true, get_smoothing_gain());
}
}
// rtl_climb_return_descent_run - implements the initial climb, return home and descent portions of RTL which all rely on the wp controller
// called by rtl_run at 100hz or more
void Copter::rtl_loiterathome_run()
{
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) {
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
// multicopters do not stabilize roll/pitch/yaw when disarmed
// reset attitude control targets
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
// To-Do: re-initialise wpnav targets
return;
}
// process pilot's yaw input
float target_yaw_rate = 0;
if (!failsafe.radio) {
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
if (!is_zero(target_yaw_rate)) {
set_auto_yaw_mode(AUTO_YAW_HOLD);
}
}
// set motors to full range
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// run waypoint controller
failsafe_terrain_set_status(wp_nav.update_wpnav());
// call z-axis position controller (wpnav should have already updated it's alt target)
pos_control.update_z_controller();
// call attitude controller
if (auto_yaw_mode == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
}else{
// roll, pitch from waypoint controller, yaw heading from auto_heading()
attitude_control.input_euler_angle_roll_pitch_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(),true, get_smoothing_gain());
}
// check if we've completed this stage of RTL
if ((millis() - rtl_loiter_start_time) >= (uint32_t)g.rtl_loiter_time.get()) {
if (auto_yaw_mode == AUTO_YAW_RESETTOARMEDYAW) {
// check if heading is within 2 degrees of heading when vehicle was armed
if (labs(wrap_180_cd(ahrs.yaw_sensor-initial_armed_bearing)) <= 200) {
rtl_state_complete = true;
}
} else {
// we have loitered long enough
rtl_state_complete = true;
}
}
}
// rtl_climb_return_run - implements the initial climb, return home and descent portions of RTL which all rely on the wp controller
// called by rtl_run at 100hz or more
void Copter::rtl_climb_return_run()
{
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) {
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
// multicopters do not stabilize roll/pitch/yaw when disarmed
// reset attitude control targets
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
// To-Do: re-initialise wpnav targets
return;
}
// process pilot's yaw input
float target_yaw_rate = 0;
if (!failsafe.radio) {
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
if (!is_zero(target_yaw_rate)) {
set_auto_yaw_mode(AUTO_YAW_HOLD);
}
}
// set motors to full range
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// run waypoint controller
failsafe_terrain_set_status(wp_nav.update_wpnav());
// call z-axis position controller (wpnav should have already updated it's alt target)
pos_control.update_z_controller();
// call attitude controller
if (auto_yaw_mode == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
}else{
// roll, pitch from waypoint controller, yaw heading from auto_heading()
attitude_control.input_euler_angle_roll_pitch_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(),true, get_smoothing_gain());
}
// check if we've completed this stage of RTL
rtl_state_complete = wp_nav.reached_wp_destination();
}
// guided_posvel_control_run - runs the guided spline controller
// called from guided_run
void Sub::guided_posvel_control_run()
{
// if motors not enabled set throttle to zero and exit immediately
if (!motors.armed()) {
// set target position and velocity to current position and velocity
pos_control.set_pos_target(inertial_nav.get_position());
pos_control.set_desired_velocity(Vector3f(0,0,0));
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
// Sub vehicles do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
return;
}
// process pilot's yaw input
float target_yaw_rate = 0;
if (!failsafe.pilot_input) {
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
if (!is_zero(target_yaw_rate)) {
set_auto_yaw_mode(AUTO_YAW_HOLD);
}
}
// set motors to full range
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// set velocity to zero if no updates received for 3 seconds
uint32_t tnow = AP_HAL::millis();
if (tnow - posvel_update_time_ms > GUIDED_POSVEL_TIMEOUT_MS && !posvel_vel_target_cms.is_zero()) {
posvel_vel_target_cms.zero();
}
// calculate dt
float dt = pos_control.time_since_last_xy_update();
// sanity check dt
if (dt >= 0.2f) {
dt = 0.0f;
}
// advance position target using velocity target
posvel_pos_target_cm += posvel_vel_target_cms * dt;
// send position and velocity targets to position controller
pos_control.set_pos_target(posvel_pos_target_cm);
pos_control.set_desired_velocity_xy(posvel_vel_target_cms.x, posvel_vel_target_cms.y);
// run position controller
pos_control.update_xy_controller(ekfNavVelGainScaler);
float lateral_out, forward_out;
translate_pos_control_rp(lateral_out, forward_out);
// Send to forward/lateral outputs
motors.set_lateral(lateral_out);
motors.set_forward(forward_out);
pos_control.update_z_controller();
// call attitude controller
if (auto_yaw_mode == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate);
} else {
// roll, pitch from waypoint controller, yaw heading from auto_heading()
attitude_control.input_euler_angle_roll_pitch_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), get_auto_heading(), true);
}
}
// guided_vel_control_run - runs the guided velocity controller
// called from guided_run
void Sub::guided_vel_control_run()
{
// ifmotors not enabled set throttle to zero and exit immediately
if (!motors.armed()) {
// initialise velocity controller
pos_control.init_vel_controller_xyz();
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
// Sub vehicles do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
return;
}
// process pilot's yaw input
float target_yaw_rate = 0;
if (!failsafe.pilot_input) {
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
if (!is_zero(target_yaw_rate)) {
set_auto_yaw_mode(AUTO_YAW_HOLD);
}
}
// set motors to full range
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// set velocity to zero if no updates received for 3 seconds
uint32_t tnow = AP_HAL::millis();
if (tnow - vel_update_time_ms > GUIDED_POSVEL_TIMEOUT_MS && !pos_control.get_desired_velocity().is_zero()) {
pos_control.set_desired_velocity(Vector3f(0,0,0));
}
// call velocity controller which includes z axis controller
pos_control.update_vel_controller_xyz(ekfNavVelGainScaler);
float lateral_out, forward_out;
translate_pos_control_rp(lateral_out, forward_out);
// Send to forward/lateral outputs
motors.set_lateral(lateral_out);
motors.set_forward(forward_out);
// call attitude controller
if (auto_yaw_mode == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate);
} else {
// roll, pitch from waypoint controller, yaw heading from auto_heading()
attitude_control.input_euler_angle_roll_pitch_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), get_auto_heading(), true);
}
}
// guided_pos_control_run - runs the guided position controller
// called from guided_run
void Sub::guided_pos_control_run()
{
// if motors not enabled set throttle to zero and exit immediately
if (!motors.armed()) {
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
// Sub vehicles do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
return;
}
// process pilot's yaw input
float target_yaw_rate = 0;
if (!failsafe.pilot_input) {
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
if (!is_zero(target_yaw_rate)) {
set_auto_yaw_mode(AUTO_YAW_HOLD);
}
}
// set motors to full range
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// run waypoint controller
failsafe_terrain_set_status(wp_nav.update_wpnav());
float lateral_out, forward_out;
translate_wpnav_rp(lateral_out, forward_out);
// Send to forward/lateral outputs
motors.set_lateral(lateral_out);
motors.set_forward(forward_out);
// call z-axis position controller (wpnav should have already updated it's alt target)
pos_control.update_z_controller();
// call attitude controller
if (auto_yaw_mode == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate);
} else {
// roll, pitch from waypoint controller, yaw heading from auto_heading()
attitude_control.input_euler_angle_roll_pitch_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), get_auto_heading(), true);
}
}
// guided_posvel_control_run - runs the guided spline controller
// called from guided_run
void Copter::guided_posvel_control_run()
{
// if not auto armed or motors not enabled set throttle to zero and exit immediately
if (!motors.armed() || !ap.auto_armed || !motors.get_interlock() || ap.land_complete) {
// set target position and velocity to current position and velocity
pos_control.set_pos_target(inertial_nav.get_position());
pos_control.set_desired_velocity(Vector3f(0,0,0));
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
// multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
return;
}
// process pilot's yaw input
float target_yaw_rate = 0;
if (!failsafe.radio) {
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
if (!is_zero(target_yaw_rate)) {
set_auto_yaw_mode(AUTO_YAW_HOLD);
}
}
// set motors to full range
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// set velocity to zero if no updates received for 3 seconds
uint32_t tnow = millis();
if (tnow - posvel_update_time_ms > GUIDED_POSVEL_TIMEOUT_MS && !posvel_vel_target_cms.is_zero()) {
posvel_vel_target_cms.zero();
}
// calculate dt
float dt = pos_control.time_since_last_xy_update();
// update at poscontrol update rate
if (dt >= pos_control.get_dt_xy()) {
// sanity check dt
if (dt >= 0.2f) {
dt = 0.0f;
}
// advance position target using velocity target
posvel_pos_target_cm += posvel_vel_target_cms * dt;
// send position and velocity targets to position controller
pos_control.set_pos_target(posvel_pos_target_cm);
pos_control.set_desired_velocity_xy(posvel_vel_target_cms.x, posvel_vel_target_cms.y);
// run position controller
pos_control.update_xy_controller(AC_PosControl::XY_MODE_POS_AND_VEL_FF, ekfNavVelGainScaler, false);
}
pos_control.update_z_controller();
// call attitude controller
if (auto_yaw_mode == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(pos_control.get_roll(), pos_control.get_pitch(), target_yaw_rate);
}else{
// roll, pitch from waypoint controller, yaw heading from auto_heading()
attitude_control.input_euler_angle_roll_pitch_yaw(pos_control.get_roll(), pos_control.get_pitch(), get_auto_heading(), true);
}
}
// guided_vel_control_run - runs the guided velocity controller
// called from guided_run
void Copter::guided_vel_control_run()
{
// if not auto armed or motors not enabled set throttle to zero and exit immediately
if (!motors.armed() || !ap.auto_armed || !motors.get_interlock() || ap.land_complete) {
// initialise velocity controller
pos_control.init_vel_controller_xyz();
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
// multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
return;
}
// process pilot's yaw input
float target_yaw_rate = 0;
if (!failsafe.radio) {
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
if (!is_zero(target_yaw_rate)) {
set_auto_yaw_mode(AUTO_YAW_HOLD);
}
}
// set motors to full range
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// set velocity to zero if no updates received for 3 seconds
uint32_t tnow = millis();
if (tnow - vel_update_time_ms > GUIDED_POSVEL_TIMEOUT_MS && !pos_control.get_desired_velocity().is_zero()) {
pos_control.set_desired_velocity(Vector3f(0,0,0));
}
// call velocity controller which includes z axis controller
pos_control.update_vel_controller_xyz(ekfNavVelGainScaler);
// call attitude controller
if (auto_yaw_mode == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(pos_control.get_roll(), pos_control.get_pitch(), target_yaw_rate);
}else{
// roll, pitch from waypoint controller, yaw heading from auto_heading()
attitude_control.input_euler_angle_roll_pitch_yaw(pos_control.get_roll(), pos_control.get_pitch(), get_auto_heading(), true);
}
}
