ardupilot/APMrover2/Steering.cpp

45 lines
1.2 KiB
C++
Raw Normal View History

2015-05-13 00:16:45 -03:00
#include "Rover.h"
/*
work out if we are going to use pivot steering
*/
bool Rover::use_pivot_steering(float yaw_error_cd)
{
// check cases where we clearly cannot use pivot steering
2017-11-06 17:03:08 -04:00
if (!g2.motors.have_skid_steering() || g.pivot_turn_angle <= 0) {
pivot_steering_active = false;
return false;
}
// calc bearing error
const float yaw_error = fabsf(yaw_error_cd) * 0.01f;
// if error is larger than pivot_turn_angle start pivot steering
if (yaw_error > g.pivot_turn_angle) {
pivot_steering_active = true;
return true;
}
// if within 10 degrees of the target heading, exit pivot steering
if (yaw_error < 10.0f) {
pivot_steering_active = false;
return false;
}
// by default stay in
return pivot_steering_active;
}
/*****************************************
Set the flight control servos based on the current calculated values
*****************************************/
void Rover::set_servos(void)
{
// send output signals to motors
2017-07-14 23:59:28 -03:00
if (motor_test) {
motor_test_output();
} else {
g2.motors.output(arming.is_armed() && hal.util->get_soft_armed(), G_Dt);
}
}