navigation.h

/*******************************************************************************
 * Copyright (c) 2009-2014, MAV'RIC Development Team
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without 
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice, 
 * this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice, 
 * this list of conditions and the following disclaimer in the documentation 
 * and/or other materials provided with the distribution.
 * 
 * 3. Neither the name of the copyright holder nor the names of its contributors
 * may be used to endorse or promote products derived from this software without
 * specific prior written permission.
 * 
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 
 * POSSIBILITY OF SUCH DAMAGE.
 ******************************************************************************/

/*******************************************************************************
 * \file navigation.h
 * 
 * \author MAV'RIC Team
 * \author Nicolas Dousse
 *   
 * \brief Waypoint navigation controller
 *
 ******************************************************************************/


#ifndef NAVIGATION_H_
#define NAVIGATION_H_

#ifdef __cplusplus
extern "C" {
#endif

#include "mavlink_waypoint_handler.h"
#include "stabilisation.h"
#include "quaternions.h"
#include "mavlink_waypoint_handler.h"
#include "position_estimation.h"
#include "tasks.h"
#include "mavlink_communication.h"
#include "state.h"
#include "remote.h"
#include "pid_controller.h"
#include <stdbool.h>


typedef struct
{
    float dist2vel_gain;                                
    pid_controller_t hovering_controller;               
    pid_controller_t wpt_nav_controller;                
    float cruise_speed;                                 
    float max_climb_rate;                               
    float soft_zone_size;                               
    
    float dt;                                           
    uint32_t last_update;                               
    
    float alt_lpf;                                      
    float LPF_gain;                                     
    
    uint8_t loop_count;                                 
    
    mav_mode_t mode;                                    
    
    bool auto_takeoff;                                  
    bool auto_landing;                                  
    
    critical_behavior_enum critical_behavior;           
    auto_landing_behavior_t auto_landing_behavior;      
    
    bool critical_next_state;                           
    
    bool auto_landing_next_state;                       
    
    bool stop_nav;                                      
    bool stop_nav_there;                                
    
    control_command_t *controls_nav;                    
    const control_command_t* control_joystick;          
    const quat_t *qe;                                   
    mavlink_waypoint_handler_t *waypoint_handler;       
    const position_estimation_t *position_estimation;       
    state_t* state;                                     
    const mavlink_stream_t* mavlink_stream;             
    remote_t* remote;                                   
}navigation_t;

typedef struct 
{
    float dist2vel_gain;                                
    float cruise_speed;                                 
    float max_climb_rate;                               
    
    float soft_zone_size;                               
    
    float alt_lpf;                                      
    float LPF_gain;                                     
    
    pid_controller_t hovering_controller;               
    pid_controller_t wpt_nav_controller;                
}navigation_config_t;

bool navigation_init(navigation_t* navigation, navigation_config_t* nav_config, control_command_t* controls_nav, const quat_t* qe, mavlink_waypoint_handler_t* waypoint_handler, const position_estimation_t* position_estimation, state_t* state, const control_command_t* control_joystick, remote_t* remote, mavlink_communication_t* mavlink_communication);

void navigation_waypoint_hold_init(mavlink_waypoint_handler_t* waypoint_handler, local_coordinates_t local_pos);

task_return_t navigation_update(navigation_t* navigation);

#ifdef __cplusplus
}
#endif

#endif // NAVIGATION_H_