|
|
|
@ -755,8 +755,8 @@ void QuadPlane::run_z_controller(void)
@@ -755,8 +755,8 @@ void QuadPlane::run_z_controller(void)
|
|
|
|
|
pos_control->set_desired_velocity_z(inertial_nav.get_velocity_z()); |
|
|
|
|
|
|
|
|
|
// initialize vertical speeds and leash lengths
|
|
|
|
|
pos_control->set_speed_z(-pilot_velocity_z_max, pilot_velocity_z_max); |
|
|
|
|
pos_control->set_accel_z(pilot_accel_z); |
|
|
|
|
pos_control->set_max_speed_z(-pilot_velocity_z_max, pilot_velocity_z_max); |
|
|
|
|
pos_control->set_max_accel_z(pilot_accel_z); |
|
|
|
|
|
|
|
|
|
// it has been two seconds since we last ran the Z
|
|
|
|
|
// controller. We need to assume the integrator may be way off
|
|
|
|
@ -790,8 +790,8 @@ void QuadPlane::check_attitude_relax(void)
@@ -790,8 +790,8 @@ void QuadPlane::check_attitude_relax(void)
|
|
|
|
|
void QuadPlane::init_hover(void) |
|
|
|
|
{ |
|
|
|
|
// initialize vertical speeds and leash lengths
|
|
|
|
|
pos_control->set_speed_z(-pilot_velocity_z_max, pilot_velocity_z_max); |
|
|
|
|
pos_control->set_accel_z(pilot_accel_z); |
|
|
|
|
pos_control->set_max_speed_z(-pilot_velocity_z_max, pilot_velocity_z_max); |
|
|
|
|
pos_control->set_max_accel_z(pilot_accel_z); |
|
|
|
|
|
|
|
|
|
// initialise position and desired velocity
|
|
|
|
|
set_alt_target_current(); |
|
|
|
@ -823,8 +823,8 @@ void QuadPlane::hold_hover(float target_climb_rate)
@@ -823,8 +823,8 @@ void QuadPlane::hold_hover(float target_climb_rate)
|
|
|
|
|
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); |
|
|
|
|
|
|
|
|
|
// initialize vertical speeds and acceleration
|
|
|
|
|
pos_control->set_speed_z(-pilot_velocity_z_max, pilot_velocity_z_max); |
|
|
|
|
pos_control->set_accel_z(pilot_accel_z); |
|
|
|
|
pos_control->set_max_speed_z(-pilot_velocity_z_max, pilot_velocity_z_max); |
|
|
|
|
pos_control->set_max_accel_z(pilot_accel_z); |
|
|
|
|
|
|
|
|
|
// call attitude controller
|
|
|
|
|
multicopter_attitude_rate_update(get_desired_yaw_rate_cds()); |
|
|
|
@ -855,8 +855,8 @@ void QuadPlane::init_loiter(void)
@@ -855,8 +855,8 @@ void QuadPlane::init_loiter(void)
|
|
|
|
|
loiter_nav->init_target(); |
|
|
|
|
|
|
|
|
|
// initialize vertical speed and acceleration
|
|
|
|
|
pos_control->set_speed_z(-pilot_velocity_z_max, pilot_velocity_z_max); |
|
|
|
|
pos_control->set_accel_z(pilot_accel_z); |
|
|
|
|
pos_control->set_max_speed_z(-pilot_velocity_z_max, pilot_velocity_z_max); |
|
|
|
|
pos_control->set_max_accel_z(pilot_accel_z); |
|
|
|
|
|
|
|
|
|
// initialise position and desired velocity
|
|
|
|
|
set_alt_target_current(); |
|
|
|
@ -984,8 +984,8 @@ void QuadPlane::control_loiter()
@@ -984,8 +984,8 @@ void QuadPlane::control_loiter()
|
|
|
|
|
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); |
|
|
|
|
|
|
|
|
|
// initialize vertical speed and acceleration
|
|
|
|
|
pos_control->set_speed_z(-pilot_velocity_z_max, pilot_velocity_z_max); |
|
|
|
|
pos_control->set_accel_z(pilot_accel_z); |
|
|
|
|
pos_control->set_max_speed_z(-pilot_velocity_z_max, pilot_velocity_z_max); |
|
|
|
|
pos_control->set_max_accel_z(pilot_accel_z); |
|
|
|
|
|
|
|
|
|
// process pilot's roll and pitch input
|
|
|
|
|
loiter_nav->set_pilot_desired_acceleration(plane.channel_roll->get_control_in(), |
|
|
|
@ -1970,8 +1970,8 @@ void QuadPlane::setup_target_position(void)
@@ -1970,8 +1970,8 @@ void QuadPlane::setup_target_position(void)
|
|
|
|
|
last_loiter_ms = now; |
|
|
|
|
|
|
|
|
|
// setup vertical speed and acceleration
|
|
|
|
|
pos_control->set_speed_z(-pilot_velocity_z_max, pilot_velocity_z_max); |
|
|
|
|
pos_control->set_accel_z(pilot_accel_z); |
|
|
|
|
pos_control->set_max_speed_z(-pilot_velocity_z_max, pilot_velocity_z_max); |
|
|
|
|
pos_control->set_max_accel_z(pilot_accel_z); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
/*
|
|
|
|
@ -2129,8 +2129,8 @@ bool QuadPlane::do_vtol_takeoff(const AP_Mission::Mission_Command& cmd)
@@ -2129,8 +2129,8 @@ bool QuadPlane::do_vtol_takeoff(const AP_Mission::Mission_Command& cmd)
|
|
|
|
|
loiter_nav->init_target(); |
|
|
|
|
|
|
|
|
|
// initialize vertical speed and acceleration
|
|
|
|
|
pos_control->set_speed_z(-pilot_velocity_z_max, pilot_velocity_z_max); |
|
|
|
|
pos_control->set_accel_z(pilot_accel_z); |
|
|
|
|
pos_control->set_max_speed_z(-pilot_velocity_z_max, pilot_velocity_z_max); |
|
|
|
|
pos_control->set_max_accel_z(pilot_accel_z); |
|
|
|
|
|
|
|
|
|
// initialise position and desired velocity
|
|
|
|
|
set_alt_target_current(); |
|
|
|
|