Rover: add control modes class

APMrover2/mode.cpp

@@ -0,0 +1,149 @@
+#include "mode.h"
+#include "Rover.h"
+
+Mode::Mode() :
+    g(rover.g),
+    g2(rover.g2),
+    channel_steer(rover.channel_steer),
+    channel_throttle(rover.channel_throttle),
+    mission(rover.mission)
+{ }
+
+void Mode::exit()
+{
+    // call sub-classes exit
+    _exit();
+
+    lateral_acceleration = 0.0f;
+    rover.throttle = 500;
+    rover.g.pidSpeedThrottle.reset_I();
+    if (!rover.in_auto_reverse) {
+        rover.set_reverse(false);
+    }
+    rover.rtl_complete = false;
+}
+
+bool Mode::enter()
+{
+    return _enter();
+}
+
+void Mode::calc_throttle(float target_speed)
+{
+    const int16_t &throttle_nudge = rover.throttle_nudge;
+    int16_t &throttle = rover.throttle;
+    const int32_t next_navigation_leg_cd = rover.next_navigation_leg_cd;
+    const AP_AHRS &ahrs = rover.ahrs;
+    const float wp_distance = rover.wp_distance;
+    float &groundspeed_error = rover.groundspeed_error;
+    const float ground_speed = rover.ground_speed;
+
+    const float throttle_base = (fabsf(target_speed) / g.speed_cruise) * g.throttle_cruise;
+    const int throttle_target = throttle_base + throttle_nudge;
+
+    /*
+        reduce target speed in proportion to turning rate, up to the
+        SPEED_TURN_GAIN percentage.
+    */
+    float steer_rate = fabsf(lateral_acceleration / (g.turn_max_g * GRAVITY_MSS));
+    steer_rate = constrain_float(steer_rate, 0.0f, 1.0f);
+
+    // use g.speed_turn_gain for a 90 degree turn, and in proportion
+    // for other turn angles
+    const int32_t turn_angle = wrap_180_cd(next_navigation_leg_cd - ahrs.yaw_sensor);
+    const float speed_turn_ratio = constrain_float(fabsf(turn_angle / 9000.0f), 0.0f, 1.0f);
+    const float speed_turn_reduction = (100 - g.speed_turn_gain) * speed_turn_ratio * 0.01f;
+
+    float reduction = 1.0f - steer_rate * speed_turn_reduction;
+
+    if (is_autopilot_mode() && rover.mode_guided.guided_mode != ModeGuided::Guided_Velocity && wp_distance <= g.speed_turn_dist) {
+        // in auto-modes we reduce speed when approaching waypoints
+        const float reduction2 = 1.0f - speed_turn_reduction;
+        if (reduction2 < reduction) {
+            reduction = reduction2;
+        }
+    }
+
+    // reduce the target speed by the reduction factor
+    target_speed *= reduction;
+
+    groundspeed_error = fabsf(target_speed) - ground_speed;
+
+    throttle = throttle_target + (g.pidSpeedThrottle.get_pid(groundspeed_error * 100.0f) / 100.0f);
+
+    // also reduce the throttle by the reduction factor. This gives a
+    // much faster response in turns
+    throttle *= reduction;
+
+    if (rover.in_reverse) {
+        g2.motors.set_throttle(constrain_int16(-throttle, -g.throttle_max, -g.throttle_min));
+    } else {
+        g2.motors.set_throttle(constrain_int16(throttle, g.throttle_min, g.throttle_max));
+    }
+
+    if (!rover.in_reverse && g.braking_percent != 0 && groundspeed_error < -g.braking_speederr) {
+        // the user has asked to use reverse throttle to brake. Apply
+        // it in proportion to the ground speed error, but only when
+        // our ground speed error is more than BRAKING_SPEEDERR.
+        //
+        // We use a linear gain, with 0 gain at a ground speed error
+        // of braking_speederr, and 100% gain when groundspeed_error
+        // is 2*braking_speederr
+        const float brake_gain = constrain_float(((-groundspeed_error)-g.braking_speederr)/g.braking_speederr, 0.0f, 1.0f);
+        const int16_t braking_throttle = g.throttle_max * (g.braking_percent * 0.01f) * brake_gain;
+        g2.motors.set_throttle(constrain_int16(-braking_throttle, -g.throttle_max, -g.throttle_min));
+
+        // temporarily set us in reverse to allow the PWM setting to
+        // go negative
+        rover.set_reverse(true);
+    }
+
+    if (rover.mode_guided.guided_mode != ModeGuided::Guided_Velocity) {
+        if (rover.use_pivot_steering()) {
+            // In Guided Velocity, only the steering input is used to calculate the pivot turn.
+            g2.motors.set_throttle(0.0f);
+        }
+    }
+}
+
+void Mode::calc_lateral_acceleration()
+{
+    calc_lateral_acceleration(rover.current_loc, rover.next_WP);
+}
+
+/*
+ * Calculate desired turn angles (in medium freq loop)
+ */
+void Mode::calc_lateral_acceleration(const struct Location &last_WP, const struct Location &next_WP)
+{
+    // Calculate the required turn of the wheels
+    // negative error = left turn
+    // positive error = right turn
+    rover.nav_controller->update_waypoint(last_WP, next_WP);
+    lateral_acceleration = rover.nav_controller->lateral_acceleration();
+    if (rover.use_pivot_steering()) {
+        const int16_t bearing_error = wrap_180_cd(rover.nav_controller->target_bearing_cd() - rover.ahrs.yaw_sensor) / 100;
+        if (bearing_error > 0) {
+            lateral_acceleration = g.turn_max_g * GRAVITY_MSS;
+        } else {
+            lateral_acceleration = -g.turn_max_g * GRAVITY_MSS;
+        }
+    }
+}
+
+/*
+    calculate steering angle given lateral_acceleration
+*/
+void Mode::calc_nav_steer()
+{
+    // add in obstacle avoidance
+    if (!rover.in_reverse) {
+        lateral_acceleration += (rover.obstacle.turn_angle / 45.0f) * g.turn_max_g;
+    }
+
+    // constrain to max G force
+    lateral_acceleration = constrain_float(lateral_acceleration, -g.turn_max_g * GRAVITY_MSS, g.turn_max_g * GRAVITY_MSS);
+
+    // send final steering command to motor library
+    g2.motors.set_steering(rover.steerController.get_steering_out_lat_accel(lateral_acceleration));
+}

