diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index c7134f157a..d36c6677d5 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -407,10 +407,8 @@ int32_t Copter::Mode::get_alt_above_ground(void) void Copter::Mode::land_run_vertical_control(bool pause_descent) { #if PRECISION_LANDING == ENABLED - AC_PrecLand &precland = copter.precland; - const bool navigating = pos_control->is_active_xy(); - bool doing_precision_landing = !ap.land_repo_active && precland.target_acquired() && navigating; + bool doing_precision_landing = !ap.land_repo_active && copter.precland.target_acquired() && navigating; #else bool doing_precision_landing = false; #endif @@ -452,9 +450,6 @@ void Copter::Mode::land_run_vertical_control(bool pause_descent) void Copter::Mode::land_run_horizontal_control() { - LowPassFilterFloat &rc_throttle_control_in_filter = copter.rc_throttle_control_in_filter; - AP_Vehicle::MultiCopter &aparm = copter.aparm; - float target_roll = 0.0f; float target_pitch = 0.0f; float target_yaw_rate = 0; @@ -466,7 +461,7 @@ void Copter::Mode::land_run_horizontal_control() // process pilot inputs if (!copter.failsafe.radio) { - if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){ + if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && copter.rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){ copter.Log_Write_Event(DATA_LAND_CANCELLED_BY_PILOT); // exit land if throttle is high if (!set_mode(LOITER, MODE_REASON_THROTTLE_LAND_ESCAPE)) { @@ -492,16 +487,15 @@ void Copter::Mode::land_run_horizontal_control() } #if PRECISION_LANDING == ENABLED - AC_PrecLand &precland = copter.precland; - bool doing_precision_landing = !ap.land_repo_active && precland.target_acquired(); + bool doing_precision_landing = !ap.land_repo_active && copter.precland.target_acquired(); // run precision landing if (doing_precision_landing) { Vector2f target_pos, target_vel_rel; - if (!precland.get_target_position_cm(target_pos)) { + if (!copter.precland.get_target_position_cm(target_pos)) { target_pos.x = inertial_nav.get_position().x; target_pos.y = inertial_nav.get_position().y; } - if (!precland.get_target_velocity_relative_cms(target_vel_rel)) { + if (!copter.precland.get_target_velocity_relative_cms(target_vel_rel)) { target_vel_rel.x = -inertial_nav.get_velocity().x; target_vel_rel.y = -inertial_nav.get_velocity().y; } @@ -527,7 +521,7 @@ void Copter::Mode::land_run_horizontal_control() // limit attitude to 7 degrees below this limit and linearly // interpolate for 1m above that const int alt_above_ground = get_alt_above_ground(); - float attitude_limit_cd = linear_interpolate(700, aparm.angle_max, alt_above_ground, + float attitude_limit_cd = linear_interpolate(700, copter.aparm.angle_max, alt_above_ground, g2.wp_navalt_min*100U, (g2.wp_navalt_min+1)*100U); float total_angle_cd = norm(nav_roll, nav_pitch); if (total_angle_cd > attitude_limit_cd) { diff --git a/ArduCopter/mode_acro.cpp b/ArduCopter/mode_acro.cpp index c94c0f2d5f..6df9d1585f 100644 --- a/ArduCopter/mode_acro.cpp +++ b/ArduCopter/mode_acro.cpp @@ -58,8 +58,6 @@ void Copter::ModeAcro::get_pilot_desired_angle_rates(int16_t roll_in, int16_t pi float rate_limit; Vector3f rate_ef_level, rate_bf_level, rate_bf_request; - AP_Vehicle::MultiCopter &aparm = copter.aparm; - // apply circular limit to pitch and roll inputs float total_in = norm(pitch_in, roll_in); @@ -118,16 +116,17 @@ void Copter::ModeAcro::get_pilot_desired_angle_rates(int16_t roll_in, int16_t pi // Calculate angle limiting earth frame rate commands if (g.acro_trainer == ACRO_TRAINER_LIMITED) { - if (roll_angle > aparm.angle_max){ - rate_ef_level.x -= g.acro_balance_roll*(roll_angle-aparm.angle_max); - }else if (roll_angle < -aparm.angle_max) { - rate_ef_level.x -= g.acro_balance_roll*(roll_angle+aparm.angle_max); + const float angle_max = copter.aparm.angle_max; + if (roll_angle > angle_max){ + rate_ef_level.x -= g.acro_balance_roll*(roll_angle-angle_max); + }else if (roll_angle < -angle_max) { + rate_ef_level.x -= g.acro_balance_roll*(roll_angle+angle_max); } - if (pitch_angle > aparm.angle_max){ - rate_ef_level.y -= g.acro_balance_pitch*(pitch_angle-aparm.angle_max); - }else if (pitch_angle < -aparm.angle_max) { - rate_ef_level.y -= g.acro_balance_pitch*(pitch_angle+aparm.angle_max); + if (pitch_angle > angle_max){ + rate_ef_level.y -= g.acro_balance_pitch*(pitch_angle-angle_max); + }else if (pitch_angle < -angle_max) { + rate_ef_level.y -= g.acro_balance_pitch*(pitch_angle+angle_max); } } diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index 868393895e..2a11a3e8ea 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -1002,18 +1002,17 @@ Location_Class Copter::ModeAuto::terrain_adjusted_location(const AP_Mission::Mis { // convert to location class Location_Class target_loc(cmd.content.location); - const Location_Class ¤t_loc = copter.current_loc; // decide if we will use terrain following int32_t curr_terr_alt_cm, target_terr_alt_cm; - if (current_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, curr_terr_alt_cm) && + if (copter.current_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, curr_terr_alt_cm) && target_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, target_terr_alt_cm)) { curr_terr_alt_cm = MAX(curr_terr_alt_cm,200); // if using terrain, set target altitude to current altitude above terrain target_loc.set_alt_cm(curr_terr_alt_cm, Location_Class::ALT_FRAME_ABOVE_TERRAIN); } else { // set target altitude to current altitude above home - target_loc.set_alt_cm(current_loc.alt, Location_Class::ALT_FRAME_ABOVE_HOME); + target_loc.set_alt_cm(copter.current_loc.alt, Location_Class::ALT_FRAME_ABOVE_HOME); } return target_loc; } @@ -1095,7 +1094,6 @@ void Copter::ModeAuto::do_loiter_unlimited(const AP_Mission::Mission_Command& cm { // convert back to location Location_Class target_loc(cmd.content.location); - const Location_Class ¤t_loc = copter.current_loc; // use current location if not provided if (target_loc.lat == 0 && target_loc.lng == 0) { @@ -1112,11 +1110,12 @@ void Copter::ModeAuto::do_loiter_unlimited(const AP_Mission::Mission_Command& cm if (target_loc.alt == 0) { // set to current altitude but in command's alt frame int32_t curr_alt; - if (current_loc.get_alt_cm(target_loc.get_alt_frame(),curr_alt)) { + if (copter.current_loc.get_alt_cm(target_loc.get_alt_frame(),curr_alt)) { target_loc.set_alt_cm(curr_alt, target_loc.get_alt_frame()); } else { // default to current altitude as alt-above-home - target_loc.set_alt_cm(current_loc.alt, current_loc.get_alt_frame()); + target_loc.set_alt_cm(copter.current_loc.alt, + copter.current_loc.get_alt_frame()); } } diff --git a/ArduCopter/mode_sport.cpp b/ArduCopter/mode_sport.cpp index 169d8c4407..6c2e5e66c9 100644 --- a/ArduCopter/mode_sport.cpp +++ b/ArduCopter/mode_sport.cpp @@ -51,17 +51,17 @@ void Copter::ModeSport::run() int32_t pitch_angle = wrap_180_cd(att_target.y); target_pitch_rate -= constrain_int32(pitch_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE) * g.acro_balance_pitch; - AP_Vehicle::MultiCopter &aparm = copter.aparm; - if (roll_angle > aparm.angle_max){ - target_roll_rate -= g.acro_rp_p*(roll_angle-aparm.angle_max); - }else if (roll_angle < -aparm.angle_max) { - target_roll_rate -= g.acro_rp_p*(roll_angle+aparm.angle_max); + const float angle_max = copter.aparm.angle_max; + if (roll_angle > angle_max){ + target_roll_rate -= g.acro_rp_p*(roll_angle-angle_max); + }else if (roll_angle < -angle_max) { + target_roll_rate -= g.acro_rp_p*(roll_angle+angle_max); } - if (pitch_angle > aparm.angle_max){ - target_pitch_rate -= g.acro_rp_p*(pitch_angle-aparm.angle_max); - }else if (pitch_angle < -aparm.angle_max) { - target_pitch_rate -= g.acro_rp_p*(pitch_angle+aparm.angle_max); + if (pitch_angle > angle_max){ + target_pitch_rate -= g.acro_rp_p*(pitch_angle-angle_max); + }else if (pitch_angle < -angle_max) { + target_pitch_rate -= g.acro_rp_p*(pitch_angle+angle_max); } // get pilot's desired yaw rate diff --git a/ArduCopter/mode_stabilize.cpp b/ArduCopter/mode_stabilize.cpp index 6f02bce70c..59d9ceddd4 100644 --- a/ArduCopter/mode_stabilize.cpp +++ b/ArduCopter/mode_stabilize.cpp @@ -40,10 +40,8 @@ void Copter::ModeStabilize::run() // apply SIMPLE mode transform to pilot inputs update_simple_mode(); - AP_Vehicle::MultiCopter &aparm = copter.aparm; - // convert pilot input to lean angles - get_pilot_desired_lean_angles(target_roll, target_pitch, aparm.angle_max, aparm.angle_max); + get_pilot_desired_lean_angles(target_roll, target_pitch, copter.aparm.angle_max, copter.aparm.angle_max); // get pilot's desired yaw rate target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());