Browse Source

Copter: stop shadowing members of Copter

These references were taken to make the breaking out of Modes in Copter.

A lot of other code has already caused these sorts of things to go away,
but these particular ones seem reasonable to fix by pointing the users
at the copter object directly.
master
Peter Barker 6 years ago committed by Andrew Tridgell
parent
commit
5f552a6ce3
  1. 18
      ArduCopter/mode.cpp
  2. 19
      ArduCopter/mode_acro.cpp
  3. 11
      ArduCopter/mode_auto.cpp
  4. 18
      ArduCopter/mode_sport.cpp
  5. 4
      ArduCopter/mode_stabilize.cpp

18
ArduCopter/mode.cpp

@ -407,10 +407,8 @@ int32_t Copter::Mode::get_alt_above_ground(void) @@ -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) @@ -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() @@ -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() @@ -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() @@ -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) {

19
ArduCopter/mode_acro.cpp

@ -58,8 +58,6 @@ void Copter::ModeAcro::get_pilot_desired_angle_rates(int16_t roll_in, int16_t pi @@ -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 @@ -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);
}
}

11
ArduCopter/mode_auto.cpp

@ -1002,18 +1002,17 @@ Location_Class Copter::ModeAuto::terrain_adjusted_location(const AP_Mission::Mis @@ -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 &current_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 @@ -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 &current_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 @@ -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());
}
}

18
ArduCopter/mode_sport.cpp

@ -51,17 +51,17 @@ void Copter::ModeSport::run() @@ -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

4
ArduCopter/mode_stabilize.cpp

@ -40,10 +40,8 @@ void Copter::ModeStabilize::run() @@ -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());

Loading…
Cancel
Save