Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-28 19:48:31 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: consider flying status for throttle_wait

Browse Source
This commit is contained in:
Andrew Tridgell 2016-01-01 18:18:53 +11:00
parent 7afa2a493d
commit 5e784ddb5c
4 changed files with 26 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
ArduPlane/ArduPlane.cpp
View File

@ -895,10 +895,6 @@ void Plane::update_flight_stage(void)
} else {
set_flight_stage(AP_SpdHgtControl::FLIGHT_NORMAL);
}
} else if (control_mode == QSTABILIZE ||
control_mode == QHOVER ||
control_mode == QLOITER) {
set_flight_stage(AP_SpdHgtControl::FLIGHT_VTOL);
} else {
set_flight_stage(AP_SpdHgtControl::FLIGHT_NORMAL);
}
@ -913,6 +909,13 @@ void Plane::update_flight_stage(void)
if (should_log(MASK_LOG_TECS)) {
Log_Write_TECS_Tuning();
}
} else if (control_mode == QSTABILIZE ||
control_mode == QHOVER ||
control_mode == QLOITER ||
quadplane.in_assisted_flight()) {
set_flight_stage(AP_SpdHgtControl::FLIGHT_VTOL);
} else {
set_flight_stage(AP_SpdHgtControl::FLIGHT_NORMAL);
}
// tell AHRS the airspeed to true airspeed ratio

2
ArduPlane/Attitude.cpp
View File

@ -1103,7 +1103,7 @@ void Plane::demo_servos(uint8_t i)
void Plane::adjust_nav_pitch_throttle(void)
{
uint8_t throttle = throttle_percentage();
if (throttle < aparm.throttle_cruise) {
if (throttle < aparm.throttle_cruise && flight_stage != AP_SpdHgtControl::FLIGHT_VTOL) {
float p = (aparm.throttle_cruise - throttle) / (float)aparm.throttle_cruise;
nav_pitch_cd -= g.stab_pitch_down * 100.0f * p;
}

20
ArduPlane/quadplane.cpp
View File

@ -469,6 +469,11 @@ void QuadPlane::control_loiter()
if (should_relax()) {
wp_nav->loiter_soften_for_landing();
}
if (millis() - last_loiter_ms > 500) {
wp_nav->init_loiter_target();
}
last_loiter_ms = millis();
// initialize vertical speed and acceleration
pos_control->set_speed_z(-pilot_velocity_z_max, pilot_velocity_z_max);
@ -601,7 +606,9 @@ void QuadPlane::update_transition(void)
see if we should provide some assistance
*/
if (have_airspeed && aspeed < assist_speed &&
(plane.auto_throttle_mode || plane.channel_throttle->control_in>0)) {
(plane.auto_throttle_mode ||
plane.channel_throttle->control_in>0 ||
plane.is_flying())) {
// the quad should provide some assistance to the plane
transition_state = TRANSITION_AIRSPEED_WAIT;
transition_start_ms = millis();
@ -682,7 +689,11 @@ void QuadPlane::update(void)
// output to motors
motors->output();
transition_start_ms = 0;
transition_state = TRANSITION_AIRSPEED_WAIT;
if (throttle_wait && !plane.is_flying()) {
transition_state = TRANSITION_DONE;
} else {
transition_state = TRANSITION_AIRSPEED_WAIT;
}
last_throttle = motors->get_throttle();
}
@ -794,12 +805,11 @@ void QuadPlane::control_auto(const Location &loc)
target.y = diff2d.y * 100;
target.z = loc.alt - origin.alt;
printf("target %.2f %.2f %.2f\n", target.x*0.01, target.y*0.01, target.z*0.01);
if (!locations_are_same(loc, last_auto_target)) {
if (!locations_are_same(loc, last_auto_target) || millis() - last_loiter_ms > 500) {
wp_nav->set_wp_destination(target);
last_auto_target = loc;
}
last_loiter_ms = millis();
// initialize vertical speed and acceleration
pos_control->set_speed_z(-pilot_velocity_z_max, pilot_velocity_z_max);

3
ArduPlane/quadplane.h
View File

@ -145,4 +145,7 @@ private:
// time when motors reached lower limit
uint32_t motors_lower_limit_start_ms;
// time we last set the loiter target
uint32_t last_loiter_ms;
};