APMrover2/mode.h

@@ -0,0 +1,242 @@
+#pragma once
+
+#include <stdint.h>
+#include <GCS_MAVLink/GCS_MAVLink.h>  // for MAV_SEVERITY
+#include "defines.h"
+
+class Mode
+{
+public:
+
+    // Constructor
+    Mode();
+
+    // enter this mode, returns false if we failed to enter
+    bool enter();
+
+    // perform any cleanups required:
+    void exit();
+
+    // returns a unique number specific to this mode
+    virtual uint32_t mode_number() const = 0;
+
+    //
+    // methods that sub classes should override to affect movement of the vehicle in this mode
+    //
+
+    // convert user input to targets, implement high level control for this mode
+    virtual void update() = 0;
+
+    // calculates the amount of throttle that should be output based
+    // on things like proximity to corners and current speed
+    virtual void calc_throttle(float target_speed);
+
+    // called to determine where the vehicle should go next, and how it should get there
+    virtual void update_navigation() { }  // most modes don't navigate
+
+    //
+    // attributes of the mode
+    //
+
+    // return if in non-manual mode : AUTO, GUIDED, RTL
+    virtual bool is_autopilot_mode() const { return false; }
+
+    // returns true if steering is directly controlled by RC
+    virtual bool manual_steering() const { return false; }
+
+    // returns true if the throttle is controlled automatically
+    virtual bool auto_throttle() { return is_autopilot_mode(); }
+
+    // return true if throttle should be supressed in event of a
+    // FAILSAFE_EVENT_THROTTLE
+    virtual bool failsafe_throttle_suppress() const { return true; }
+
+    //
+    // attributes for mavlink system status reporting
+    //
+
+    // returns true if any RC input is used
+    virtual bool has_manual_input() const { return false; }
+
+    // true if heading is controlled
+    virtual bool attitude_stabilized() const { return true; }
+
+    // Navigation control variables
+    // The instantaneous desired lateral acceleration in m/s/s
+    float lateral_acceleration;
+
+protected:
+
+    // subclasses override this to perform checks before entering the mode
+    virtual bool _enter() { return true; }
+
+    // subclasses override this to perform any required cleanup when exiting the mode
+    virtual void _exit() { return; }
+
+    // calculate steering angle given a desired lateral acceleration
+    virtual void calc_nav_steer();
+
+    // calculate desired lateral acceleration using current location and target held in next_WP
+    virtual void calc_lateral_acceleration();
+
+    // calculate desired lateral acceleration
+    void calc_lateral_acceleration(const struct Location &last_wp, const struct Location &next_WP);
+
+    // references to avoid code churn:
+    class Parameters &g;
+    class ParametersG2 &g2;
+    class RC_Channel *&channel_steer;  // TODO : Pointer reference ?
+    class RC_Channel *&channel_throttle;
+    class AP_Mission &mission;
+};
+
+
+class ModeAuto : public Mode
+{
+public:
+
+    uint32_t mode_number() const override { return AUTO; }
+
+    // methods that affect movement of the vehicle in this mode
+    void update() override;
+    void calc_throttle(float target_speed) override;
+    void update_navigation() override;
+
+    // attributes of the mode
+    bool is_autopilot_mode() const override { return true; }
+    bool failsafe_throttle_suppress() const override { return false; }
+
+protected:
+
+    bool _enter() override;
+    void _exit() override;
+    void calc_nav_steer() override;
+    void calc_lateral_acceleration() override;
+
+private:
+
+    bool check_trigger(void);
+
+    // this is set to true when auto has been triggered to start
+    bool auto_triggered;
+};
+
+
+class ModeGuided : public Mode
+{
+public:
+
+    uint32_t mode_number() const override { return GUIDED; }
+
+    // methods that affect movement of the vehicle in this mode
+    void update() override;
+    void update_navigation() override;
+
+    // attributes of the mode
+    bool is_autopilot_mode() const override { return true; }
+    bool failsafe_throttle_suppress() const override { return false; }
+
+    enum GuidedMode {
+        Guided_WP,
+        Guided_Angle,
+        Guided_Velocity
+    };
+
+    // Guided
+    GuidedMode guided_mode;  // stores which GUIDED mode the vehicle is in
+
+protected:
+
+    bool _enter() override;
+};
+
+
+class ModeHold : public Mode
+{
+public:
+
+    uint32_t mode_number() const override { return HOLD; }
+
+    // methods that affect movement of the vehicle in this mode
+    void update() override;
+
+    // attributes for mavlink system status reporting
+    bool attitude_stabilized() const override { return false; }
+};
+
+
+class ModeManual : public Mode
+{
+public:
+
+    uint32_t mode_number() const override { return MANUAL; }
+
+    // methods that affect movement of the vehicle in this mode
+    void update() override;
+
+    // attributes of the mode
+    bool manual_steering() const override { return true; }
+
+    // attributes for mavlink system status reporting
+    bool has_manual_input() const override { return true; }
+    bool attitude_stabilized() const override { return false; }
+};
+
+
+class ModeLearning : public ModeManual
+{
+public:
+
+    uint32_t mode_number() const override { return LEARNING; }
+
+    // attributes for mavlink system status reporting
+    bool has_manual_input() const override { return true; }
+};
+
+
+class ModeRTL : public Mode
+{
+public:
+
+    uint32_t mode_number() const override { return RTL; }
+
+    // methods that affect movement of the vehicle in this mode
+    void update() override;
+    void update_navigation() override;
+
+    // attributes of the mode
+    bool is_autopilot_mode() const override { return true; }
+    bool failsafe_throttle_suppress() const override { return false; }
+
+protected:
+
+    bool _enter() override;
+};
+
+
+class ModeSteering : public Mode
+{
+public:
+
+    uint32_t mode_number() const override { return STEERING; }
+
+    // methods that affect movement of the vehicle in this mode
+    void update() override;
+
+    // attributes for mavlink system status reporting
+    bool has_manual_input() const override { return true; }
+};
+
+class ModeInitializing : public Mode
+{
+public:
+
+    uint32_t mode_number() const override { return INITIALISING; }
+
+    // methods that affect movement of the vehicle in this mode
+    void update() override { }
+
+    // attributes for mavlink system status reporting
+    bool has_manual_input() const override { return true; }
+    bool attitude_stabilized() const override { return false; }
+};

