navigator.h

Go to the documentation of this file.

/***************************************************************************
 *
 *   Copyright (c) 2013-2017 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 "datalinkloss.h"
#include "enginefailure.h"
#include "follow_target.h"
#include "geofence.h"
#include "gpsfailure.h"
#include "land.h"
#include "precland.h"
#include "loiter.h"
#include "mission.h"
#include "navigator_mode.h"
#include "rcloss.h"
#include "rtl.h"
#include "takeoff.h"

#include "navigation.h"

#include <lib/perf/perf_counter.h>
#include <px4_module.h>
#include <px4_module_params.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/geofence_result.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/mission_result.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/position_controller_status.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/uORB.h>

#define NAVIGATOR_MODE_ARRAY_SIZE 11


class Navigator : public ModuleBase<Navigator>, public ModuleParams
{
public:
    Navigator();
    ~Navigator() override;

    Navigator(const Navigator &) = delete;
    Navigator operator=(const Navigator &) = delete;

    static int task_spawn(int argc, char *argv[]);

    static Navigator *instantiate(int argc, char *argv[]);

    static int custom_command(int argc, char *argv[]);

    static int print_usage(const char *reason = nullptr);

    void run() override;

    int print_status() override;

    void        load_fence_from_file(const char *filename);

    void        publish_geofence_result();

    void        publish_vehicle_cmd(vehicle_command_s *vcmd);

    void        fake_traffic(const char *callsign, float distance, float direction, float traffic_heading, float altitude_diff,
                     float hor_velocity, float ver_velocity);

    void        check_traffic();

    void        set_can_loiter_at_sp(bool can_loiter) { _can_loiter_at_sp = can_loiter; }
    void        set_position_setpoint_triplet_updated() { _pos_sp_triplet_updated = true; }
    void        set_mission_result_updated() { _mission_result_updated = true; }

    struct home_position_s *get_home_position() { return &_home_pos; }
    struct mission_result_s *get_mission_result() { return &_mission_result; }
    struct position_setpoint_triplet_s *get_position_setpoint_triplet() { return &_pos_sp_triplet; }
    struct position_setpoint_triplet_s *get_reposition_triplet() { return &_reposition_triplet; }
    struct position_setpoint_triplet_s *get_takeoff_triplet() { return &_takeoff_triplet; }
    struct vehicle_global_position_s *get_global_position() { return &_global_pos; }
    struct vehicle_land_detected_s *get_land_detected() { return &_land_detected; }
    struct vehicle_local_position_s *get_local_position() { return &_local_pos; }
    struct vehicle_status_s *get_vstatus() { return &_vstatus; }
    PrecLand *get_precland() { return &_precland; } 
    const vehicle_roi_s &get_vroi() { return _vroi; }
    void reset_vroi() { _vroi = {}; }

    bool home_alt_valid() { return (_home_pos.timestamp > 0 && _home_pos.valid_alt); }
    bool home_position_valid() { return (_home_pos.timestamp > 0 && _home_pos.valid_alt && _home_pos.valid_hpos); }

    int     get_mission_sub() { return _mission_sub; }

    Geofence    &get_geofence() { return _geofence; }

    bool        get_can_loiter_at_sp() { return _can_loiter_at_sp; }
    float       get_loiter_radius() { return _param_nav_loiter_rad.get(); }

    float       get_default_acceptance_radius();

    float       get_acceptance_radius();

    float       get_default_altitude_acceptance_radius();

    float       get_altitude_acceptance_radius();

    float       get_cruising_speed();

    void        set_cruising_speed(float speed = -1.0f);

    void        reset_cruising_speed();


    void        reset_triplets();

    float       get_cruising_throttle();

    void        set_cruising_throttle(float throttle = -1.0f) { _mission_throttle = throttle; }

    float       get_acceptance_radius(float mission_item_radius);

    float       get_yaw_acceptance(float mission_item_yaw);

    orb_advert_t    *get_mavlink_log_pub() { return &_mavlink_log_pub; }

    void        increment_mission_instance_count() { _mission_result.instance_count++; }

    void        set_mission_failure(const char *reason);

    // MISSION
    bool        is_planned_mission() const { return _navigation_mode == &_mission; }
    bool        on_mission_landing() { return _mission.landing(); }
    bool        start_mission_landing() { return _mission.land_start(); }
    bool        mission_start_land_available() { return _mission.get_land_start_available(); }

    // RTL
    bool        mission_landing_required() { return _rtl.rtl_type() == RTL::RTL_LAND; }
    int         rtl_type() { return _rtl.rtl_type(); }
    bool        in_rtl_state() const { return _vstatus.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; }

    bool        abort_landing();

    // Param access
    float       get_loiter_min_alt() const { return _param_mis_ltrmin_alt.get(); }
    float       get_takeoff_min_alt() const { return _param_mis_takeoff_alt.get(); }
    bool        get_takeoff_required() const { return _param_mis_takeoff_req.get(); }
    float       get_yaw_timeout() const { return _param_mis_yaw_tmt.get(); }
    float       get_yaw_threshold() const { return _param_mis_yaw_err.get(); }

    float       get_vtol_back_trans_deceleration() const { return _param_back_trans_dec_mss; }
    float       get_vtol_reverse_delay() const { return _param_reverse_delay; }

    bool        force_vtol();

private:
    DEFINE_PARAMETERS(
        (ParamFloat<px4::params::NAV_LOITER_RAD>) _param_nav_loiter_rad,    
        (ParamFloat<px4::params::NAV_ACC_RAD>) _param_nav_acc_rad,  
        (ParamFloat<px4::params::NAV_FW_ALT_RAD>)
        _param_nav_fw_alt_rad,  
        (ParamFloat<px4::params::NAV_FW_ALTL_RAD>)
        _param_nav_fw_altl_rad, 
        (ParamFloat<px4::params::NAV_MC_ALT_RAD>)
        _param_nav_mc_alt_rad,  
        (ParamInt<px4::params::NAV_FORCE_VT>) _param_nav_force_vt,  
        (ParamInt<px4::params::NAV_TRAFF_AVOID>) _param_nav_traff_avoid,    
        // non-navigator parameters
        // Mission (MIS_*)
        (ParamFloat<px4::params::MIS_LTRMIN_ALT>) _param_mis_ltrmin_alt,
        (ParamFloat<px4::params::MIS_TAKEOFF_ALT>) _param_mis_takeoff_alt,
        (ParamBool<px4::params::MIS_TAKEOFF_REQ>) _param_mis_takeoff_req,
        (ParamFloat<px4::params::MIS_YAW_TMT>) _param_mis_yaw_tmt,
        (ParamFloat<px4::params::MIS_YAW_ERR>) _param_mis_yaw_err
    )

    int     _global_pos_sub{-1};        
    int     _gps_pos_sub{-1};       
    int     _home_pos_sub{-1};      
    int     _land_detected_sub{-1};     
    int     _local_pos_sub{-1};     
    int     _mission_sub{-1};       
    int     _param_update_sub{-1};      
    int     _pos_ctrl_landing_status_sub{-1};   
    int     _traffic_sub{-1};       
    int     _vehicle_command_sub{-1};   
    int     _vstatus_sub{-1};       
    orb_advert_t    _geofence_result_pub{nullptr};
    orb_advert_t    _mavlink_log_pub{nullptr};  
    orb_advert_t    _mission_result_pub{nullptr};
    orb_advert_t    _pos_sp_triplet_pub{nullptr};
    orb_advert_t    _vehicle_cmd_ack_pub{nullptr};
    orb_advert_t    _vehicle_cmd_pub{nullptr};
    orb_advert_t    _vehicle_roi_pub{nullptr};

    // Subscriptions
    home_position_s                 _home_pos{};        
    mission_result_s                _mission_result{};
    vehicle_global_position_s           _global_pos{};      
    vehicle_gps_position_s              _gps_pos{};     
    vehicle_land_detected_s             _land_detected{};   
    vehicle_local_position_s            _local_pos{};       
    vehicle_status_s                _vstatus{};     
    uORB::Subscription<position_controller_status_s>    _position_controller_status_sub{ORB_ID(position_controller_status)};

    uint8_t                     _previous_nav_state{}; 
    // Publications
    geofence_result_s               _geofence_result{};
    position_setpoint_triplet_s         _pos_sp_triplet{};  
    position_setpoint_triplet_s         _reposition_triplet{};  
    position_setpoint_triplet_s         _takeoff_triplet{}; 
    vehicle_roi_s                   _vroi{};        
    perf_counter_t  _loop_perf;         
    Geofence    _geofence;          
    bool        _geofence_violation_warning_sent{false}; 
    bool        _can_loiter_at_sp{false};           
    bool        _pos_sp_triplet_updated{false};     
    bool        _pos_sp_triplet_published_invalid_once{false};  
    bool        _mission_result_updated{false};     
    NavigatorMode   *_navigation_mode{nullptr};     
    Mission     _mission;           
    Loiter      _loiter;            
    Takeoff     _takeoff;           
    Land        _land;          
    PrecLand    _precland;          
    RTL         _rtl;               
    RCLoss      _rcLoss;                
    DataLinkLoss    _dataLinkLoss;          
    EngineFailure   _engineFailure;         
    GpsFailure  _gpsFailure;            
    FollowTarget    _follow_target;

    NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE];   
    param_t _handle_back_trans_dec_mss{PARAM_INVALID};
    param_t _handle_reverse_delay{PARAM_INVALID};
    float _param_back_trans_dec_mss{0.f};
    float _param_reverse_delay{0.f};

    float _mission_cruising_speed_mc{-1.0f};
    float _mission_cruising_speed_fw{-1.0f};
    float _mission_throttle{-1.0f};

    // update subscriptions
    void        global_position_update();
    void        gps_position_update();
    void        home_position_update(bool force = false);
    void        local_position_update();
    void        params_update();
    void        vehicle_land_detected_update();
    void        vehicle_status_update();

    void        publish_position_setpoint_triplet();

    void        publish_mission_result();

    void        publish_vehicle_command_ack(const vehicle_command_s &cmd, uint8_t result);
};