mirror of https://github.com/ArduPilot/ardupilot
Copter : correct spelling on comment
This commit is contained in:
parent
a723abf986
commit
2172095e3f
|
@ -169,7 +169,7 @@ bool AP_Arming_Copter::parameter_checks(bool display_failure)
|
||||||
}
|
}
|
||||||
|
|
||||||
char fail_msg[50];
|
char fail_msg[50];
|
||||||
// check input mangager parameters
|
// check input manager parameters
|
||||||
if (!copter.input_manager.parameter_check(fail_msg, ARRAY_SIZE(fail_msg))) {
|
if (!copter.input_manager.parameter_check(fail_msg, ARRAY_SIZE(fail_msg))) {
|
||||||
check_failed(ARMING_CHECK_PARAMETERS, display_failure, "%s", fail_msg);
|
check_failed(ARMING_CHECK_PARAMETERS, display_failure, "%s", fail_msg);
|
||||||
return false;
|
return false;
|
||||||
|
|
|
@ -191,7 +191,7 @@ void Copter::yaw_imbalance_check()
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
// thrust loss is trigerred, yaw issues are expected
|
// thrust loss is triggered, yaw issues are expected
|
||||||
if (motors->get_thrust_boost()) {
|
if (motors->get_thrust_boost()) {
|
||||||
yaw_I_filt.reset(0.0f);
|
yaw_I_filt.reset(0.0f);
|
||||||
return;
|
return;
|
||||||
|
@ -219,8 +219,8 @@ void Copter::yaw_imbalance_check()
|
||||||
|
|
||||||
const float I_max = attitude_control->get_rate_yaw_pid().imax();
|
const float I_max = attitude_control->get_rate_yaw_pid().imax();
|
||||||
if ((is_positive(I_max) && ((I > YAW_IMBALANCE_IMAX_THRESHOLD * I_max) || (is_equal(I_term,I_max))))) {
|
if ((is_positive(I_max) && ((I > YAW_IMBALANCE_IMAX_THRESHOLD * I_max) || (is_equal(I_term,I_max))))) {
|
||||||
// filtered using over precentage of I max or unfiltered = I max
|
// filtered using over percentage of I max or unfiltered = I max
|
||||||
// I makes up more than precentage of total available control power
|
// I makes up more than percentage of total available control power
|
||||||
const uint32_t now = millis();
|
const uint32_t now = millis();
|
||||||
if (now - last_yaw_warn_ms > YAW_IMBALANCE_WARN_MS) {
|
if (now - last_yaw_warn_ms > YAW_IMBALANCE_WARN_MS) {
|
||||||
last_yaw_warn_ms = now;
|
last_yaw_warn_ms = now;
|
||||||
|
|
|
@ -772,7 +772,7 @@ void Mode::precland_retry_position(const Vector3f &retry_pos)
|
||||||
}
|
}
|
||||||
|
|
||||||
// allow user to take control during repositioning. Note: copied from land_run_horizontal_control()
|
// allow user to take control during repositioning. Note: copied from land_run_horizontal_control()
|
||||||
// To-Do: this code exists at several different places in slightly diffrent forms and that should be fixed
|
// To-Do: this code exists at several different places in slightly different forms and that should be fixed
|
||||||
if (g.land_repositioning) {
|
if (g.land_repositioning) {
|
||||||
float target_roll = 0.0f;
|
float target_roll = 0.0f;
|
||||||
float target_pitch = 0.0f;
|
float target_pitch = 0.0f;
|
||||||
|
@ -796,7 +796,7 @@ void Mode::precland_retry_position(const Vector3f &retry_pos)
|
||||||
}
|
}
|
||||||
|
|
||||||
Vector3p retry_pos_NEU{retry_pos.x, retry_pos.y, retry_pos.z * -1.0f};
|
Vector3p retry_pos_NEU{retry_pos.x, retry_pos.y, retry_pos.z * -1.0f};
|
||||||
//pos contoller expects input in NEU cm's
|
// pos controller expects input in NEU cm's
|
||||||
retry_pos_NEU = retry_pos_NEU * 100.0f;
|
retry_pos_NEU = retry_pos_NEU * 100.0f;
|
||||||
pos_control->input_pos_xyz(retry_pos_NEU, 0.0f, 1000.0f);
|
pos_control->input_pos_xyz(retry_pos_NEU, 0.0f, 1000.0f);
|
||||||
|
|
||||||
|
|
|
@ -633,7 +633,7 @@ private:
|
||||||
int8_t pitch_deg; // target pitch angle in degrees. provided by mission command
|
int8_t pitch_deg; // target pitch angle in degrees. provided by mission command
|
||||||
int16_t yaw_deg; // target yaw angle in degrees. provided by mission command
|
int16_t yaw_deg; // target yaw angle in degrees. provided by mission command
|
||||||
float climb_rate; // climb rate in m/s. provided by mission command
|
float climb_rate; // climb rate in m/s. provided by mission command
|
||||||
uint32_t start_ms; // system time that nav attitude time command was recieved (used for timeout)
|
uint32_t start_ms; // system time that nav attitude time command was received (used for timeout)
|
||||||
} nav_attitude_time;
|
} nav_attitude_time;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
|
@ -41,7 +41,7 @@ bool ModeAutorotate::init(bool ignore_checks)
|
||||||
g2.arot.init_hs_controller();
|
g2.arot.init_hs_controller();
|
||||||
g2.arot.init_fwd_spd_controller();
|
g2.arot.init_fwd_spd_controller();
|
||||||
|
|
||||||
// Retrive rpm and start rpm sensor health checks
|
// Retrieve rpm and start rpm sensor health checks
|
||||||
_initial_rpm = g2.arot.get_rpm(true);
|
_initial_rpm = g2.arot.get_rpm(true);
|
||||||
|
|
||||||
// Display message
|
// Display message
|
||||||
|
@ -173,7 +173,7 @@ void ModeAutorotate::run()
|
||||||
g2.arot.set_desired_fwd_speed();
|
g2.arot.set_desired_fwd_speed();
|
||||||
|
|
||||||
// Set target head speed in head speed controller
|
// Set target head speed in head speed controller
|
||||||
_target_head_speed = HEAD_SPEED_TARGET_RATIO; //Ensure target hs is set to glide incase hs hasent reached target for glide
|
_target_head_speed = HEAD_SPEED_TARGET_RATIO; //Ensure target hs is set to glide in case hs hasent reached target for glide
|
||||||
g2.arot.set_target_head_speed(_target_head_speed);
|
g2.arot.set_target_head_speed(_target_head_speed);
|
||||||
|
|
||||||
// Prevent running the initial glide functions again
|
// Prevent running the initial glide functions again
|
||||||
|
@ -209,7 +209,7 @@ void ModeAutorotate::run()
|
||||||
gcs().send_text(MAV_SEVERITY_INFO, "Bailing Out of Autorotation");
|
gcs().send_text(MAV_SEVERITY_INFO, "Bailing Out of Autorotation");
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
// Set bail out timer remaining equal to the paramter value, bailout time
|
// Set bail out timer remaining equal to the parameter value, bailout time
|
||||||
// cannot be less than the motor spool-up time: BAILOUT_MOTOR_RAMP_TIME.
|
// cannot be less than the motor spool-up time: BAILOUT_MOTOR_RAMP_TIME.
|
||||||
_bail_time = MAX(g2.arot.get_bail_time(),BAILOUT_MOTOR_RAMP_TIME+0.1f);
|
_bail_time = MAX(g2.arot.get_bail_time(),BAILOUT_MOTOR_RAMP_TIME+0.1f);
|
||||||
|
|
||||||
|
|
|
@ -77,7 +77,7 @@ void Copter::init_ardupilot()
|
||||||
init_rc_in(); // sets up rc channels from radio
|
init_rc_in(); // sets up rc channels from radio
|
||||||
|
|
||||||
// initialise surface to be tracked in SurfaceTracking
|
// initialise surface to be tracked in SurfaceTracking
|
||||||
// must be before rc init to not override inital switch position
|
// must be before rc init to not override initial switch position
|
||||||
surface_tracking.init((SurfaceTracking::Surface)copter.g2.surftrak_mode.get());
|
surface_tracking.init((SurfaceTracking::Surface)copter.g2.surftrak_mode.get());
|
||||||
|
|
||||||
// allocate the motors class
|
// allocate the motors class
|
||||||
|
|
Loading…
Reference in New Issue