mission.h

Go to the documentation of this file.

/****************************************************************************
 *
 *   Copyright (c) 2013-2018 PX4 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 PX4 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 OWNER 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.
 *
 ****************************************************************************/
#pragma once

#include "mission_block.h"
#include "mission_feasibility_checker.h"
#include "navigator_mode.h"

#include <float.h>

#include <dataman/dataman.h>
#include <drivers/drv_hrt.h>
#include <px4_module_params.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/mission_result.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_roi.h>
#include <uORB/uORB.h>

class Navigator;

class Mission : public MissionBlock, public ModuleParams
{
public:
    Mission(Navigator *navigator);
    ~Mission() override = default;

    void on_inactive() override;
    void on_inactivation() override;
    void on_activation() override;
    void on_active() override;

    enum mission_altitude_mode {
        MISSION_ALTMODE_ZOH = 0,
        MISSION_ALTMODE_FOH = 1
    };

    bool set_current_mission_index(uint16_t index);

    bool land_start();
    bool landing();

    uint16_t get_land_start_index() const { return _land_start_index; }
    bool get_land_start_available() const { return _land_start_available; }
    bool get_mission_finished() const { return _mission_type == MISSION_TYPE_NONE; }
    bool get_mission_changed() const { return _mission_changed ; }
    bool get_mission_waypoints_changed() const { return _mission_waypoints_changed ; }

    void set_closest_item_as_current();

    void set_execution_mode(const uint8_t mode);
private:

    void update_mission();

    void advance_mission();

    void set_mission_items();

    bool do_need_vertical_takeoff();

    bool do_need_move_to_land();

    bool do_need_move_to_takeoff();

    void copy_position_if_valid(struct mission_item_s *mission_item, struct position_setpoint_s *setpoint);

    void set_align_mission_item(struct mission_item_s *mission_item, struct mission_item_s *mission_item_next);

    float calculate_takeoff_altitude(struct mission_item_s *mission_item);

    void heading_sp_update();

    void altitude_sp_foh_update();

    void cruising_speed_sp_update();

    void do_abort_landing();

    bool prepare_mission_items(mission_item_s *mission_item,
                   mission_item_s *next_position_mission_item, bool *has_next_position_item);

    bool read_mission_item(int offset, struct mission_item_s *mission_item);

    void save_mission_state();

    void report_do_jump_mission_changed(int index, int do_jumps_remaining);

    void set_mission_item_reached();

    void set_current_mission_item();

    void check_mission_valid(bool force);

    void reset_mission(struct mission_s &mission);

    bool need_to_reset_mission(bool active);

    void generate_waypoint_from_heading(struct position_setpoint_s *setpoint, float yaw);

    bool find_mission_land_start();

    int32_t index_closest_mission_item() const;

    bool position_setpoint_equal(const position_setpoint_s *p1, const position_setpoint_s *p2) const;

    DEFINE_PARAMETERS(
        (ParamFloat<px4::params::MIS_DIST_1WP>) _param_mis_dist_1wp,
        (ParamFloat<px4::params::MIS_DIST_WPS>) _param_mis_dist_wps,
        (ParamInt<px4::params::MIS_ALTMODE>) _param_mis_altmode,
        (ParamInt<px4::params::MIS_MNT_YAW_CTL>) _param_mis_mnt_yaw_ctl
    )

    struct mission_s _mission {};

    int32_t _current_mission_index{-1};

    // track location of planned mission landing
    bool    _land_start_available{false};
    uint16_t _land_start_index{UINT16_MAX};     
    bool _need_takeoff{true};                   
    enum {
        MISSION_TYPE_NONE,
        MISSION_TYPE_MISSION
    } _mission_type{MISSION_TYPE_NONE};

    bool _inited{false};
    bool _home_inited{false};
    bool _need_mission_reset{false};
    bool _mission_waypoints_changed{false};
    bool _mission_changed{false}; 
    float _min_current_sp_distance_xy{FLT_MAX}; 
    float _distance_current_previous{0.0f}; 
    enum work_item_type {
        WORK_ITEM_TYPE_DEFAULT,     
        WORK_ITEM_TYPE_TAKEOFF,     
        WORK_ITEM_TYPE_MOVE_TO_LAND,    
        WORK_ITEM_TYPE_ALIGN,       
        WORK_ITEM_TYPE_CMD_BEFORE_MOVE,
        WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF,
        WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION,
        WORK_ITEM_TYPE_PRECISION_LAND
    } _work_item_type{WORK_ITEM_TYPE_DEFAULT};  
    uint8_t _mission_execution_mode{mission_result_s::MISSION_EXECUTION_MODE_NORMAL};   
    bool _execution_mode_changed{false};
};