vtol_follower_fsm.c

Go to the documentation of this file.

/*
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
 * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
 * for more details.
 *
 * You should have received a copy of the GNU General Public License along
 * with this program; if not, see <http://www.gnu.org/licenses/>
 */

#include "openpilot.h"

#include <pios_thread.h>

#include "coordinate_conversions.h"
#include "physical_constants.h"

#include "vtol_follower_priv.h"

#include "pathdesired.h"
#include "positionactual.h"
#include "vtolpathfollowersettings.h"
#include "vtolpathfollowerstatus.h"

// Various navigation constants
const static float RTH_ALT_ERROR    = 0.8f;  
const static float RTH_CLIMB_SPEED  = 1.0f;  

enum vtol_fsm_event {
        FSM_EVENT_AUTO,           
        FSM_EVENT_TIMEOUT,        
        FSM_EVENT_HIT_TARGET,     
        FSM_EVENT_LEFT_TARGET,    
        FSM_EVENT_NUM_EVENTS
};

enum vtol_fsm_state {
        FSM_STATE_FAULT,           
        FSM_STATE_INIT,            
        FSM_STATE_HOLDING,         
        FSM_STATE_FLYING_PATH,     
        FSM_STATE_LANDING,         
        FSM_STATE_PRE_RTH_RISE,    
        FSM_STATE_POST_RTH_HOLD,   
        FSM_STATE_DISARM,          
        FSM_STATE_UNCHANGED,       
        FSM_STATE_NUM_STATES
};

struct vtol_fsm_transition {
        void (*entry_fn)();       
        int32_t (*static_fn)();   
        uint32_t timeout;         
        enum vtol_fsm_state next_state[FSM_EVENT_NUM_EVENTS];
};

#define MILLI 1000

// State transition methods, typically enabling for certain actions
static void go_enable_hold_here(void);
static void go_enable_fly_path(void);
static void go_enable_rise_here(void);
static void go_enable_pause_home_10s(void);
static void go_enable_fly_home(void);
static void go_enable_land_home(void);

// Methods that actually achieve the desired nav mode
static int32_t do_hold(void);
static int32_t do_path(void);
static int32_t do_requested_path(void);
static int32_t do_land(void);
static int32_t do_loiter(void);
static int32_t do_ph_climb(void);

// Utility functions
static void vtol_fsm_timer_set(int32_t ms);
static bool vtol_fsm_timer_expired();
static void hold_position(float north, float east, float down);

const static struct vtol_fsm_transition fsm_hold_position[FSM_STATE_NUM_STATES] = {
        [FSM_STATE_INIT] = {
                .next_state = {
                        [FSM_EVENT_AUTO] = FSM_STATE_HOLDING,
                },
        },
        [FSM_STATE_HOLDING] = {
                .entry_fn = go_enable_hold_here,
                .static_fn = do_loiter,
                .next_state = {
                        [FSM_EVENT_HIT_TARGET] = FSM_STATE_UNCHANGED,
                        [FSM_EVENT_LEFT_TARGET] = FSM_STATE_UNCHANGED,
                },
        },
};

const static struct vtol_fsm_transition fsm_follow_path[FSM_STATE_NUM_STATES] = {
        [FSM_STATE_INIT] = {
                .next_state = {
                        [FSM_EVENT_AUTO] = FSM_STATE_FLYING_PATH,
                },
        },
        [FSM_STATE_FLYING_PATH] = {
                .entry_fn = go_enable_fly_path,
                .static_fn = do_requested_path,
                .next_state = {
                        [FSM_EVENT_HIT_TARGET] = FSM_STATE_UNCHANGED,
                        [FSM_EVENT_LEFT_TARGET] = FSM_STATE_UNCHANGED,
                },
        },
};

