ardupilot/Rover/mode_simple.cpp

Ignoring revisions in .git-blame-ignore-revs. Click here to bypass and see the normal blame view.

33 lines
960 B
C++
Raw Normal View History

2018-07-02 04:21:37 -03:00
#include "Rover.h"
2018-09-10 01:45:06 -03:00
void ModeSimple::init_heading()
2018-07-02 04:21:37 -03:00
{
2018-09-10 01:45:06 -03:00
_initial_heading_cd = ahrs.yaw_sensor;
_desired_heading_cd = ahrs.yaw_sensor;
2018-07-02 04:21:37 -03:00
}
void ModeSimple::update()
{
2018-09-10 01:45:06 -03:00
float desired_heading_cd, desired_speed;
2018-07-02 04:21:37 -03:00
2018-09-10 01:45:06 -03:00
// get pilot input
get_pilot_desired_heading_and_speed(desired_heading_cd, desired_speed);
2018-07-02 04:21:37 -03:00
2018-09-10 01:45:06 -03:00
// 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);
2018-07-02 04:21:37 -03:00
}
2018-09-10 01:45:06 -03:00
// 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;
2018-07-02 04:21:37 -03:00
}
2018-09-10 01:45:06 -03:00
// run throttle and steering controllers
calc_steering_to_heading(desired_heading_cd);
calc_throttle(desired_speed, true);
2018-07-02 04:21:37 -03:00
}