ardupilot/Rover/mode_simple.cpp
Shiv Tyagi a8a952d656 Rover: include fix in mode.h
Co-authored-by: Peter Barker <pbarker@barker.dropbear.id.au>
2022-08-22 10:04:22 +10:00

33 lines
960 B
C++

#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);
}