const static struct vtol_fsm_transition fsm_land_home[FSM_STATE_NUM_STATES] = {
        [FSM_STATE_INIT] = {
                .next_state = {
                        [FSM_EVENT_AUTO] = FSM_STATE_PRE_RTH_RISE,
                },
        },
        [FSM_STATE_PRE_RTH_RISE] = {
                .entry_fn = go_enable_rise_here,
                .static_fn = do_ph_climb,
                .timeout = 8 * MILLI,   // Spend at least 8s in this state
                .next_state = {
                        [FSM_EVENT_TIMEOUT] = FSM_STATE_UNCHANGED,
                        [FSM_EVENT_HIT_TARGET] = FSM_STATE_FLYING_PATH,
                        [FSM_EVENT_LEFT_TARGET] = FSM_STATE_UNCHANGED,
                },
        },
        [FSM_STATE_FLYING_PATH] = {
                .entry_fn = go_enable_fly_home,
                .static_fn = do_path,
                .next_state = {
                        [FSM_EVENT_HIT_TARGET] = FSM_STATE_POST_RTH_HOLD,
                },
        },
        [FSM_STATE_POST_RTH_HOLD] = {
                .entry_fn = go_enable_pause_home_10s,
                .static_fn = do_hold,
                .timeout = 10 * MILLI,
                .next_state = {
                        [FSM_EVENT_TIMEOUT] = FSM_STATE_LANDING,
                        [FSM_EVENT_HIT_TARGET] = FSM_STATE_UNCHANGED,
                        [FSM_EVENT_LEFT_TARGET] = FSM_STATE_UNCHANGED,
                },
        },
        [FSM_STATE_LANDING] = {
                .entry_fn = go_enable_land_home,
                .static_fn = do_land,
                .next_state = {
                        [FSM_EVENT_HIT_TARGET] = FSM_STATE_DISARM,
                },
        },

};

static uint32_t vtol_fsm_timer_expiration = 0;

static void vtol_fsm_timer_set(int32_t ms)
{
        if (ms) {
                uint32_t now = PIOS_Thread_Systime();
                vtol_fsm_timer_expiration = ms+now;

                // If we wrap and get very unlucky, make sure we still
                // have a timer 1ms later.
                if (vtol_fsm_timer_expiration == 0) {
                        vtol_fsm_timer_expiration++;
                }
        } else {
                vtol_fsm_timer_expiration = 0;
        }
}

static bool vtol_fsm_timer_expired() {
        /* If there's a timer... */
        if (vtol_fsm_timer_expiration > 0) {
                /* See if it expires. */

                uint32_t now = PIOS_Thread_Systime();
                uint32_t interval = vtol_fsm_timer_expiration - now;

                /* If it has expired, this will wrap around and be a big
                 * number.  Use a windowed scheme to detect this:
                 * Assume we will run at least every 0x10000000 us
                 * (3 days) and timeouts can't exceed 0xf0000000 us (46 days)
                 */
                if (interval > 0xf0000000) {
                        return true;
                }
        }

        return false;
}


const static struct vtol_fsm_transition *current_goal;
static enum vtol_fsm_state curr_state;

static int vtol_fsm_enter_state(enum vtol_fsm_state state)
{
        if (state != FSM_STATE_UNCHANGED) {
                curr_state = state;

                /* Call the entry function (if any) for the next state. */
                if (current_goal[curr_state].entry_fn) {
                        current_goal[curr_state].entry_fn();
                }

                /* 0 disables any pending timeout, otherwise it's set to the
                 * value for this state */
                vtol_fsm_timer_set(current_goal[curr_state].timeout);

                return 1;
        }

        return 0;
}

static bool vtol_fsm_process_event(enum vtol_fsm_event event)
{
        enum vtol_fsm_state next = current_goal[curr_state].next_state[event];

        /* Special if condition to not have to explicitly define auto
         * (don't do fault transitions on auto) */
        if ((event != FSM_EVENT_AUTO) || (next != FSM_STATE_FAULT)) {
                return vtol_fsm_enter_state(next);
        }

        return false;
}

static void vtol_fsm_process_auto()
{
        while (vtol_fsm_process_event(FSM_EVENT_AUTO));
}

static void vtol_fsm_fsm_init(const struct vtol_fsm_transition *goal)
{
        current_goal = goal;

        vtol_fsm_enter_state(FSM_STATE_INIT);

        /* Process any AUTO transitions in the FSM */
        vtol_fsm_process_auto();
}

static void vtol_fsm_inject_event(enum vtol_fsm_event event)
{
        vtol_fsm_process_event(event);

        /* Process any AUTO transitions in the FSM */
        vtol_fsm_process_auto();
}

static int32_t vtol_fsm_static()
{
        VtolPathFollowerStatusFSM_StateSet((uint8_t *) &curr_state);

        // If the current state has a static function, call it
        if (current_goal[curr_state].static_fn) {
                current_goal[curr_state].static_fn();
        }

        if (vtol_fsm_timer_expired()) {
                vtol_fsm_inject_event(FSM_EVENT_TIMEOUT);
        }

        return 0;
}


