Browse Source

Copter: remove climb_rate cache of inertial_nav.get_velocity_z

There were only two users of it and dozens of places using
get_velocity_z
master
Peter Barker 6 years ago committed by Peter Barker
parent
commit
8441542a52
  1. 2
      ArduCopter/Attitude.cpp
  2. 2
      ArduCopter/Copter.h
  3. 2
      ArduCopter/Log.cpp
  4. 3
      ArduCopter/inertia.cpp

2
ArduCopter/Attitude.cpp

@ -56,7 +56,7 @@ void Copter::update_throttle_hover() @@ -56,7 +56,7 @@ void Copter::update_throttle_hover()
float throttle = motors->get_throttle();
// calc average throttle if we are in a level hover
if (throttle > 0.0f && abs(climb_rate) < 60 && labs(ahrs.roll_sensor) < 500 && labs(ahrs.pitch_sensor) < 500) {
if (throttle > 0.0f && abs(inertial_nav.get_velocity_z()) < 60 && labs(ahrs.roll_sensor) < 500 && labs(ahrs.pitch_sensor) < 500) {
// Can we set the time constant automatically
motors->update_throttle_hover(0.01f);
}

2
ArduCopter/Copter.h

@ -402,8 +402,6 @@ private: @@ -402,8 +402,6 @@ private:
#endif
// Altitude
// The cm/s we are moving up or down based on filtered data - Positive = UP
int16_t climb_rate;
float target_rangefinder_alt; // desired altitude in cm above the ground
bool target_rangefinder_alt_used; // true if mode is using target_rangefinder_alt
int32_t baro_alt; // barometer altitude in cm above home

2
ArduCopter/Log.cpp

@ -59,7 +59,7 @@ void Copter::Log_Write_Control_Tuning() @@ -59,7 +59,7 @@ void Copter::Log_Write_Control_Tuning()
rangefinder_alt : rangefinder_state.alt_cm,
terr_alt : terr_alt,
target_climb_rate : target_climb_rate_cms,
climb_rate : climb_rate
climb_rate : int16_t(inertial_nav.get_velocity_z()) // float -> int16_t
};
logger.WriteBlock(&pkt, sizeof(pkt));
}

3
ArduCopter/inertia.cpp

@ -24,7 +24,4 @@ void Copter::read_inertia() @@ -24,7 +24,4 @@ void Copter::read_inertia()
}
current_loc.set_alt_cm(inertial_nav.get_altitude(), frame);
current_loc.change_alt_frame(Location::AltFrame::ABOVE_HOME);
// set flags and get velocity
climb_rate = inertial_nav.get_velocity_z();
}

Loading…
Cancel
Save