diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index b7196fa31a..e379e3ce81 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -999,11 +999,11 @@ void Copter::ModeAuto::payload_place_run() bool Copter::ModeAuto::payload_place_run_should_run() { - // muts be armed + // must be armed if (!motors->armed()) { return false; } - // muts be auto-armed + // must be auto-armed if (!ap.auto_armed) { return false; } diff --git a/ArduCopter/mode_brake.cpp b/ArduCopter/mode_brake.cpp index 71182879f0..4977a8a9a8 100644 --- a/ArduCopter/mode_brake.cpp +++ b/ArduCopter/mode_brake.cpp @@ -52,8 +52,6 @@ void Copter::ModeBrake::run() // call attitude controller attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), 0.0f); - // body-frame rate controller is run directly from 100hz loop - // update altitude target and call position controller // protects heli's from inflight motor interlock disable if (motors->get_desired_spool_state() == AP_Motors::DESIRED_GROUND_IDLE && !ap.land_complete) { diff --git a/ArduCopter/mode_circle.cpp b/ArduCopter/mode_circle.cpp index 1952098be0..8b478eb186 100644 --- a/ArduCopter/mode_circle.cpp +++ b/ArduCopter/mode_circle.cpp @@ -64,7 +64,7 @@ void Copter::ModeCircle::run() attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(copter.circle_nav->get_roll(), copter.circle_nav->get_pitch(), target_yaw_rate); - }else{ + } else { attitude_control->input_euler_angle_roll_pitch_yaw(copter.circle_nav->get_roll(), copter.circle_nav->get_pitch(), copter.circle_nav->get_yaw(), true); diff --git a/ArduCopter/mode_drift.cpp b/ArduCopter/mode_drift.cpp index 69aed245a0..b8ff409c2f 100644 --- a/ArduCopter/mode_drift.cpp +++ b/ArduCopter/mode_drift.cpp @@ -52,29 +52,29 @@ void Copter::ModeDrift::run() float roll_vel = vel.y * ahrs.cos_yaw() - vel.x * ahrs.sin_yaw(); // body roll vel float pitch_vel = vel.y * ahrs.sin_yaw() + vel.x * ahrs.cos_yaw(); // body pitch vel - // gain sceduling for Yaw + // gain scheduling for yaw float pitch_vel2 = MIN(fabsf(pitch_vel), 2000); float target_yaw_rate = ((float)target_roll/1.0f) * (1.0f - (pitch_vel2 / 5000.0f)) * g.acro_yaw_p; roll_vel = constrain_float(roll_vel, -DRIFT_SPEEDLIMIT, DRIFT_SPEEDLIMIT); pitch_vel = constrain_float(pitch_vel, -DRIFT_SPEEDLIMIT, DRIFT_SPEEDLIMIT); - + roll_input = roll_input * .96f + (float)channel_yaw->get_control_in() * .04f; - //convert user input into desired roll velocity + // convert user input into desired roll velocity float roll_vel_error = roll_vel - (roll_input / DRIFT_SPEEDGAIN); - // Roll velocity is feed into roll acceleration to minimize slip + // roll velocity is feed into roll acceleration to minimize slip target_roll = roll_vel_error * -DRIFT_SPEEDGAIN; target_roll = constrain_float(target_roll, -4500.0f, 4500.0f); // If we let go of sticks, bring us to a stop - if(is_zero(target_pitch)){ + if (is_zero(target_pitch)) { // .14/ (.03 * 100) = 4.6 seconds till full braking braker += .03f; braker = MIN(braker, DRIFT_SPEEDGAIN); target_pitch = pitch_vel * braker; - }else{ + } else { braker = 0.0f; } diff --git a/ArduCopter/mode_flowhold.cpp b/ArduCopter/mode_flowhold.cpp index d18dbf89f7..9fa497effe 100644 --- a/ArduCopter/mode_flowhold.cpp +++ b/ArduCopter/mode_flowhold.cpp @@ -226,7 +226,7 @@ void Copter::ModeFlowHold::run() copter.pos_control->set_max_accel_z(copter.g.pilot_accel_z); // apply SIMPLE mode transform to pilot inputs - copter.update_simple_mode(); + update_simple_mode(); // check for filter change if (!is_equal(flow_filter.get_cutoff_freq(), flow_filter_hz.get())) { diff --git a/ArduCopter/mode_poshold.cpp b/ArduCopter/mode_poshold.cpp index 0d48505db5..222406f423 100644 --- a/ArduCopter/mode_poshold.cpp +++ b/ArduCopter/mode_poshold.cpp @@ -103,7 +103,7 @@ bool Copter::ModePosHold::init(bool ignore_checks) // if landed begin in loiter mode poshold.roll_mode = POSHOLD_LOITER; poshold.pitch_mode = POSHOLD_LOITER; - }else{ + } else { // if not landed start in pilot override to avoid hard twitch poshold.roll_mode = POSHOLD_PILOT_OVERRIDE; poshold.pitch_mode = POSHOLD_PILOT_OVERRIDE; diff --git a/ArduCopter/mode_stabilize.cpp b/ArduCopter/mode_stabilize.cpp index f06dd433f4..aac81c0cb2 100644 --- a/ArduCopter/mode_stabilize.cpp +++ b/ArduCopter/mode_stabilize.cpp @@ -46,8 +46,6 @@ void Copter::ModeStabilize::run() // call attitude controller attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); - // body-frame rate controller is run directly from 100hz loop - // output pilot's throttle attitude_control->set_throttle_out(get_pilot_desired_throttle(), true,