static float vtol_hold_position_ned[3];

static int32_t do_hold()
{
        if (vtol_follower_control_endpoint(vtol_hold_position_ned) == 0) {
                if (vtol_follower_control_attitude(vtol_dT, NULL) == 0) {
                        return 0;
                }
        }

        return -1;
}

static PathDesiredData vtol_fsm_path_desired = {
        .Waypoint = 8000        // Unlikely to clash with pathplanner
};

static int32_t do_path()
{
        struct path_status progress;
        if (vtol_follower_control_path(&vtol_fsm_path_desired, &progress) == 0) {
                if (vtol_follower_control_attitude(vtol_dT, NULL) == 0) {

                        if (progress.fractional_progress >= 1.0f) {
                                vtol_fsm_inject_event(FSM_EVENT_HIT_TARGET);
                        }

                        return 0;
                }
        }

        return -1;
}

static int32_t do_requested_path()
{
        // Fetch the path desired from the path planner
        PathDesiredGet(&vtol_fsm_path_desired);

        switch(vtol_fsm_path_desired.Mode) {
        case PATHDESIRED_MODE_LAND:
                for (uint8_t i = 0; i < 3; i++)
                        vtol_hold_position_ned[i] = vtol_fsm_path_desired.End[i];
                return do_land();
        case PATHDESIRED_MODE_HOLDPOSITION:
        case PATHDESIRED_MODE_ENDPOINT:
                for (uint8_t i = 0; i < 3; i++)
                        vtol_hold_position_ned[i] = vtol_fsm_path_desired.End[i];
                return do_hold();
        default:
                return do_path();
        }
}

static int32_t do_land()
{
        bool landed;
        if (vtol_follower_control_land(vtol_hold_position_ned, &landed) == 0) {
                if (vtol_follower_control_attitude(vtol_dT, NULL) == 0) {
                        return 0;
                }
        }

        return 0;
}

static int32_t do_loiter()
{
        float att_adj[2] = { 0, 0 };
        float hold_pos[3] = {
                vtol_hold_position_ned[0],
                vtol_hold_position_ned[1],
                vtol_hold_position_ned[2]
        };

        float alt_adj = 0;

        if (vtol_follower_control_loiter(vtol_dT, hold_pos, att_adj, &alt_adj)) {
                // If hold position changed, use it!
                // We follow this conditional just to avoid unnecessarily
                // spamming updates to the PositionDesired object.

                hold_position(hold_pos[0], hold_pos[1], hold_pos[2]);
        }

        if (vtol_follower_control_altrate(vtol_hold_position_ned,
                                alt_adj) == 0) {
                if (vtol_follower_control_attitude(vtol_dT, att_adj) == 0) {
                        return 0;
                }
        }

        return -1;
}

static int32_t do_slow_altitude_change(float descent_rate)
{
        if (vtol_follower_control_altrate(vtol_hold_position_ned,
                                descent_rate) == 0) {
                if (vtol_follower_control_attitude(vtol_dT, NULL) == 0) {
                        return 0;
                }
        }

        return -1;
}

static int32_t do_ph_climb()
{
        float cur_down;
        PositionActualDownGet(&cur_down);

        int32_t ret_val;

        const float err = fabsf(cur_down - vtol_hold_position_ned[2]);

        if (err < RTH_ALT_ERROR) {
                // If we're close to desired altitude, use poshold logic
                // until timer expires.
                ret_val = do_hold();

                if (vtol_fsm_timer_expired()) {
                        vtol_fsm_inject_event(FSM_EVENT_HIT_TARGET);
                }
        } else {
                // If we are low, control for altitude change speed.
                // If we are high, use the normal control loop.
                if (cur_down > vtol_hold_position_ned[2]) {
                        ret_val = do_slow_altitude_change(-RTH_CLIMB_SPEED);
                } else {
                        ret_val = do_hold();
                }
        }

        return ret_val;
}