APMrover2/mode_auto.cpp

@@ -0,0 +1,123 @@
+#include "mode.h"
+#include "Rover.h"
+
+bool ModeAuto::_enter()
+{
+    auto_triggered = false;
+    rover.restart_nav();
+    rover.loiter_start_time = 0;
+    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();
+}

APMrover2/mode_guided.cpp

@@ -0,0 +1,82 @@
+#include "mode.h"
+#include "Rover.h"
+
+bool ModeGuided::_enter()
+{
+    /*
+      when entering guided mode we set the target as the current
+      location. This matches the behaviour of the copter code.
+    */
+    lateral_acceleration = 0.0f;
+    rover.set_guided_WP(rover.current_loc);
+    return true;
+}
+
+void ModeGuided::update()
+{
+    if (!rover.in_auto_reverse) {
+        rover.set_reverse(false);
+    }
+
+    switch (guided_mode) {
+        case Guided_Angle:
+            rover.nav_set_yaw_speed();
+            break;
+
+        case Guided_WP:
+            if (rover.rtl_complete || rover.verify_RTL()) {
+                // we have reached destination so stop where we are
+                if (fabsf(g2.motors.get_throttle()) > g.throttle_min.get()) {
+                    rover.gcs().send_mission_item_reached_message(0);
+                }
+                g2.motors.set_throttle(g.throttle_min.get());
+                g2.motors.set_steering(0.0f);
+                lateral_acceleration = 0.0f;
+            } else {
+                calc_lateral_acceleration();
+                calc_nav_steer();
+                calc_throttle(rover.guided_control.target_speed);
+                rover.Log_Write_GuidedTarget(guided_mode, Vector3f(rover.next_WP.lat, rover.next_WP.lng, rover.next_WP.alt),
+                                       Vector3f(rover.guided_control.target_speed, g2.motors.get_throttle(), 0.0f));
+            }
+            break;
+
+        case Guided_Velocity:
+            rover.nav_set_speed();
+            break;
+
+        default:
+            gcs().send_text(MAV_SEVERITY_WARNING, "Unknown GUIDED mode");
+            break;
+    }
+}
+
+void ModeGuided::update_navigation()
+{
+    switch (guided_mode) {
+        case Guided_Angle:
+            rover.nav_set_yaw_speed();
+            break;
+
+        case Guided_WP:
+            // no loitering around the wp with the rover, goes direct to the wp position
+            if (rover.rtl_complete || rover.verify_RTL()) {
+                // we have reached destination so stop where we are
+                g2.motors.set_throttle(g.throttle_min.get());
+                g2.motors.set_steering(0.0f);
+                lateral_acceleration = 0.0f;
+            } else {
+                calc_lateral_acceleration();
+                calc_nav_steer();
+            }
+            break;
+
+        case Guided_Velocity:
+            rover.nav_set_speed();
+            break;
+
+        default:
+            gcs().send_text(MAV_SEVERITY_WARNING, "Unknown GUIDED mode");
+            break;
+    }
+}

