|
|
@ -9,29 +9,29 @@ |
|
|
|
// loiter_init - initialise loiter controller
|
|
|
|
// loiter_init - initialise loiter controller
|
|
|
|
bool Copter::ModeLoiter::init(bool ignore_checks) |
|
|
|
bool Copter::ModeLoiter::init(bool ignore_checks) |
|
|
|
{ |
|
|
|
{ |
|
|
|
if (!copter.failsafe.radio) { |
|
|
|
if (!copter.failsafe.radio) { |
|
|
|
float target_roll, target_pitch; |
|
|
|
float target_roll, target_pitch; |
|
|
|
// apply SIMPLE mode transform to pilot inputs
|
|
|
|
// apply SIMPLE mode transform to pilot inputs
|
|
|
|
update_simple_mode(); |
|
|
|
update_simple_mode(); |
|
|
|
|
|
|
|
|
|
|
|
// convert pilot input to lean angles
|
|
|
|
// convert pilot input to lean angles
|
|
|
|
get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max()); |
|
|
|
get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max()); |
|
|
|
|
|
|
|
|
|
|
|
// process pilot's roll and pitch input
|
|
|
|
// process pilot's roll and pitch input
|
|
|
|
loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch, G_Dt); |
|
|
|
loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch, G_Dt); |
|
|
|
} else { |
|
|
|
} else { |
|
|
|
// clear out pilot desired acceleration in case radio failsafe event occurs and we do not switch to RTL for some reason
|
|
|
|
// clear out pilot desired acceleration in case radio failsafe event occurs and we do not switch to RTL for some reason
|
|
|
|
loiter_nav->clear_pilot_desired_acceleration(); |
|
|
|
loiter_nav->clear_pilot_desired_acceleration(); |
|
|
|
} |
|
|
|
} |
|
|
|
loiter_nav->init_target(); |
|
|
|
loiter_nav->init_target(); |
|
|
|
|
|
|
|
|
|
|
|
// initialise position and desired velocity
|
|
|
|
// initialise position and desired velocity
|
|
|
|
if (!pos_control->is_active_z()) { |
|
|
|
if (!pos_control->is_active_z()) { |
|
|
|
pos_control->set_alt_target_to_current_alt(); |
|
|
|
pos_control->set_alt_target_to_current_alt(); |
|
|
|
pos_control->set_desired_velocity_z(inertial_nav.get_velocity_z()); |
|
|
|
pos_control->set_desired_velocity_z(inertial_nav.get_velocity_z()); |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
return true; |
|
|
|
return true; |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
#if PRECISION_LANDING == ENABLED |
|
|
|
#if PRECISION_LANDING == ENABLED |
|
|
|