static void hold_position(float north, float east, float down)
{
        vtol_hold_position_ned[0] = north;
        vtol_hold_position_ned[1] = east;
        vtol_hold_position_ned[2] = down;

        /* Store the data in the UAVO */
        vtol_fsm_path_desired.Start[0] = north;
        vtol_fsm_path_desired.Start[1] = east;
        vtol_fsm_path_desired.Start[2] = down;
        vtol_fsm_path_desired.End[0] = north;
        vtol_fsm_path_desired.End[1] = east;
        vtol_fsm_path_desired.End[2] = down;
        vtol_fsm_path_desired.StartingVelocity = 0;
        vtol_fsm_path_desired.EndingVelocity   = 0;
        vtol_fsm_path_desired.Mode = PATHDESIRED_MODE_ENDPOINT;
        vtol_fsm_path_desired.ModeParameters = 0;

        vtol_fsm_path_desired.Waypoint++;

        PathDesiredSet(&vtol_fsm_path_desired);
}

static void go_enable_hold_here()
{
        PositionActualData positionActual;
        PositionActualGet(&positionActual);

        hold_position(positionActual.North, positionActual.East, positionActual.Down);
}

static void go_enable_fly_path()
{
}

static void go_enable_rise_here()
{
        PositionActualData positionActual;
        PositionActualGet(&positionActual);

        float down = positionActual.Down;

        float min_rise, rth_alt;

        VtolPathFollowerSettingsReturnToHomeMinRiseGet(&min_rise);
        VtolPathFollowerSettingsReturnToHomeAltitudeGet(&rth_alt);

        // Set the target altitude for MIN_RISE above our current alt
        down -= min_rise;

        // But also make sure we return at a minimum of 15 m above home
        if (down > -rth_alt)
                down = -rth_alt;

        hold_position(positionActual.North, positionActual.East, down);
}

static void go_enable_pause_home_10s()
{
        float down = vtol_hold_position_ned[2];

        float rth_alt;
        VtolPathFollowerSettingsReturnToHomeAltitudeGet(&rth_alt);

        if (down > -rth_alt)
                down = -rth_alt;

        hold_position(0, 0, down);
}

static void go_enable_fly_home()
{
        PositionActualData positionActual;
        PositionActualGet(&positionActual);

        // Set start position at current position
        vtol_fsm_path_desired.Start[0] = positionActual.North;
        vtol_fsm_path_desired.Start[1] = positionActual.East;
        vtol_fsm_path_desired.Start[2] = positionActual.Down;

        // Set end position above home
        vtol_fsm_path_desired.End[0] = 0;
        vtol_fsm_path_desired.End[1] = 0;
        vtol_fsm_path_desired.End[2] = positionActual.Down;

        float rth_alt;
        VtolPathFollowerSettingsReturnToHomeAltitudeGet(&rth_alt);

        if (positionActual.Down > -rth_alt) {
                vtol_fsm_path_desired.Start[2] = -rth_alt;
                vtol_fsm_path_desired.End[2] = -rth_alt;
        }

        /* Paranoia: set to 3.0f just in case calls below fail.  TODO: change
         * UAVO semantics to be guaranteed to either succeed or crash */
        float rth_vel = 3.0f;
        VtolPathFollowerSettingsReturnToHomeVelGet(&rth_vel);

        vtol_fsm_path_desired.StartingVelocity = rth_vel;
        vtol_fsm_path_desired.EndingVelocity = rth_vel;

        vtol_fsm_path_desired.Mode = PATHDESIRED_MODE_VECTOR;
        vtol_fsm_path_desired.ModeParameters = 0;

        /* It's necessary that this increment so that we don't end up
         * latching completion status. Wraparound, etc, is OK.
         * This is for compatibility with the waypoint handshaking in the
         * path planner.
         */
        vtol_fsm_path_desired.Waypoint++;

        PathDesiredSet(&vtol_fsm_path_desired);
}

static void go_enable_land_home()
{
        vtol_hold_position_ned[0] = 0;
        vtol_hold_position_ned[1] = 0;
        vtol_hold_position_ned[2] = 0; // Has no effect
}


// Public API methods
int32_t vtol_follower_fsm_activate_goal(enum vtol_goals new_goal)
{
        switch(new_goal) {
        case GOAL_LAND_HOME:
                vtol_fsm_fsm_init(fsm_land_home);
                return 0;
        case GOAL_HOLD_POSITION:
                vtol_fsm_fsm_init(fsm_hold_position);
                return 0;
        case GOAL_FLY_PATH:
                vtol_fsm_fsm_init(fsm_follow_path);
                return 0;
        default:
                return -1;
        }
}

int32_t vtol_follower_fsm_update()
{
        vtol_fsm_static();
        return 0;
}