You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
33 lines
985 B
33 lines
985 B
#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); |
|
}
|
|
|