APMrover2/mode_hold.cpp

@@ -0,0 +1,12 @@
+#include "mode.h"
+#include "Rover.h"
+
+void ModeHold::update()
+{
+    // hold position - stop motors and center steering
+    g2.motors.set_throttle(0.0f);
+    g2.motors.set_steering(0.0f);
+    if (!rover.in_auto_reverse) {
+        rover.set_reverse(false);
+    }
+}

APMrover2/mode_learning.cpp

@@ -0,0 +1,2 @@
+#include "mode.h"
+#include "Rover.h"

APMrover2/mode_manual.cpp

@@ -0,0 +1,9 @@
+#include "mode.h"
+#include "Rover.h"
+
+void ModeManual::update()
+{
+    // mark us as in_reverse when using a negative throttle to
+    // stop AHRS getting off
+    rover.set_reverse(is_negative(g2.motors.get_throttle()));
+}

APMrover2/mode_rtl.cpp

@@ -0,0 +1,30 @@
+#include "mode.h"
+#include "Rover.h"
+
+bool ModeRTL::_enter()
+{
+    rover.do_RTL();
+    return true;
+}
+
+void ModeRTL::update()
+{
+    if (!rover.in_auto_reverse) {
+        rover.set_reverse(false);
+    }
+    calc_lateral_acceleration();
+    calc_nav_steer();
+    calc_throttle(g.speed_cruise);
+}
+
+void ModeRTL::update_navigation()
+{
+    // no loitering around the wp with the rover, goes direct to the wp position
+    if (rover.verify_RTL()) {
+        g2.motors.set_throttle(g.throttle_min.get());
+        rover.set_mode(rover.mode_hold);
+    } else {
+        calc_lateral_acceleration();
+        calc_nav_steer();
+    }
+}

APMrover2/mode_steering.cpp

@@ -0,0 +1,34 @@
+#include "mode.h"
+#include "Rover.h"
+
+void ModeSteering::update() {
+    /*
+        in steering mode we control lateral acceleration
+        directly. We first calculate the maximum lateral
+        acceleration at full steering lock for this speed. That is
+        V^2/R where R is the radius of turn. We get the radius of
+        turn from half the STEER2SRV_P.
+    */
+    const float ground_speed = rover.ground_speed;
+    float max_g_force = ground_speed * ground_speed / rover.steerController.get_turn_radius();
+
+    // constrain to user set TURN_MAX_G
+    max_g_force = constrain_float(max_g_force, 0.1f, g.turn_max_g * GRAVITY_MSS);
+
+    // convert pilot steering input to desired lateral acceleration
+    lateral_acceleration = max_g_force * (channel_steer->get_control_in() / 4500.0f);
+
+    // run steering controller
+    calc_nav_steer();
+
+    // convert pilot throttle input to desired speed
+    // speed in proportion to cruise speed, up to 50% throttle, then uses nudging above that.
+    float target_speed = channel_throttle->get_control_in() * 0.01f * 2.0f * g.speed_cruise;
+    target_speed = constrain_float(target_speed, -g.speed_cruise, g.speed_cruise);
+
+    // mark us as in_reverse when using a negative throttle to stop AHRS getting off
+    rover.set_reverse(is_negative(target_speed));
+
+    // run speed to throttle output controller
+    calc_throttle(target_speed);
+}