// circle_init - initialise circle controller flight mode
bool Copter::circle_init(bool ignore_checks)
{
#if FRAME_CONFIG == HELI_FRAME
// do not allow helis to enter Alt Hold if the Rotor Runup is not complete and current control mode has manual throttle control,
// as this will force the helicopter to descend.
if (!ignore_checks && mode_has_manual_throttle(control_mode) && !motors.rotor_runup_complete()){
return false;
}
#endif
if (position_ok() || ignore_checks) {
circle_pilot_yaw_override = false;
// initialize speeds and accelerations
pos_control.set_speed_xy(wp_nav.get_speed_xy());
pos_control.set_accel_xy(wp_nav.get_wp_acceleration());
pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
pos_control.set_accel_z(g.pilot_accel_z);
// initialise circle controller including setting the circle center based on vehicle speed
circle_nav.init();
return true;
}else{
return false;
}
}
// loiter_init - initialise loiter controller
bool Copter::loiter_init(bool ignore_checks)
{
#if FRAME_CONFIG == HELI_FRAME
// do not allow helis to enter Loiter if the Rotor Runup is not complete
if (!ignore_checks && !motors.rotor_runup_complete()){
return false;
}
#endif
if (position_ok() || ignore_checks) {
// set target to current position
wp_nav.init_loiter_target();
// initialize vertical speed and acceleration
pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
pos_control.set_accel_z(g.pilot_accel_z);
// initialise position and desired velocity
pos_control.set_alt_target(inertial_nav.get_altitude());
pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
return true;
}else{
return false;
}
}
// one_hz_loop - runs at 1Hz
void Sub::one_hz_loop()
{
bool arm_check = arming.pre_arm_checks(false);
ap.pre_arm_check = arm_check;
AP_Notify::flags.pre_arm_check = arm_check;
AP_Notify::flags.pre_arm_gps_check = position_ok();
if (should_log(MASK_LOG_ANY)) {
Log_Write_Data(DATA_AP_STATE, ap.value);
}
if (!motors.armed()) {
// make it possible to change ahrs orientation at runtime during initial config
ahrs.set_orientation();
// set all throttle channel settings
motors.set_throttle_range(channel_throttle->get_radio_min(), channel_throttle->get_radio_max());
}
// update assigned functions and enable auxiliary servos
SRV_Channels::enable_aux_servos();
// update position controller alt limits
update_poscon_alt_max();
// log terrain data
terrain_logging();
}
// land_init - initialise land controller using precision landing
bool Copter::land_precision_init()
{
#if PRECISION_LANDING == ENABLED
land_state.use_gps = position_ok();
// check if precision landing available
land_state.use_precision = land_state.use_gps && precland.enabled() && precland.healthy();
if (!land_state.use_precision) {
return false;
}
// initialize vertical speeds and leash lengths
pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
pos_control.set_accel_z(g.pilot_accel_z);
// initialise velocity controller
pos_control.init_vel_controller_xyz();
// initialise precland desired vel
precland.set_desired_velocity(inertial_nav.get_velocity());
return true;
#else
land_state.use_precision = false;
return false;
#endif
}
// auto_init - initialise auto controller
bool Copter::auto_init(bool ignore_checks)
{
if ((position_ok() && mission.num_commands() > 1) || ignore_checks) {
auto_mode = Auto_Loiter;
// reject switching to auto mode if landed with motors armed but first command is not a takeoff (reduce change of flips)
if (motors.armed() && ap.land_complete && !mission.starts_with_takeoff_cmd()) {
gcs_send_text(MAV_SEVERITY_CRITICAL, "Auto: Missing Takeoff Cmd");
return false;
}
// stop ROI from carrying over from previous runs of the mission
// To-Do: reset the yaw as part of auto_wp_start when the previous command was not a wp command to remove the need for this special ROI check
if (auto_yaw_mode == AUTO_YAW_ROI) {
set_auto_yaw_mode(AUTO_YAW_HOLD);
}
// initialise waypoint and spline controller
wp_nav.wp_and_spline_init();
// clear guided limits
guided_limit_clear();
// start/resume the mission (based on MIS_RESTART parameter)
mission.start_or_resume();
return true;
}else{
return false;
}
}
// auto_init - initialise auto controller
bool Copter::auto_init(bool ignore_checks)
{
#if FRAME_CONFIG == HELI_FRAME
// do not allow helis to enter Alt Hold if the Rotor Runup is not complete and current control mode has manual throttle control,
// as this will force the helicopter to descend.
if (!ignore_checks && mode_has_manual_throttle(control_mode) && !motors.rotor_runup_complete()){
return false;
}
#endif
if ((position_ok() && mission.num_commands() > 1) || ignore_checks) {
auto_mode = Auto_Loiter;
// stop ROI from carrying over from previous runs of the mission
// To-Do: reset the yaw as part of auto_wp_start when the previous command was not a wp command to remove the need for this special ROI check
if (auto_yaw_mode == AUTO_YAW_ROI) {
set_auto_yaw_mode(AUTO_YAW_HOLD);
}
// initialise waypoint and spline controller
wp_nav.wp_and_spline_init();
// clear guided limits
guided_limit_clear();
// start/resume the mission (based on MIS_RESTART parameter)
mission.start_or_resume();
return true;
}else{
return false;
}
}
// land_init - initialise land controller
bool Copter::land_init(bool ignore_checks)
{
// check if we have GPS and decide which LAND we're going to do
land_with_gps = position_ok();
if (land_with_gps) {
// set target to stopping point
Vector3f stopping_point;
wp_nav.get_loiter_stopping_point_xy(stopping_point);
wp_nav.init_loiter_target(stopping_point);
}
// initialize vertical speeds and leash lengths
pos_control.set_speed_z(wp_nav.get_speed_down(), wp_nav.get_speed_up());
pos_control.set_accel_z(wp_nav.get_accel_z());
// initialise altitude target to stopping point
pos_control.set_target_to_stopping_point_z();
land_start_time = millis();
land_pause = false;
// reset flag indicating if pilot has applied roll or pitch inputs during landing
ap.land_repo_active = false;
return true;
}
// distance between vehicle and home in cm
uint32_t Copter::home_distance()
{
if (position_ok()) {
_home_distance = current_loc.get_distance(ahrs.get_home()) * 100;
}
return _home_distance;
}
// The location of home in relation to the vehicle in centi-degrees
int32_t Copter::home_bearing()
{
if (position_ok()) {
_home_bearing = current_loc.get_bearing_to(ahrs.get_home());
}
return _home_bearing;
}
// brake_init - initialise brake controller
bool Copter::brake_init(bool ignore_checks)
{
if ((position_ok() && !failsafe.gps_glitch) || ignore_checks) {
// set desired acceleration to zero
wp_nav.clear_pilot_desired_acceleration();
// set target to current position
wp_nav.init_brake_target(BRAKE_MODE_DECEL_RATE);
// initialize vertical speed and acceleration
pos_control.set_speed_z(BRAKE_MODE_SPEED_Z, BRAKE_MODE_SPEED_Z);
pos_control.set_accel_z(BRAKE_MODE_DECEL_RATE);
// initialise position and desired velocity
pos_control.set_alt_target(inertial_nav.get_altitude());
pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
brake_timeout_ms = 0;
return true;
}else{
return false;
}
}
// drift_init - initialise drift controller
bool Copter::drift_init(bool ignore_checks)
{
if (position_ok() || ignore_checks) {
return true;
} else {
return false;
}
}
// drift_init - initialise drift controller
bool Copter::drift_init(bool ignore_checks)
{
if ((position_ok() && !failsafe.gps_glitch) || ignore_checks) {
return true;
}else{
return false;
}
}
// rtl_init - initialise rtl controller
bool Copter::rtl_init(bool ignore_checks)
{
if (position_ok() || ignore_checks) {
rtl_climb_start();
return true;
}else{
return false;
}
}
// rtl_init - initialise rtl controller
bool Copter::rtl_init(bool ignore_checks)
{
if (position_ok() || ignore_checks) {
rtl_build_path(!failsafe.terrain);
rtl_climb_start();
return true;
}else{
return false;
}
}
float Copter::get_look_ahead_yaw()
{
const Vector3f& vel = inertial_nav.get_velocity();
float speed = pythagorous2(vel.x,vel.y);
// Commanded Yaw to automatically look ahead.
if (position_ok() && (speed > YAW_LOOK_AHEAD_MIN_SPEED)) {
yaw_look_ahead_bearing = degrees(atan2f(vel.y,vel.x))*100.0f;
}
return yaw_look_ahead_bearing;
}
// guided_init - initialise guided controller
bool Sub::guided_init(bool ignore_checks)
{
if (!position_ok() && !ignore_checks) {
return false;
}
// initialise yaw
set_auto_yaw_mode(get_default_auto_yaw_mode(false));
// start in position control mode
guided_pos_control_start();
return true;
}
// guided_init - initialise guided controller
bool Copter::guided_init(bool ignore_checks)
{
if (position_ok() || ignore_checks) {
// initialise yaw
set_auto_yaw_mode(get_default_auto_yaw_mode(false));
// start in position control mode
guided_pos_control_start();
return true;
}else{
return false;
}
}
// calc_home_distance_and_bearing - calculate distance and bearing to home for reporting and autopilot decisions
void Copter::calc_home_distance_and_bearing()
{
// calculate home distance and bearing
if (position_ok()) {
Vector3f home = pv_location_to_vector(ahrs.get_home());
Vector3f curr = inertial_nav.get_position();
home_distance = pv_get_horizontal_distance_cm(curr, home);
home_bearing = pv_get_bearing_cd(curr,home);
// update super simple bearing (if required) because it relies on home_bearing
update_super_simple_bearing(false);
}
}
// poshold_init - initialise PosHold controller
bool Copter::poshold_init(bool ignore_checks)
{
// fail to initialise PosHold mode if no GPS lock
if (!position_ok() && !ignore_checks) {
return false;
}
// initialize vertical speeds and acceleration
pos_control->set_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);
pos_control->set_accel_z(g.pilot_accel_z);
// initialise position and desired velocity
if (!pos_control->is_active_z()) {
pos_control->set_alt_target_to_current_alt();
pos_control->set_desired_velocity_z(inertial_nav.get_velocity_z());
}
// initialise lean angles to current attitude
poshold.pilot_roll = 0;
poshold.pilot_pitch = 0;
// compute brake_gain
poshold.brake_gain = (15.0f * (float)g.poshold_brake_rate + 95.0f) / 100.0f;
if (ap.land_complete) {
// if landed begin in loiter mode
poshold.roll_mode = POSHOLD_LOITER;
poshold.pitch_mode = POSHOLD_LOITER;
// set target to current position
// only init here as we can switch to PosHold in flight with a velocity <> 0 that will be used as _last_vel in PosControl and never updated again as we inhibit Reset_I
wp_nav->init_loiter_target();
}else{
// if not landed start in pilot override to avoid hard twitch
poshold.roll_mode = POSHOLD_PILOT_OVERRIDE;
poshold.pitch_mode = POSHOLD_PILOT_OVERRIDE;
}
// loiter's I terms should be reset the first time only
poshold.loiter_reset_I = true;
// initialise wind_comp each time PosHold is switched on
poshold.wind_comp_ef.zero();
poshold.wind_comp_roll = 0;
poshold.wind_comp_pitch = 0;
poshold.wind_comp_timer = 0;
return true;
}
// heli_check_dynamic_flight - updates the dynamic_flight flag based on our horizontal velocity
// should be called at 50hz
void Copter::check_dynamic_flight(void)
{
if (!motors.armed() || !motors.rotor_runup_complete() ||
control_mode == LAND || (control_mode==RTL && rtl_state == RTL_Land) || (control_mode == AUTO && auto_mode == Auto_Land)) {
heli_dynamic_flight_counter = 0;
heli_flags.dynamic_flight = false;
return;
}
bool moving = false;
// with GPS lock use inertial nav to determine if we are moving
if (position_ok()) {
// get horizontal velocity
float velocity = inertial_nav.get_velocity_xy();
moving = (velocity >= HELI_DYNAMIC_FLIGHT_SPEED_MIN);
}else{
// with no GPS lock base it on throttle and forward lean angle
moving = (motors.get_throttle() > 800.0f || ahrs.pitch_sensor < -1500);
}
if (!moving && sonar_enabled && sonar.status() == RangeFinder::RangeFinder_Good) {
// when we are more than 2m from the ground with good
// rangefinder lock consider it to be dynamic flight
moving = (sonar.distance_cm() > 200);
}
if (moving) {
// if moving for 2 seconds, set the dynamic flight flag
if (!heli_flags.dynamic_flight) {
heli_dynamic_flight_counter++;
if (heli_dynamic_flight_counter >= 100) {
heli_flags.dynamic_flight = true;
heli_dynamic_flight_counter = 100;
}
}
}else{
// if not moving for 2 seconds, clear the dynamic flight flag
if (heli_flags.dynamic_flight) {
if (heli_dynamic_flight_counter > 0) {
heli_dynamic_flight_counter--;
}else{
heli_flags.dynamic_flight = false;
}
}
}
}
// loiter_init - initialise loiter controller
bool Copter::loiter_init(bool ignore_checks)
{
if (position_ok() || ignore_checks) {
// set target to current position
wp_nav.init_loiter_target();
// initialize vertical speed and accelerationj
pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
pos_control.set_accel_z(g.pilot_accel_z);
// initialise altitude target to stopping point
pos_control.set_target_to_stopping_point_z();
return true;
} else {
return false;
}
}
// circle_init - initialise circle controller flight mode
bool Copter::circle_init(bool ignore_checks)
{
if (position_ok() || ignore_checks) {
circle_pilot_yaw_override = false;
// initialize speeds and accelerations
pos_control.set_speed_xy(wp_nav.get_speed_xy());
pos_control.set_accel_xy(wp_nav.get_wp_acceleration());
pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
pos_control.set_accel_z(g.pilot_accel_z);
// initialise circle controller including setting the circle center based on vehicle speed
circle_nav.init();
return true;
}else{
return false;
}
}
// circle_init - initialise circle controller flight mode
bool Sub::circle_init()
{
if (!position_ok()) {
return false;
}
circle_pilot_yaw_override = false;
// initialize speeds and accelerations
pos_control.set_max_speed_xy(wp_nav.get_default_speed_xy());
pos_control.set_max_accel_xy(wp_nav.get_wp_acceleration());
pos_control.set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);
pos_control.set_max_accel_z(g.pilot_accel_z);
// initialise circle controller including setting the circle center based on vehicle speed
circle_nav.init();
return true;
}
// throw_init - initialise throw controller
bool Copter::throw_init(bool ignore_checks)
{
#if FRAME_CONFIG == HELI_FRAME
// do not allow helis to use throw to start
return false;
#endif
// do not enter the mode when already armed
if (motors.armed()) {
return false;
}
// this mode needs a position reference
if (position_ok()) {
return true;
} else {
return false;
}
}
// loiter_init - initialise loiter controller
bool Copter::loiter_init(bool ignore_checks)
{
if (position_ok() || ignore_checks) {
// set target to current position
wp_nav.init_loiter_target();
// initialize vertical speed and acceleration
pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
pos_control.set_accel_z(g.pilot_accel_z);
// initialise position and desired velocity
pos_control.set_alt_target(inertial_nav.get_altitude());
pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
return true;
}else{
return false;
}
}
// circle_init - initialise circle controller flight mode
bool Copter::circle_init(bool ignore_checks)
{
if (position_ok() || ignore_checks) {
circle_pilot_yaw_override = false;
// initialize speeds and accelerations
pos_control->set_speed_xy(wp_nav->get_speed_xy());
pos_control->set_accel_xy(wp_nav->get_wp_acceleration());
pos_control->set_jerk_xy_to_default();
pos_control->set_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);
pos_control->set_accel_z(g.pilot_accel_z);
// initialise circle controller including setting the circle center based on vehicle speed
circle_nav->init();
return true;
}else{
return false;
}
}
// brake_init - initialise brake controller
bool Copter::brake_init(bool ignore_checks)
{
if (position_ok() || optflow_position_ok() || ignore_checks) {
// set desired acceleration to zero
wp_nav.clear_pilot_desired_acceleration();
// set target to current position
wp_nav.init_brake_target(BRAKE_MODE_DECEL_RATE);
// initialize vertical speed and acceleration
pos_control.set_speed_z(BRAKE_MODE_SPEED_Z, BRAKE_MODE_SPEED_Z);
pos_control.set_accel_z(BRAKE_MODE_DECEL_RATE);
// initialise altitude target to stopping point
pos_control.set_target_to_stopping_point_z();
return true;
} else {
return false;
}
}
// poshold_init - initialise PosHold controller
bool Sub::poshold_init()
{
// fail to initialise PosHold mode if no GPS lock
if (!position_ok()) {
return false;
}
// initialize vertical speeds and acceleration
pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
pos_control.set_accel_z(g.pilot_accel_z);
// initialise position and desired velocity
pos_control.set_alt_target(inertial_nav.get_altitude());
pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
// set target to current position
// only init here as we can switch to PosHold in flight with a velocity <> 0 that will be used as _last_vel in PosControl and never updated again as we inhibit Reset_I
wp_nav.init_loiter_target();
last_pilot_heading = ahrs.yaw_sensor;
return true;
}
// auto_loiter_start - initialises loitering in auto mode
// returns success/failure because this can be called by exit_mission
bool Copter::auto_loiter_start()
{
// return failure if GPS is bad
if (!position_ok()) {
return false;
}
auto_mode = Auto_Loiter;
Vector3f origin = inertial_nav.get_position();
// calculate stopping point
Vector3f stopping_point;
pos_control.get_stopping_point_xy(stopping_point);
pos_control.get_stopping_point_z(stopping_point);
// initialise waypoint controller target to stopping point
wp_nav.set_wp_origin_and_destination(origin, stopping_point);
// hold yaw at current heading
set_auto_yaw_mode(AUTO_YAW_HOLD);
return true;
}
// land_gps_init - initialise gps-based land controller
bool Copter::land_gps_init()
{
// check if we have GPS and decide which LAND we're going to do
land_state.use_gps = position_ok();
if (!land_state.use_gps) {
return false;
}
// set target to stopping point
Vector3f stopping_point;
wp_nav.get_loiter_stopping_point_xy(stopping_point);
wp_nav.init_loiter_target(stopping_point);
// initialize vertical speeds and leash lengths
pos_control.set_speed_z(wp_nav.get_speed_down(), wp_nav.get_speed_up());
pos_control.set_accel_z(wp_nav.get_accel_z());
// initialise altitude target to stopping point
pos_control.set_target_to_stopping_point_z();
return true;
}
