Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Plane: Move cruise state into cruise flight mode.

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;
} acro_state;
// CRUISE controller state
struct CruiseState {
bool locked_heading;
int32_t locked_heading_cd;
uint32_t lock_timer_ms;
} cruise_state;
struct {
uint32_t last_tkoff_arm_time;
uint32_t last_check_ms;

6
ArduPlane/mode.h
View File

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

35
ArduPlane/mode_cruise.cpp
View File

@ -6,8 +6,8 @@ bool ModeCruise::_enter()
plane.throttle_allows_nudging = false;
plane.auto_throttle_mode = true;
plane.auto_navigation_mode = false;
plane.cruise_state.locked_heading = false;
plane.cruise_state.lock_timer_ms = 0;
locked_heading = false;
lock_timer_ms = 0;
#if SOARING_ENABLED == ENABLED
// for ArduSoar soaring_controller
@ -27,11 +27,11 @@ void ModeCruise::update()
any aileron or rudder input
*/
if (plane.channel_roll->get_control_in() != 0 || plane.channel_rudder->get_control_in() != 0) {
plane.cruise_state.locked_heading = false;
plane.cruise_state.lock_timer_ms = 0;
locked_heading = false;
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.update_load_factor();
} else {
@ -46,29 +46,34 @@ void ModeCruise::update()
*/
void ModeCruise::navigate()
{
if (!plane.cruise_state.locked_heading &&
if (!locked_heading &&
plane.channel_roll->get_control_in() == 0 &&
plane.rudder_input() == 0 &&
plane.gps.status() >= AP_GPS::GPS_OK_FIX_2D &&
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
plane.cruise_state.lock_timer_ms = millis();
lock_timer_ms = millis();
}
if (plane.cruise_state.lock_timer_ms != 0 &&
(millis() - plane.cruise_state.lock_timer_ms) > 500) {
if (lock_timer_ms != 0 &&
(millis() - lock_timer_ms) > 500) {
// lock the heading after 0.5 seconds of zero heading input
// from user
plane.cruise_state.locked_heading = true;
plane.cruise_state.lock_timer_ms = 0;
plane.cruise_state.locked_heading_cd = plane.gps.ground_course_cd();
locked_heading = true;
lock_timer_ms = 0;
locked_heading_cd = plane.gps.ground_course_cd();
plane.prev_WP_loc = plane.current_loc;
}
if (plane.cruise_state.locked_heading) {
if (locked_heading) {
plane.next_WP_loc = plane.prev_WP_loc;
// 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);
}
}
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:
return (!AP::ahrs().home_is_set() || plane.mode_loiter.isHeadingLinedUp(next_WP_loc, AP::ahrs().get_home()));
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:
break;
}