ardupilot/APMrover2/mode_simple.cpp

34 lines
985 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, false, true);
}