Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-05 23:43:58 -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 { } else {
set_flight_stage(AP_SpdHgtControl::FLIGHT_NORMAL); 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 { } else {
set_flight_stage(AP_SpdHgtControl::FLIGHT_NORMAL); set_flight_stage(AP_SpdHgtControl::FLIGHT_NORMAL);
} }
@ -913,6 +909,13 @@ void Plane::update_flight_stage(void)
if (should_log(MASK_LOG_TECS)) { if (should_log(MASK_LOG_TECS)) {
Log_Write_TECS_Tuning(); 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 // 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) void Plane::adjust_nav_pitch_throttle(void)
{ {
uint8_t throttle = throttle_percentage(); 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; float p = (aparm.throttle_cruise - throttle) / (float)aparm.throttle_cruise;
nav_pitch_cd -= g.stab_pitch_down * 100.0f * p; 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()) { if (should_relax()) {
wp_nav->loiter_soften_for_landing(); 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 // initialize vertical speed and acceleration
pos_control->set_speed_z(-pilot_velocity_z_max, pilot_velocity_z_max); 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 see if we should provide some assistance
*/ */
if (have_airspeed && aspeed < assist_speed && 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 // the quad should provide some assistance to the plane
transition_state = TRANSITION_AIRSPEED_WAIT; transition_state = TRANSITION_AIRSPEED_WAIT;
transition_start_ms = millis(); transition_start_ms = millis();
@ -682,7 +689,11 @@ void QuadPlane::update(void)
// output to motors // output to motors
motors->output(); motors->output();
transition_start_ms = 0; 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(); last_throttle = motors->get_throttle();
} }
@ -794,12 +805,11 @@ void QuadPlane::control_auto(const Location &loc)
target.y = diff2d.y * 100; target.y = diff2d.y * 100;
target.z = loc.alt - origin.alt; 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) || millis() - last_loiter_ms > 500) {
if (!locations_are_same(loc, last_auto_target)) {
wp_nav->set_wp_destination(target); wp_nav->set_wp_destination(target);
last_auto_target = loc; last_auto_target = loc;
} }
last_loiter_ms = millis();
// initialize vertical speed and acceleration // initialize vertical speed and acceleration
pos_control->set_speed_z(-pilot_velocity_z_max, pilot_velocity_z_max); 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 // time when motors reached lower limit
uint32_t motors_lower_limit_start_ms; uint32_t motors_lower_limit_start_ms;
// time we last set the loiter target
uint32_t last_loiter_ms;
}; };