Line data    Source code

       1             : #pragma once
       2             : 
       3             : #include <stdint.h>
       4             : #include <AP_Math/AP_Math.h>
       5             : 
       6             : class Mode {
       7             : public:
       8             :     enum class Number {
       9             :         MANUAL=0,
      10             :         STOP=1,
      11             :         SCAN=2,
      12             :         SERVOTEST=3,
      13             :         GUIDED=4,
      14             :         AUTO=10,
      15             :         INITIALISING=16
      16             :         // Mode number 30 reserved for "offboard" for external/lua control.
      17             :     };
      18             : 
      19         231 :     Mode() {}
      20             : 
      21             :     // do not allow copying
      22             :     CLASS_NO_COPY(Mode);
      23             : 
      24             :     // returns a unique number specific to this mode
      25             :     virtual Mode::Number number() const = 0;
      26             :     virtual const char* name() const = 0;
      27             : 
      28             :     virtual bool requires_armed_servos() const = 0;
      29             : 
      30             :     virtual void update() = 0;
      31             : 
      32             : protected:
      33             :     void update_scan();
      34             :     void update_auto();
      35             : 
      36             :     bool get_ef_yaw_direction();
      37             : 
      38             :     void calc_angle_error(float pitch, float yaw, bool direction_reversed);
      39             :     void convert_ef_to_bf(float pitch, float yaw, float& bf_pitch, float& bf_yaw);
      40             :     bool convert_bf_to_ef(float pitch, float yaw, float& ef_pitch, float& ef_yaw);
      41             : };
      42             : 
      43             : class ModeAuto : public Mode {
      44             : public:
      45        5879 :     Mode::Number number() const override { return Mode::Number::AUTO; }
      46           0 :     const char* name() const override { return "Auto"; }
      47          36 :     bool requires_armed_servos() const override { return true; }
      48             :     void update() override;
      49             : 
      50        4991 :     void set_target(float target_yaw_deg, float target_pitch_deg) {
      51        4991 :         _target_yaw_deg = target_yaw_deg;
      52        4991 :         _target_pitch_deg = target_pitch_deg;
      53        4991 :     }
      54           0 :     float get_auto_target_yaw_deg() const { return _target_yaw_deg; }
      55           0 :     float get_auto_target_pitch_deg() const { return _target_pitch_deg; }
      56             : 
      57             : private:
      58             :     float _target_yaw_deg;
      59             :     float _target_pitch_deg;
      60             : };
      61             : 
      62             : class ModeGuided : public Mode {
      63             : public:
      64         181 :     Mode::Number number() const override { return Mode::Number::GUIDED; }
      65           0 :     const char* name() const override { return "Guided"; }
      66           1 :     bool requires_armed_servos() const override { return true; }
      67             :     void update() override;
      68             : 
      69          40 :     void set_angle(const Quaternion &target_att,
      70             :                    bool use_yaw_rate, float yaw_rate_rads,
      71             :                    bool use_pitch_rate, float pitch_rate_rads) {
      72          40 :         _target_att = target_att;
      73          40 :         _use_yaw_rate = use_yaw_rate;
      74          40 :         _yaw_rate_rads = yaw_rate_rads;
      75          40 :         _use_pitch_rate = use_pitch_rate;
      76          40 :         _pitch_rate_rads = pitch_rate_rads;
      77          40 :     }
      78           0 :     Quaternion get_attitude_target_quat() { return _target_att; }
      79           0 :     bool get_attitude_target_use_yaw_rate() const { return _use_yaw_rate; }
      80           0 :     float get_attitude_target_yaw_rate_rads() const { return _yaw_rate_rads; }
      81           0 :     bool get_attitude_target_use_pitch_rate() const { return _use_pitch_rate; }
      82           0 :     float get_attitude_target_pitch_rate_rads() const { return _pitch_rate_rads; }
      83             : 
      84             : private:
      85             :     Quaternion _target_att;
      86             :     bool _use_yaw_rate;
      87             :     float _yaw_rate_rads;
      88             :     bool _use_pitch_rate;
      89             :     float _pitch_rate_rads;
      90             : };
      91             : 
      92             : class ModeInitialising : public Mode {
      93             : public:
      94         512 :     Mode::Number number() const override { return Mode::Number::INITIALISING; }
      95           0 :     const char* name() const override { return "Initialising"; }
      96           0 :     bool requires_armed_servos() const override { return false; }
      97           0 :     void update() override {};
      98             : };
      99             : 
     100             : class ModeManual : public Mode {
     101             : public:
     102          80 :     Mode::Number number() const override { return Mode::Number::MANUAL; }
     103           0 :     const char* name() const override { return "Manual"; }
     104           4 :     bool requires_armed_servos() const override { return true; }
     105             :     void update() override;
     106             : };
     107             : 
     108             : class ModeScan : public Mode {
     109             : public:
     110          95 :     Mode::Number number() const override { return Mode::Number::SCAN; }
     111           0 :     const char* name() const override { return "Scan"; }
     112           1 :     bool requires_armed_servos() const override { return true; }
     113             :     void update() override;
     114             : };
     115             : 
     116             : class ModeServoTest : public Mode {
     117             : public:
     118          63 :     Mode::Number number() const override { return Mode::Number::SERVOTEST; }
     119           0 :     const char* name() const override { return "ServoTest"; }
     120           1 :     bool requires_armed_servos() const override { return true; }
     121        1500 :     void update() override {};
     122             : 
     123             :     bool set_servo(uint8_t servo_num, uint16_t pwm);
     124             : };
     125             : 
     126             : class ModeStop : public Mode {
     127             : public:
     128         142 :     Mode::Number number() const override { return Mode::Number::STOP; }
     129           0 :     const char* name() const override { return "Stop"; }
     130           1 :     bool requires_armed_servos() const override { return false; }
     131           0 :     void update() override {};
     132             : };