mirror of https://github.com/ArduPilot/ardupilot
131 lines
3.6 KiB
C++
131 lines
3.6 KiB
C++
#include "mode.h"
|
|
#include "Rover.h"
|
|
|
|
bool ModeAuto::_enter()
|
|
{
|
|
// fail to enter auto if no mission commands
|
|
if (mission.num_commands() == 0) {
|
|
gcs().send_text(MAV_SEVERITY_NOTICE, "No Mission. Can't set AUTO.");
|
|
return false;
|
|
}
|
|
|
|
auto_triggered = false;
|
|
rover.restart_nav();
|
|
rover.loiter_start_time = 0;
|
|
g2.motors.slew_limit_throttle(true);
|
|
return true;
|
|
}
|
|
|
|
void ModeAuto::_exit()
|
|
{
|
|
// If we are changing out of AUTO mode reset the loiter timer
|
|
rover.loiter_start_time = 0;
|
|
// ... and stop running the mission
|
|
if (mission.state() == AP_Mission::MISSION_RUNNING) {
|
|
mission.stop();
|
|
}
|
|
}
|
|
|
|
void ModeAuto::update()
|
|
{
|
|
if (!rover.in_auto_reverse) {
|
|
rover.set_reverse(false);
|
|
}
|
|
if (!rover.do_auto_rotation) {
|
|
calc_lateral_acceleration();
|
|
calc_nav_steer();
|
|
calc_throttle(g.speed_cruise);
|
|
} else {
|
|
rover.do_yaw_rotation();
|
|
}
|
|
}
|
|
|
|
void ModeAuto::update_navigation()
|
|
{
|
|
mission.update();
|
|
if (rover.do_auto_rotation) {
|
|
rover.do_yaw_rotation();
|
|
}
|
|
}
|
|
|
|
/*
|
|
check for triggering of start of auto mode
|
|
*/
|
|
bool ModeAuto::check_trigger(void)
|
|
{
|
|
// check for user pressing the auto trigger to off
|
|
if (auto_triggered && g.auto_trigger_pin != -1 && rover.check_digital_pin(g.auto_trigger_pin) == 1) {
|
|
gcs().send_text(MAV_SEVERITY_WARNING, "AUTO triggered off");
|
|
auto_triggered = false;
|
|
return false;
|
|
}
|
|
|
|
// if already triggered, then return true, so you don't
|
|
// need to hold the switch down
|
|
if (auto_triggered) {
|
|
return true;
|
|
}
|
|
|
|
// return true if auto trigger and kickstart are disabled
|
|
if (g.auto_trigger_pin == -1 && is_zero(g.auto_kickstart)) {
|
|
// no trigger configured - let's go!
|
|
auto_triggered = true;
|
|
return true;
|
|
}
|
|
|
|
// check if trigger pin has been pushed
|
|
if (g.auto_trigger_pin != -1 && rover.check_digital_pin(g.auto_trigger_pin) == 0) {
|
|
gcs().send_text(MAV_SEVERITY_WARNING, "Triggered AUTO with pin");
|
|
auto_triggered = true;
|
|
return true;
|
|
}
|
|
|
|
// check if mission is started by giving vehicle a kick with acceleration > AUTO_KICKSTART
|
|
if (!is_zero(g.auto_kickstart)) {
|
|
const float xaccel = rover.ins.get_accel().x;
|
|
if (xaccel >= g.auto_kickstart) {
|
|
gcs().send_text(MAV_SEVERITY_WARNING, "Triggered AUTO xaccel=%.1f", static_cast<double>(xaccel));
|
|
auto_triggered = true;
|
|
return true;
|
|
}
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
void ModeAuto::calc_throttle(float target_speed)
|
|
{
|
|
// If not autostarting OR we are loitering at a waypoint
|
|
// then set the throttle to minimum
|
|
if (!check_trigger() || rover.in_stationary_loiter()) {
|
|
g2.motors.set_throttle(g.throttle_min.get());
|
|
// Stop rotation in case of loitering and skid steering
|
|
if (g2.motors.have_skid_steering()) {
|
|
g2.motors.set_steering(0.0f);
|
|
}
|
|
return;
|
|
}
|
|
Mode::calc_throttle(target_speed);
|
|
}
|
|
|
|
void ModeAuto::calc_lateral_acceleration()
|
|
{
|
|
// If we have reached the waypoint previously navigate
|
|
// back to it from our current position
|
|
if (rover.previously_reached_wp && (rover.loiter_duration > 0)) {
|
|
Mode::calc_lateral_acceleration(rover.current_loc, rover.next_WP);
|
|
} else {
|
|
Mode::calc_lateral_acceleration(rover.prev_WP, rover.next_WP);
|
|
}
|
|
}
|
|
|
|
void ModeAuto::calc_nav_steer()
|
|
{
|
|
// check to see if the rover is loitering
|
|
if (rover.in_stationary_loiter()) {
|
|
g2.motors.set_steering(0.0f);
|
|
return;
|
|
}
|
|
Mode::calc_nav_steer();
|
|
}
|