land_control.cpp

/**
TODO
-Handle user input:
    -during approach(RTL) and during descent
    -abort landing to loiter or althold
-Allow for a pause before starting descent
-slow down on final descent
-dont allow for a mode switch out of NO_GPS land if there is no gps
**/



//initialize a land at current position or zero horizontal vel
void init_land(uint8_t typ){
    Vector3f target;
    init_land(typ,target)

}

//initialize a land with a target position or velocity
void init_land(uint8_t typ, Vector3f target){
    switch(typ){
        case LAND_POS:
            init_land_pos(target);
        break;
        case LAND_VEL:
            init_land_vel(target);
        break;
        case LAND_NO_GPS:
            Vector3f no_target;
            init_land_vel(no_target);
        break;
    }
}

//initialize a position land
void init_land_pos(Vector3f target){
    land_pos_initd = true;
    land_vel_initd = false;
    //initialize loiter contoller and descent velocity
}

//initialize a velocity land
void init_land_vel(Vector3f target){
    land_vel_initd = true;
    land_pos_initd = false;
    //initialize velocity contoller and descent velocity
}

//set target position for loiter controller
bool set_land_pos(Vector3f target){
    //check if we are in position mode
    if(!land_pos_initd){
        return false;
    }
    //set position
    return true;
}

//set velocity vector for velocity controller
bool set_land_vel(Vector3f target){
    //check if we are in velocity mode
    if(!land_vel_initd){
        return false;
    }
    //set velocity
    return true;
}

//change landing mode
void set_mode(uint8_t typ){
    init_land(typ);
}

//set flag to state if we want to disarm upon landing
void set_disarm_on_land(bool disarm){
    //set flag
    disarm_on_land = disarm;
}

//set a delay before we start descending
void set_delay_descent(uint8_t ms){
    //set delay
}

//run landing routine
void land_run(){

    //finished land
    if(complete){return;}

    check_user_input();

    if(land_pos_initd){
        land_run_pos();
    }
    else if(land_vel_initd){
        land_run_vel();
    }

    complete = run_land_detector();

    //finished land
    if(complete && disarm_on_land){
        disarm();
    }
}

//run position land sub routine
void land_run_pos(){
    //run loiter controller
}

//run velocity land sub routine
void land_run_vel(){
    //run velocity controller
}

void check_user_input(){
    //get user input


    if(roll != 0 || pitch != 0){
        land_repo = true;
        //handle land reposition
    }
    if(throttle > mid_throttle){
        abort_land = true;
        //handle abort land
    }
    if(yaw != 0){
        //handle change heading
    }
}

//state machine to see if we have landed
void run_land_detector(){
    //check state of landing
}

//return true is land is complete
bool is_land_complete(){
    //return if land is complete
}