mirror of https://github.com/ArduPilot/ardupilot
69 lines
1.8 KiB
C++
69 lines
1.8 KiB
C++
#include "Rover.h"
|
|
|
|
/*
|
|
work out if we are going to use pivot steering at next waypoint
|
|
*/
|
|
bool Rover::use_pivot_steering_at_next_WP(float yaw_error_cd)
|
|
{
|
|
// check cases where we clearly cannot use pivot steering
|
|
if (!g2.motors.have_skid_steering() || g.pivot_turn_angle <= 0) {
|
|
return false;
|
|
}
|
|
|
|
// if error is larger than pivot_turn_angle then use pivot steering at next WP
|
|
if (fabsf(yaw_error_cd) * 0.01f > g.pivot_turn_angle) {
|
|
return true;
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
/*
|
|
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);
|
|
}
|
|
}
|