Browse Source

Copter: reindent mode init functions (NFC)

master
Peter Barker 6 years ago committed by Randy Mackay
parent
commit
7c05364612
  1. 24
      ArduCopter/mode_brake.cpp
  2. 18
      ArduCopter/mode_circle.cpp
  3. 6
      ArduCopter/mode_guided.cpp
  4. 38
      ArduCopter/mode_loiter.cpp
  5. 10
      ArduCopter/mode_rtl.cpp

24
ArduCopter/mode_brake.cpp

@ -9,22 +9,22 @@
// brake_init - initialise brake controller // brake_init - initialise brake controller
bool Copter::ModeBrake::init(bool ignore_checks) bool Copter::ModeBrake::init(bool ignore_checks)
{ {
// set target to current position // set target to current position
wp_nav->init_brake_target(BRAKE_MODE_DECEL_RATE); wp_nav->init_brake_target(BRAKE_MODE_DECEL_RATE);
// initialize vertical speed and acceleration // initialize vertical speed and acceleration
pos_control->set_max_speed_z(BRAKE_MODE_SPEED_Z, BRAKE_MODE_SPEED_Z); pos_control->set_max_speed_z(BRAKE_MODE_SPEED_Z, BRAKE_MODE_SPEED_Z);
pos_control->set_max_accel_z(BRAKE_MODE_DECEL_RATE); pos_control->set_max_accel_z(BRAKE_MODE_DECEL_RATE);
// 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());
} }
_timeout_ms = 0; _timeout_ms = 0;
return true; return true;
} }
// brake_run - runs the brake controller // brake_run - runs the brake controller

18
ArduCopter/mode_circle.cpp

@ -9,18 +9,18 @@
// circle_init - initialise circle controller flight mode // circle_init - initialise circle controller flight mode
bool Copter::ModeCircle::init(bool ignore_checks) bool Copter::ModeCircle::init(bool ignore_checks)
{ {
pilot_yaw_override = false; pilot_yaw_override = false;
// initialize speeds and accelerations // initialize speeds and accelerations
pos_control->set_max_speed_xy(wp_nav->get_default_speed_xy()); 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_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_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);
pos_control->set_max_accel_z(g.pilot_accel_z); pos_control->set_max_accel_z(g.pilot_accel_z);
// initialise circle controller including setting the circle center based on vehicle speed // initialise circle controller including setting the circle center based on vehicle speed
copter.circle_nav->init(); copter.circle_nav->init();
return true; return true;
} }
// circle_run - runs the circle flight mode // circle_run - runs the circle flight mode

6
ArduCopter/mode_guided.cpp

@ -40,9 +40,9 @@ struct Guided_Limit {
// guided_init - initialise guided controller // guided_init - initialise guided controller
bool Copter::ModeGuided::init(bool ignore_checks) bool Copter::ModeGuided::init(bool ignore_checks)
{ {
// start in position control mode // start in position control mode
pos_control_start(); pos_control_start();
return true; return true;
} }

38
ArduCopter/mode_loiter.cpp

@ -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

10
ArduCopter/mode_rtl.cpp

@ -12,11 +12,11 @@
// rtl_init - initialise rtl controller // rtl_init - initialise rtl controller
bool Copter::ModeRTL::init(bool ignore_checks) bool Copter::ModeRTL::init(bool ignore_checks)
{ {
// initialise waypoint and spline controller // initialise waypoint and spline controller
wp_nav->wp_and_spline_init(); wp_nav->wp_and_spline_init();
build_path(!copter.failsafe.terrain); build_path(!copter.failsafe.terrain);
climb_start(); climb_start();
return true; return true;
} }
// re-start RTL with terrain following disabled // re-start RTL with terrain following disabled

Loading…
Cancel
Save