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: added accessors for poscontrol state

Browse Source 
allows for time since state entered
This commit is contained in:
Andrew Tridgell 2021-05-18 15:15:46 +10:00
parent f1f7f01300
commit b4992cc226
2 changed files with 38 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

44
ArduPlane/quadplane.cpp
View File

@ -1335,7 +1335,7 @@ void QuadPlane::init_qland(void)
init_loiter(); init_loiter();
throttle_wait = false; throttle_wait = false;
setup_target_position(); setup_target_position();
poscontrol.state = QPOS_LAND_DESCEND; poscontrol.set_state(QPOS_LAND_DESCEND);
poscontrol.pilot_correction_done = false; poscontrol.pilot_correction_done = false;
last_land_final_agl = plane.relative_ground_altitude(plane.g.rangefinder_landing); last_land_final_agl = plane.relative_ground_altitude(plane.g.rangefinder_landing);
landing_detect.lower_limit_start_ms = 0; landing_detect.lower_limit_start_ms = 0;
@ -1426,7 +1426,7 @@ bool QuadPlane::is_flying_vtol(void) const
*/ */
float QuadPlane::landing_descent_rate_cms(float height_above_ground) const float QuadPlane::landing_descent_rate_cms(float height_above_ground) const
{ {
if (poscontrol.state == QPOS_LAND_FINAL) { if (poscontrol.get_state() == QPOS_LAND_FINAL) {
// when in final use descent rate for final even if alt has climbed again // when in final use descent rate for final even if alt has climbed again
height_above_ground = MIN(height_above_ground, land_final_alt); height_above_ground = MIN(height_above_ground, land_final_alt);
} }
@ -1524,8 +1524,8 @@ void QuadPlane::control_loiter()
get_desired_yaw_rate_cds()); get_desired_yaw_rate_cds());
if (plane.control_mode == &plane.mode_qland) { if (plane.control_mode == &plane.mode_qland) {
if (poscontrol.state < QPOS_LAND_FINAL && check_land_final()) { if (poscontrol.get_state() < QPOS_LAND_FINAL && check_land_final()) {
poscontrol.state = QPOS_LAND_FINAL; poscontrol.set_state(QPOS_LAND_FINAL);
setup_target_position(); setup_target_position();
// cut IC engine if enabled // cut IC engine if enabled
if (land_icengine_cut != 0) { if (land_icengine_cut != 0) {
@ -1535,7 +1535,7 @@ void QuadPlane::control_loiter()
float height_above_ground = plane.relative_ground_altitude(plane.g.rangefinder_landing); float height_above_ground = plane.relative_ground_altitude(plane.g.rangefinder_landing);
float descent_rate_cms = landing_descent_rate_cms(height_above_ground); float descent_rate_cms = landing_descent_rate_cms(height_above_ground);
if (poscontrol.state == QPOS_LAND_FINAL && (options & OPTION_DISABLE_GROUND_EFFECT_COMP) == 0) { if (poscontrol.get_state() == QPOS_LAND_FINAL && (options & OPTION_DISABLE_GROUND_EFFECT_COMP) == 0) {
ahrs.set_touchdown_expected(true); ahrs.set_touchdown_expected(true);
} }
@ -2557,7 +2557,7 @@ void QuadPlane::vtol_position_controller(void)
check_attitude_relax(); check_attitude_relax();
// horizontal position control // horizontal position control
switch (poscontrol.state) { switch (poscontrol.get_state()) {
case QPOS_POSITION1: { case QPOS_POSITION1: {
setup_target_position(); setup_target_position();
@ -2650,7 +2650,7 @@ void QuadPlane::vtol_position_controller(void)
desired_auto_yaw_rate_cds() + get_weathervane_yaw_rate_cds()); desired_auto_yaw_rate_cds() + get_weathervane_yaw_rate_cds());
if (plane.auto_state.wp_proportion >= 1 || if (plane.auto_state.wp_proportion >= 1 ||
plane.auto_state.wp_distance < 5) { plane.auto_state.wp_distance < 5) {
poscontrol.state = QPOS_POSITION2; poscontrol.set_state(QPOS_POSITION2);
poscontrol.pilot_correction_done = false; poscontrol.pilot_correction_done = false;
gcs().send_text(MAV_SEVERITY_INFO,"VTOL position2 started v=%.1f d=%.1f", gcs().send_text(MAV_SEVERITY_INFO,"VTOL position2 started v=%.1f d=%.1f",
(double)ahrs.groundspeed(), (double)plane.auto_state.wp_distance); (double)ahrs.groundspeed(), (double)plane.auto_state.wp_distance);
@ -2721,7 +2721,7 @@ void QuadPlane::vtol_position_controller(void)
} }
// now height control // now height control
switch (poscontrol.state) { switch (poscontrol.get_state()) {
case QPOS_POSITION1: case QPOS_POSITION1:
case QPOS_POSITION2: { case QPOS_POSITION2: {
bool vtol_loiter_auto = false; bool vtol_loiter_auto = false;
@ -2772,7 +2772,7 @@ void QuadPlane::vtol_position_controller(void)
case QPOS_LAND_DESCEND: case QPOS_LAND_DESCEND:
case QPOS_LAND_FINAL: { case QPOS_LAND_FINAL: {
float height_above_ground = plane.relative_ground_altitude(plane.g.rangefinder_landing); float height_above_ground = plane.relative_ground_altitude(plane.g.rangefinder_landing);
if (poscontrol.state == QPOS_LAND_FINAL) { if (poscontrol.get_state() == QPOS_LAND_FINAL) {
if ((options & OPTION_DISABLE_GROUND_EFFECT_COMP) == 0) { if ((options & OPTION_DISABLE_GROUND_EFFECT_COMP) == 0) {
ahrs.set_touchdown_expected(true); ahrs.set_touchdown_expected(true);
} }
@ -2927,7 +2927,7 @@ void QuadPlane::control_auto(void)
void QuadPlane::control_qrtl(void) void QuadPlane::control_qrtl(void)
{ {
vtol_position_controller(); vtol_position_controller();
if (poscontrol.state >= QPOS_POSITION2) { if (poscontrol.get_state() >= QPOS_POSITION2) {
// change target altitude to home alt // change target altitude to home alt
plane.next_WP_loc.alt = plane.home.alt; plane.next_WP_loc.alt = plane.home.alt;
verify_vtol_land(); verify_vtol_land();
@ -2942,7 +2942,7 @@ void QuadPlane::init_qrtl(void)
// use do_RTL() to setup next_WP_loc // use do_RTL() to setup next_WP_loc
plane.do_RTL(plane.home.alt + qrtl_alt*100UL); plane.do_RTL(plane.home.alt + qrtl_alt*100UL);
plane.prev_WP_loc = plane.current_loc; plane.prev_WP_loc = plane.current_loc;
poscontrol.state = QPOS_POSITION1; poscontrol.set_state(QPOS_POSITION1);
poscontrol.speed_scale = 0; poscontrol.speed_scale = 0;
pos_control->set_accel_desired_xy_cmss(Vector2f()); pos_control->set_accel_desired_xy_cmss(Vector2f());
pos_control->init_xy_controller(); pos_control->init_xy_controller();
@ -3030,7 +3030,7 @@ bool QuadPlane::do_vtol_land(const AP_Mission::Mission_Command& cmd)
plane.set_next_WP(cmd.content.location); plane.set_next_WP(cmd.content.location);
// initially aim for current altitude // initially aim for current altitude
plane.next_WP_loc.alt = plane.current_loc.alt; plane.next_WP_loc.alt = plane.current_loc.alt;
poscontrol.state = QPOS_POSITION1; poscontrol.set_state(QPOS_POSITION1);
poscontrol.speed_scale = 0; poscontrol.speed_scale = 0;
// initialise the position controller // initialise the position controller
@ -3149,12 +3149,12 @@ bool QuadPlane::land_detector(uint32_t timeout_ms)
*/ */
bool QuadPlane::check_land_complete(void) bool QuadPlane::check_land_complete(void)
{ {
if (poscontrol.state != QPOS_LAND_FINAL) { if (poscontrol.get_state() != QPOS_LAND_FINAL) {
// only apply to final landing phase // only apply to final landing phase
return false; return false;
} }
if (land_detector(4000)) { if (land_detector(4000)) {
poscontrol.state = QPOS_LAND_COMPLETE; poscontrol.set_state(QPOS_LAND_COMPLETE);
gcs().send_text(MAV_SEVERITY_INFO,"Land complete"); gcs().send_text(MAV_SEVERITY_INFO,"Land complete");
if (plane.control_mode != &plane.mode_auto || if (plane.control_mode != &plane.mode_auto ||
!plane.mission.continue_after_land()) { !plane.mission.continue_after_land()) {
@ -3199,10 +3199,10 @@ bool QuadPlane::verify_vtol_land(void)
if (!available()) { if (!available()) {
return true; return true;
} }
if (poscontrol.state == QPOS_POSITION2 && if (poscontrol.get_state() == QPOS_POSITION2 &&
plane.auto_state.wp_distance < 2 && plane.auto_state.wp_distance < 2 &&
plane.ahrs.groundspeed() < 3) { plane.ahrs.groundspeed() < 3) {
poscontrol.state = QPOS_LAND_DESCEND; poscontrol.set_state(QPOS_LAND_DESCEND);
poscontrol.pilot_correction_done = false; poscontrol.pilot_correction_done = false;
#if AC_FENCE == ENABLED #if AC_FENCE == ENABLED
plane.fence.auto_disable_fence_for_landing(); plane.fence.auto_disable_fence_for_landing();
@ -3224,8 +3224,8 @@ bool QuadPlane::verify_vtol_land(void)
} }
// at land_final_alt begin final landing // at land_final_alt begin final landing
if (poscontrol.state == QPOS_LAND_DESCEND && check_land_final()) { if (poscontrol.get_state() == QPOS_LAND_DESCEND && check_land_final()) {
poscontrol.state = QPOS_LAND_FINAL; poscontrol.set_state(QPOS_LAND_FINAL);
// cut IC engine if enabled // cut IC engine if enabled
if (land_icengine_cut != 0) { if (land_icengine_cut != 0) {
@ -3447,7 +3447,7 @@ float QuadPlane::get_weathervane_yaw_rate_cds(void)
*/ */
void QuadPlane::guided_start(void) void QuadPlane::guided_start(void)
{ {
poscontrol.state = QPOS_POSITION1; poscontrol.set_state(QPOS_POSITION1);
poscontrol.speed_scale = 0; poscontrol.speed_scale = 0;
guided_takeoff = false; guided_takeoff = false;
setup_target_position(); setup_target_position();
@ -3673,7 +3673,7 @@ void QuadPlane::update_throttle_mix(void)
bool QuadPlane::in_vtol_land_approach(void) const bool QuadPlane::in_vtol_land_approach(void) const
{ {
if (in_vtol_auto() && is_vtol_land(plane.mission.get_current_nav_cmd().id) && if (in_vtol_auto() && is_vtol_land(plane.mission.get_current_nav_cmd().id) &&
(poscontrol.state == QPOS_POSITION1 || poscontrol.state == QPOS_POSITION2)) { (poscontrol.get_state() == QPOS_POSITION1 || poscontrol.get_state() == QPOS_POSITION2)) {
return true; return true;
} }
return false; return false;
@ -3685,7 +3685,7 @@ bool QuadPlane::in_vtol_land_approach(void) const
bool QuadPlane::in_vtol_land_descent(void) const bool QuadPlane::in_vtol_land_descent(void) const
{ {
if (in_vtol_auto() && is_vtol_land(plane.mission.get_current_nav_cmd().id) && if (in_vtol_auto() && is_vtol_land(plane.mission.get_current_nav_cmd().id) &&
(poscontrol.state == QPOS_LAND_DESCEND || poscontrol.state == QPOS_LAND_FINAL)) { (poscontrol.get_state() == QPOS_LAND_DESCEND || poscontrol.get_state() == QPOS_LAND_FINAL)) {
return true; return true;
} }
return false; return false;
@ -3696,7 +3696,7 @@ bool QuadPlane::in_vtol_land_descent(void) const
*/ */
bool QuadPlane::in_vtol_land_final(void) const bool QuadPlane::in_vtol_land_final(void) const
{ {
return in_vtol_land_descent() && poscontrol.state == QPOS_LAND_FINAL; return in_vtol_land_descent() && poscontrol.get_state() == QPOS_LAND_FINAL;
} }
/* /*

15
ArduPlane/quadplane.h
View File

@ -466,7 +466,17 @@ private:
QPOS_LAND_COMPLETE QPOS_LAND_COMPLETE
}; };
struct { struct {
enum position_control_state state; public:
enum position_control_state get_state() const {
return state;
}
void set_state(enum position_control_state s) {
state = s;
last_state_change_ms = AP_HAL::millis();
}
uint32_t time_since_state_start_ms() const {
return AP_HAL::millis() - last_state_change_ms;
}
float speed_scale; float speed_scale;
Vector2f target_velocity; Vector2f target_velocity;
float max_speed; float max_speed;
@ -475,6 +485,9 @@ private:
bool slow_descent:1; bool slow_descent:1;
bool pilot_correction_active; bool pilot_correction_active;
bool pilot_correction_done; bool pilot_correction_done;
private:
uint32_t last_state_change_ms;
enum position_control_state state;
} poscontrol; } poscontrol;
struct { struct {