ardupilot/APMrover2/Steering.cpp

51 lines
1.3 KiB
C++

#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
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
if (motor_test) {
motor_test_output();
} else {
// get ground speed
float speed;
if (!g2.attitude_control.get_forward_speed(speed)) {
speed = 0.0f;
}
g2.motors.output(arming.is_armed(), speed, G_Dt);
}
}