aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Plane: moved sink_rate calculation to update_alt()

Browse Source 
this makes it available to non-landing code
This commit is contained in:
Andrew Tridgell 2015-06-13 19:11:49 +10:00
parent 5e80f0cf72
commit 358a13261c
4 changed files with 20 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
ArduPlane/ArduPlane.cpp
View File

@ -503,7 +503,6 @@ void Plane::handle_auto_mode(void)
// are for takeoff and landing
steer_state.hold_course_cd = -1;
auto_state.land_complete = false;
auto_state.land_sink_rate = 0;
calc_nav_roll();
calc_nav_pitch();
calc_throttle();
@ -783,6 +782,20 @@ void Plane::update_alt()
Log_Write_Baro();
}
// calculate the sink rate.
float sink_rate;
Vector3f vel;
if (ahrs.get_velocity_NED(vel)) {
sink_rate = vel.z;
} else if (gps.status() >= AP_GPS::GPS_OK_FIX_3D && gps.have_vertical_velocity()) {
sink_rate = gps.velocity().z;
} else {
sink_rate = -barometer.get_climb_rate();
}
// low pass the sink rate to take some of the noise out
auto_state.sink_rate = 0.8f * auto_state.sink_rate + 0.2f*sink_rate;
geofence_check(true);
update_flight_stage();
@ -852,7 +865,7 @@ void Plane::determine_is_flying(void)
make is_flying() more accurate for landing approach
*/
if (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH &&
fabsf(auto_state.land_sink_rate) > 0.2f) {
fabsf(auto_state.sink_rate) > 0.2f) {
isFlyingBool = true;
}
} else {

2
ArduPlane/Plane.h
View File

@ -452,7 +452,7 @@ private:
float next_turn_angle {90};
// filtered sink rate for landing
float land_sink_rate;
float sink_rate;
// time when we first pass min GPS speed on takeoff
uint32_t takeoff_speed_time_ms;

2
ArduPlane/commands_logic.cpp
View File

@ -16,7 +16,7 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd)
if (AP_Mission::is_nav_cmd(cmd)) {
// set land_complete to false to stop us zeroing the throttle
auto_state.land_complete = false;
auto_state.land_sink_rate = 0;
auto_state.sink_rate = 0;
// set takeoff_complete to true so we don't add extra evevator
// except in a takeoff

20
ArduPlane/landing.cpp
View File

@ -30,20 +30,6 @@ bool Plane::verify_land()
// use rangefinder to correct if possible
height -= rangefinder_correction();
// calculate the sink rate.
float sink_rate;
Vector3f vel;
if (ahrs.get_velocity_NED(vel)) {
sink_rate = vel.z;
} else if (gps.status() >= AP_GPS::GPS_OK_FIX_3D && gps.have_vertical_velocity()) {
sink_rate = gps.velocity().z;
} else {
sink_rate = -barometer.get_climb_rate();
}
// low pass the sink rate to take some of the noise out
auto_state.land_sink_rate = 0.8f * auto_state.land_sink_rate + 0.2f*sink_rate;
/* Set land_complete (which starts the flare) under 3 conditions:
1) we are within LAND_FLARE_ALT meters of the landing altitude
2) we are within LAND_FLARE_SEC of the landing point vertically
@ -58,16 +44,16 @@ bool Plane::verify_land()
bool rangefinder_in_range = false;
#endif
if (height <= g.land_flare_alt ||
(aparm.land_flare_sec > 0 && height <= auto_state.land_sink_rate * aparm.land_flare_sec) ||
(aparm.land_flare_sec > 0 && height <= auto_state.sink_rate * aparm.land_flare_sec) ||
(!rangefinder_in_range && location_passed_point(current_loc, prev_WP_loc, next_WP_loc)) ||
(fabsf(auto_state.land_sink_rate) < 0.2f && !is_flying())) {
(fabsf(auto_state.sink_rate) < 0.2f && !is_flying())) {
if (!auto_state.land_complete) {
if (!is_flying() && (millis()-auto_state.last_flying_ms) > 3000) {
gcs_send_text_fmt(PSTR("Flare crash detected: speed=%.1f"), (double)gps.ground_speed());
} else {
gcs_send_text_fmt(PSTR("Flare %.1fm sink=%.2f speed=%.1f"),
(double)height, (double)auto_state.land_sink_rate, (double)gps.ground_speed());
(double)height, (double)auto_state.sink_rate, (double)gps.ground_speed());
}
}
auto_state.land_complete = true;