mirror of https://github.com/ArduPilot/ardupilot
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);
|
|
}
|