AntennaTracker/Tracker.h

   1 /*
   2    Lead developers: Matthew Ridley and Andrew Tridgell
   3
   4    Please contribute your ideas! See https://ardupilot.org/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.h"
  50 #include "Parameters.h"
  51 #include "GCS_Mavlink.h"
  52 #include "GCS_Tracker.h"
  53
  54 #include "AP_Arming.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     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     const AP_Int32 &get_log_bitmask() override { return g.log_bitmask; }
 160     const struct LogStructure *get_log_structures() const override {
 161         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 compass_save();
 181     void update_compass(void);
 182     void update_GPS(void);
 183     void handle_battery_failsafe(const char* type_str, const int8_t action);
 184
 185     // servos.cpp
 186     void init_servos();
 187     void update_pitch_servo(float pitch);
 188     void update_pitch_position_servo(void);
 189     void update_pitch_onoff_servo(float pitch) const;
 190     void update_pitch_cr_servo(float pitch);
 191     void update_yaw_servo(float yaw);
 192     void update_yaw_position_servo(void);
 193     void update_yaw_onoff_servo(float yaw) const;
 194     void update_yaw_cr_servo(float yaw);
 195
 196     // system.cpp
 197     void init_ardupilot() override;
 198     bool get_home_eeprom(Location &loc) const;
 199     bool set_home_eeprom(const Location &temp) WARN_IF_UNUSED;
 200     bool set_home_to_current_location(bool lock) override WARN_IF_UNUSED;
 201     bool set_home(const Location &temp, bool lock) override WARN_IF_UNUSED;
 202     void prepare_servos();
 203     void set_mode(Mode &newmode, ModeReason reason);
 204     bool set_mode(uint8_t new_mode, ModeReason reason) override;
 205     uint8_t get_mode() const override { return (uint8_t)mode->number(); }
 206     bool should_log(uint32_t mask);
 207     bool start_command_callback(const AP_Mission::Mission_Command& cmd) { return false; }
 208     void exit_mission_callback() { return; }
 209     bool verify_command_callback(const AP_Mission::Mission_Command& cmd) { return false; }
 210
 211     // tracking.cpp
 212     void update_vehicle_pos_estimate();
 213     void update_tracker_position();
 214     void update_bearing_and_distance();
 215     void update_tracking(void);
 216     void tracking_update_position(const mavlink_global_position_int_t &msg);
 217     void tracking_update_pressure(const mavlink_scaled_pressure_t &msg);
 218     void tracking_manual_control(const mavlink_manual_control_t &msg);
 219     void update_armed_disarmed() const;
 220     bool get_pan_tilt_norm(float &pan_norm, float &tilt_norm) const override;
 221
 222     // Arming/Disarming management class
 223     AP_Arming_Tracker arming;
 224
 225     // Mission library
 226     AP_Mission mission{
 227             FUNCTOR_BIND_MEMBER(&Tracker::start_command_callback, bool, const AP_Mission::Mission_Command &),
 228             FUNCTOR_BIND_MEMBER(&Tracker::verify_command_callback, bool, const AP_Mission::Mission_Command &),
 229             FUNCTOR_BIND_MEMBER(&Tracker::exit_mission_callback, void)};
 230 };
 231
 232 extern Tracker tracker;