mavlink_waypoint_handler.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 mavlink_waypoint_handler.h
 * 
 * \author MAV'RIC Team
 * \author Nicolas Dousse
 *  
 * \brief The MAVLink waypoint handler
 *
 ******************************************************************************/


#ifndef MAVLINK_WAYPOINT_HANDLER__
#define MAVLINK_WAYPOINT_HANDLER__

#ifdef __cplusplus
extern "C" {
#endif

#include "mavlink_stream.h"
#include "stdbool.h"
#include "position_estimation.h"
#include "imu.h"
#include "mavlink_message_handler.h"
#include "mavlink_communication.h"
#include "state.h"
#include "qfilter.h"

#define MAX_WAYPOINTS 10        

/*
 * N.B.: Reference Frames and MAV_CMD_NAV are defined in "maveric.h"
 */

typedef struct
{
    uint8_t frame;                                              
    uint16_t command;                                           
    uint8_t current;                                            
    uint8_t autocontinue;                                       
    float param1;                                               
    float param2;                                               
    float param3;                                               
    float param4;                                               
    double x;                                                   
    double y;                                                   
    double z;                                                   
} waypoint_struct_t;

typedef struct
{
    waypoint_struct_t waypoint_list[MAX_WAYPOINTS];             
    waypoint_struct_t current_waypoint;                         
    uint16_t number_of_waypoints;                               
    int8_t current_waypoint_count;                              
    
    local_coordinates_t waypoint_coordinates;                   
    local_coordinates_t waypoint_hold_coordinates;              
    local_coordinates_t waypoint_critical_coordinates;          
    float dist2wp_sqr;                                          
    
    bool hold_waypoint_set;                                     

    bool waypoint_sending;                                      
    bool waypoint_receiving;                                    
    
    int32_t sending_waypoint_num;                               
    int32_t waypoint_request_number;                            

    uint16_t num_waypoint_onboard;                              

    uint32_t start_timeout;                                     
    uint32_t timeout_max_waypoint;                              

    position_estimation_t* position_estimation;                 
    const ahrs_t* ahrs;                                         
    state_t* state;                                             
    mavlink_communication_t* mavlink_communication;             
    const mavlink_stream_t* mavlink_stream;                     

} mavlink_waypoint_handler_t;

void waypoint_handler_init_homing_waypoint(mavlink_waypoint_handler_t* waypoint_handler);

void waypoint_handler_init_waypoint_list(mavlink_waypoint_handler_t* waypoint_handler);

bool waypoint_handler_init( mavlink_waypoint_handler_t* waypoint_handler, 
                            position_estimation_t* position_estimation, 
                            const ahrs_t* ahrs, state_t* state, 
                            mavlink_communication_t* mavlink_communication, 
                            const mavlink_stream_t* mavlink_stream);

void waypoint_handler_nav_plan_init(mavlink_waypoint_handler_t* waypoint_handler);

task_return_t waypoint_handler_control_time_out_waypoint_msg(mavlink_waypoint_handler_t* waypoint_handler);

local_coordinates_t waypoint_handler_set_waypoint_from_frame(waypoint_struct_t* current_waypoint, global_position_t origin);

#ifdef __cplusplus
}
#endif

#endif // MAVLINK_WAYPOINT_HANDLER__