mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 14:38:30 -04:00
75ba96b7a2
This is required because AR_WPNav produces an acceleration adjusted desired speed meaning in rare cases where the vehicle is moving in reverse at the time auto is engaged, the desired speed may be temporarily negative as the vehicle slows. In these situations we do not want to allow the vehicle's speed to be nudged to a higher reverse speed if the pilot's throttle stick is all the way down
34 lines
978 B
C++
34 lines
978 B
C++
#include "mode.h"
|
|
#include "Rover.h"
|
|
|
|
void ModeSimple::init_heading()
|
|
{
|
|
_initial_heading_cd = ahrs.yaw_sensor;
|
|
_desired_heading_cd = ahrs.yaw_sensor;
|
|
}
|
|
|
|
void ModeSimple::update()
|
|
{
|
|
float desired_heading_cd, desired_speed;
|
|
|
|
// get pilot input
|
|
get_pilot_desired_heading_and_speed(desired_heading_cd, desired_speed);
|
|
|
|
// rotate heading around based on initial heading
|
|
if (g2.simple_type == Simple_InitialHeading) {
|
|
desired_heading_cd = wrap_360_cd(_initial_heading_cd + desired_heading_cd);
|
|
}
|
|
|
|
// if sticks in middle, use previous desired heading (important when vehicle is slowing down)
|
|
if (!is_positive(desired_speed)) {
|
|
desired_heading_cd = _desired_heading_cd;
|
|
} else {
|
|
// record desired heading for next iteration
|
|
_desired_heading_cd = desired_heading_cd;
|
|
}
|
|
|
|
// run throttle and steering controllers
|
|
calc_steering_to_heading(desired_heading_cd);
|
|
calc_throttle(desired_speed, true);
|
|
}
|