Line data    Source code

       1             : /*
       2             :    Lead developers: Matthew Ridley and Andrew Tridgell
       3             : 
       4             :    Please contribute your ideas! See https://ardupilot.club/dev for details
       5             : 
       6             :    This program is free software: you can redistribute it and/or modify
       7             :    it under the terms of the GNU General Public License as published by
       8             :    the Free Software Foundation, either version 3 of the License, or
       9             :    (at your option) any later version.
      10             : 
      11             :    This program is distributed in the hope that it will be useful,
      12             :    but WITHOUT ANY WARRANTY; without even the implied warranty of
      13             :    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
      14             :    GNU General Public License for more details.
      15             : 
      16             :    You should have received a copy of the GNU General Public License
      17             :    along with this program.  If not, see <http://www.gnu.org/licenses/>.
      18             :  */
      19             : #pragma once
      20             : 
      21             : ////////////////////////////////////////////////////////////////////////////////
      22             : // Header includes
      23             : ////////////////////////////////////////////////////////////////////////////////
      24             : 
      25             : #include <cmath>
      26             : #include <stdarg.h>
      27             : #include <stdio.h>
      28             : 
      29             : #include <AP_Common/AP_Common.h>
      30             : #include <AP_Param/AP_Param.h>
      31             : #include <AP_Math/AP_Math.h>        // ArduPilot Mega Vector/Matrix math Library
      32             : #include <AP_AHRS/AP_AHRS.h>         // ArduPilot Mega DCM Library
      33             : #include <Filter/Filter.h>                     // Filter library
      34             : 
      35             : #include <AP_Logger/AP_Logger.h>
      36             : #include <AP_Scheduler/AP_Scheduler.h>       // main loop scheduler
      37             : #include <AP_NavEKF2/AP_NavEKF2.h>
      38             : #include <AP_NavEKF3/AP_NavEKF3.h>
      39             : 
      40             : #include <SRV_Channel/SRV_Channel.h>
      41             : #include <AP_Vehicle/AP_Vehicle.h>
      42             : #include <AP_Mission/AP_Mission.h>
      43             : #include <AP_BattMonitor/AP_BattMonitor.h> // Battery monitor library
      44             : 
      45             : // Configuration
      46             : #include "config.h"
      47             : #include "defines.h"
      48             : 
      49             : #include "RC_Channel_Tracker.h"
      50             : #include "Parameters.h"
      51             : #include "GCS_MAVLink_Tracker.h"
      52             : #include "GCS_Tracker.h"
      53             : 
      54             : #include "AP_Arming_Tracker.h"
      55             : 
      56             : #include "mode.h"
      57             : 
      58             : class Tracker : public AP_Vehicle {
      59             : public:
      60             :     friend class GCS_MAVLINK_Tracker;
      61             :     friend class GCS_Tracker;
      62             :     friend class Parameters;
      63             :     friend class ModeAuto;
      64             :     friend class ModeGuided;
      65             :     friend class Mode;
      66             : 
      67             :     void arm_servos();
      68             :     void disarm_servos();
      69             : 
      70             : private:
      71             :     Parameters g;
      72             : 
      73             :     uint32_t start_time_ms = 0;
      74             : 
      75             :     /**
      76             :        antenna control channels
      77             :     */
      78             :     RC_Channels_Tracker rc_channels;
      79             :     SRV_Channels servo_channels;
      80             : 
      81             :     LowPassFilterFloat yaw_servo_out_filt;
      82             :     LowPassFilterFloat pitch_servo_out_filt;
      83             : 
      84             :     bool yaw_servo_out_filt_init = false;
      85             :     bool pitch_servo_out_filt_init = false;
      86             : 
      87             :     GCS_Tracker _gcs; // avoid using this; use gcs()
      88         110 :     GCS_Tracker &gcs() { return _gcs; }
      89             : 
      90             :     // Battery Sensors
      91             :     AP_BattMonitor battery{MASK_LOG_CURRENT,
      92             :                            FUNCTOR_BIND_MEMBER(&Tracker::handle_battery_failsafe, void, const char*, const int8_t),
      93             :                            nullptr};
      94             :     Location current_loc;
      95             : 
      96             :     Mode *mode_from_mode_num(enum Mode::Number num);
      97             : 
      98             :     Mode *mode = &mode_initialising;
      99             : 
     100             :     ModeAuto mode_auto;
     101             :     ModeInitialising mode_initialising;
     102             :     ModeManual mode_manual;
     103             :     ModeGuided mode_guided;
     104             :     ModeScan mode_scan;
     105             :     ModeServoTest mode_servotest;
     106             :     ModeStop mode_stop;
     107             : 
     108             :     // Vehicle state
     109             :     struct {
     110             :         bool location_valid;    // true if we have a valid location for the vehicle
     111             :         Location location;      // lat, long in degrees * 10^7; alt in meters * 100
     112             :         Location location_estimate; // lat, long in degrees * 10^7; alt in meters * 100
     113             :         uint32_t last_update_us;    // last position update in microseconds
     114             :         uint32_t last_update_ms;    // last position update in milliseconds
     115             :         Vector3f vel;           // the vehicle's velocity in m/s
     116             :         int32_t relative_alt;   // the vehicle's relative altitude in meters * 100
     117             :     } vehicle;
     118             : 
     119             :     // Navigation controller state
     120             :     struct NavStatus {
     121             :         float bearing;                  // bearing to vehicle in centi-degrees
     122             :         float distance;                 // distance to vehicle in meters
     123             :         float pitch;                    // pitch to vehicle in degrees (positive means vehicle is above tracker, negative means below)
     124             :         float angle_error_pitch;        // angle error between target and current pitch in centi-degrees
     125             :         float angle_error_yaw;          // angle error between target and current yaw in centi-degrees
     126             :         float alt_difference_baro;      // altitude difference between tracker and vehicle in meters according to the barometer.  positive value means vehicle is above tracker
     127             :         float alt_difference_gps;       // altitude difference between tracker and vehicle in meters according to the gps.  positive value means vehicle is above tracker
     128             :         float altitude_offset;          // offset in meters which is added to tracker altitude to align altitude measurements with vehicle's barometer
     129             :         bool manual_control_yaw         : 1;// true if tracker yaw is under manual control
     130             :         bool manual_control_pitch       : 1;// true if tracker pitch is manually controlled
     131             :         bool need_altitude_calibration  : 1;// true if tracker altitude has not been determined (true after startup)
     132             :         bool scan_reverse_pitch         : 1;// controls direction of pitch movement in SCAN mode
     133             :         bool scan_reverse_yaw           : 1;// controls direction of yaw movement in SCAN mode
     134             :     } nav_status;
     135             : 
     136             :     // setup the var_info table
     137             :     AP_Param param_loader{var_info};
     138             : 
     139             :     bool target_set = false;
     140             :     bool stationary = true; // are we using the start lat and log?
     141             : 
     142             :     static const AP_Scheduler::Task scheduler_tasks[];
     143             :     static const AP_Param::Info var_info[];
     144             :     static const struct LogStructure log_structure[];
     145             : 
     146             :     // Tracker.cpp
     147             :     void get_scheduler_tasks(const AP_Scheduler::Task *&tasks,
     148             :                              uint8_t &task_count,
     149             :                              uint32_t &log_bit) override;
     150             :     void one_second_loop();
     151             :     void ten_hz_logging_loop();
     152             :     void stats_update();
     153             : 
     154             :     // GCS_Mavlink.cpp
     155             :     void send_nav_controller_output(mavlink_channel_t chan);
     156             : 
     157             : #if HAL_LOGGING_ENABLED
     158             :     // methods for AP_Vehicle:
     159          33 :     const AP_Int32 &get_log_bitmask() override { return g.log_bitmask; }
     160          33 :     const struct LogStructure *get_log_structures() const override {
     161          33 :         return log_structure;
     162             :     }
     163             :     uint8_t get_num_log_structures() const override;
     164             : 
     165             :     // Log.cpp
     166             :     void Log_Write_Attitude();
     167             :     void Log_Write_Vehicle_Baro(float pressure, float altitude);
     168             :     void Log_Write_Vehicle_Pos(int32_t lat,int32_t lng,int32_t alt, const Vector3f& vel);
     169             :     void Log_Write_Vehicle_Startup_Messages();
     170             : #endif
     171             : 
     172             :     // Parameters.cpp
     173             :     void load_parameters(void) override;
     174             : 
     175             :     // radio.cpp
     176             :     void read_radio();
     177             : 
     178             :     // sensors.cpp
     179             :     void update_ahrs();
     180             :     void update_compass(void);
     181             :     void update_GPS(void);
     182             :     void handle_battery_failsafe(const char* type_str, const int8_t action);
     183             : 
     184             :     // servos.cpp
     185             :     void init_servos();
     186             :     void update_pitch_servo(float pitch);
     187             :     void update_pitch_position_servo(void);
     188             :     void update_pitch_onoff_servo(float pitch) const;
     189             :     void update_pitch_cr_servo(float pitch);
     190             :     void update_yaw_servo(float yaw);
     191             :     void update_yaw_position_servo(void);
     192             :     void update_yaw_onoff_servo(float yaw) const;
     193             :     void update_yaw_cr_servo(float yaw);
     194             : 
     195             :     // system.cpp
     196             :     void init_ardupilot() override;
     197             :     bool get_home_eeprom(Location &loc) const;
     198             :     bool set_home_eeprom(const Location &temp) WARN_IF_UNUSED;
     199             :     bool set_home_to_current_location(bool lock) override WARN_IF_UNUSED;
     200             :     bool set_home(const Location &temp, bool lock) override WARN_IF_UNUSED;
     201             :     void prepare_servos();
     202             :     void set_mode(Mode &newmode, ModeReason reason);
     203             :     bool set_mode(uint8_t new_mode, ModeReason reason) override;
     204           0 :     uint8_t get_mode() const override { return (uint8_t)mode->number(); }
     205             :     bool should_log(uint32_t mask);
     206           0 :     bool start_command_callback(const AP_Mission::Mission_Command& cmd) { return false; }
     207           0 :     void exit_mission_callback() { return; }
     208           0 :     bool verify_command_callback(const AP_Mission::Mission_Command& cmd) { return false; }
     209             : 
     210             :     // tracking.cpp
     211             :     void update_vehicle_pos_estimate();
     212             :     void update_tracker_position();
     213             :     void update_bearing_and_distance();
     214             :     void update_tracking(void);
     215             :     void tracking_update_position(const mavlink_global_position_int_t &msg);
     216             :     void tracking_update_pressure(const mavlink_scaled_pressure_t &msg);
     217             :     void tracking_manual_control(const mavlink_manual_control_t &msg);
     218             :     void update_armed_disarmed() const;
     219             :     bool get_pan_tilt_norm(float &pan_norm, float &tilt_norm) const override;
     220             : 
     221             :     // Arming/Disarming management class
     222             :     AP_Arming_Tracker arming;
     223             : 
     224             :     // Mission library
     225             :     AP_Mission mission{
     226             :             FUNCTOR_BIND_MEMBER(&Tracker::start_command_callback, bool, const AP_Mission::Mission_Command &),
     227             :             FUNCTOR_BIND_MEMBER(&Tracker::verify_command_callback, bool, const AP_Mission::Mission_Command &),
     228             :             FUNCTOR_BIND_MEMBER(&Tracker::exit_mission_callback, void)};
     229             : };
     230             : 
     231             : extern Tracker tracker;