Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: Move cruise state into cruise flight mode.

Browse Source
This commit is contained in:
Samuel Tabor 2020-08-03 19:22:00 +01:00 committed by Andrew Tridgell
parent be2d03b02a
commit 67c152cef2
4 changed files with 28 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
ArduPlane/Plane.h
View File

@ -395,13 +395,6 @@ private:
int32_t locked_pitch_cd; int32_t locked_pitch_cd;
} acro_state; } acro_state;
// CRUISE controller state
struct CruiseState {
bool locked_heading;
int32_t locked_heading_cd;
uint32_t lock_timer_ms;
} cruise_state;
struct { struct {
uint32_t last_tkoff_arm_time; uint32_t last_tkoff_arm_time;
uint32_t last_check_ms; uint32_t last_check_ms;

6
ArduPlane/mode.h
View File

@ -329,9 +329,15 @@ public:
void navigate() override; void navigate() override;
bool get_target_heading_cd(int32_t &target_heading);
protected: protected:
bool _enter() override; bool _enter() override;
bool locked_heading;
int32_t locked_heading_cd;
uint32_t lock_timer_ms;
}; };
class ModeAvoidADSB : public Mode class ModeAvoidADSB : public Mode

35
ArduPlane/mode_cruise.cpp
View File

@ -6,8 +6,8 @@ bool ModeCruise::_enter()
plane.throttle_allows_nudging = false; plane.throttle_allows_nudging = false;
plane.auto_throttle_mode = true; plane.auto_throttle_mode = true;
plane.auto_navigation_mode = false; plane.auto_navigation_mode = false;
plane.cruise_state.locked_heading = false; locked_heading = false;
plane.cruise_state.lock_timer_ms = 0; lock_timer_ms = 0;
#if SOARING_ENABLED == ENABLED #if SOARING_ENABLED == ENABLED
// for ArduSoar soaring_controller // for ArduSoar soaring_controller
@ -27,11 +27,11 @@ void ModeCruise::update()
any aileron or rudder input any aileron or rudder input
*/ */
if (plane.channel_roll->get_control_in() != 0 || plane.channel_rudder->get_control_in() != 0) { if (plane.channel_roll->get_control_in() != 0 || plane.channel_rudder->get_control_in() != 0) {
plane.cruise_state.locked_heading = false; locked_heading = false;
plane.cruise_state.lock_timer_ms = 0; lock_timer_ms = 0;
} }
if (!plane.cruise_state.locked_heading) { if (!locked_heading) {
plane.nav_roll_cd = plane.channel_roll->norm_input() * plane.roll_limit_cd; plane.nav_roll_cd = plane.channel_roll->norm_input() * plane.roll_limit_cd;
plane.update_load_factor(); plane.update_load_factor();
} else { } else {
@ -46,29 +46,34 @@ void ModeCruise::update()
*/ */
void ModeCruise::navigate() void ModeCruise::navigate()
{ {
if (!plane.cruise_state.locked_heading && if (!locked_heading &&
plane.channel_roll->get_control_in() == 0 && plane.channel_roll->get_control_in() == 0 &&
plane.rudder_input() == 0 && plane.rudder_input() == 0 &&
plane.gps.status() >= AP_GPS::GPS_OK_FIX_2D && plane.gps.status() >= AP_GPS::GPS_OK_FIX_2D &&
plane.gps.ground_speed() >= 3 && plane.gps.ground_speed() >= 3 &&
plane.cruise_state.lock_timer_ms == 0) { lock_timer_ms == 0) {
// user wants to lock the heading - start the timer // user wants to lock the heading - start the timer
plane.cruise_state.lock_timer_ms = millis(); lock_timer_ms = millis();
} }
if (plane.cruise_state.lock_timer_ms != 0 && if (lock_timer_ms != 0 &&
(millis() - plane.cruise_state.lock_timer_ms) > 500) { (millis() - lock_timer_ms) > 500) {
// lock the heading after 0.5 seconds of zero heading input // lock the heading after 0.5 seconds of zero heading input
// from user // from user
plane.cruise_state.locked_heading = true; locked_heading = true;
plane.cruise_state.lock_timer_ms = 0; lock_timer_ms = 0;
plane.cruise_state.locked_heading_cd = plane.gps.ground_course_cd(); locked_heading_cd = plane.gps.ground_course_cd();
plane.prev_WP_loc = plane.current_loc; plane.prev_WP_loc = plane.current_loc;
} }
if (plane.cruise_state.locked_heading) { if (locked_heading) {
plane.next_WP_loc = plane.prev_WP_loc; plane.next_WP_loc = plane.prev_WP_loc;
// always look 1km ahead // always look 1km ahead
plane.next_WP_loc.offset_bearing(plane.cruise_state.locked_heading_cd*0.01f, plane.prev_WP_loc.get_distance(plane.current_loc) + 1000); plane.next_WP_loc.offset_bearing(locked_heading_cd*0.01f, plane.prev_WP_loc.get_distance(plane.current_loc) + 1000);
plane.nav_controller->update_waypoint(plane.prev_WP_loc, plane.next_WP_loc); plane.nav_controller->update_waypoint(plane.prev_WP_loc, plane.next_WP_loc);
} }
} }
bool ModeCruise::get_target_heading_cd(int32_t &target_heading)
{
target_heading = locked_heading_cd;
return locked_heading;
}

3
ArduPlane/soaring.cpp
View File

@ -169,7 +169,8 @@ bool Plane::soaring_exit_heading_aligned() const
case Mode::Number::FLY_BY_WIRE_B: case Mode::Number::FLY_BY_WIRE_B:
return (!AP::ahrs().home_is_set() || plane.mode_loiter.isHeadingLinedUp(next_WP_loc, AP::ahrs().get_home())); return (!AP::ahrs().home_is_set() || plane.mode_loiter.isHeadingLinedUp(next_WP_loc, AP::ahrs().get_home()));
case Mode::Number::CRUISE: case Mode::Number::CRUISE:
return (!cruise_state.locked_heading || plane.mode_loiter.isHeadingLinedUp_cd(cruise_state.locked_heading_cd)); int32_t target_heading_cd;
return (!plane.mode_cruise.get_target_heading_cd(target_heading_cd) || plane.mode_loiter.isHeadingLinedUp_cd(target_heading_cd));
default: default:
break; break;
} }