AntennaTracker/mode.h

   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     };
  17
  18     Mode() {}
  19
  20     // do not allow copying
  21     CLASS_NO_COPY(Mode);
  22
  23     // returns a unique number specific to this mode
  24     virtual Mode::Number number() const = 0;
  25
  26     virtual bool requires_armed_servos() const = 0;
  27
  28     virtual void update() = 0;
  29
  30 protected:
  31     void update_scan();
  32     void update_auto();
  33
  34     bool get_ef_yaw_direction();
  35
  36     void calc_angle_error(float pitch, float yaw, bool direction_reversed);
  37     void convert_ef_to_bf(float pitch, float yaw, float& bf_pitch, float& bf_yaw);
  38     bool convert_bf_to_ef(float pitch, float yaw, float& ef_pitch, float& ef_yaw);
  39 };
  40
  41 class ModeAuto : public Mode {
  42 public:
  43     Mode::Number number() const override { return Mode::Number::AUTO; }
  44     bool requires_armed_servos() const override { return true; }
  45     void update() override;
  46 };
  47
  48 class ModeGuided : public Mode {
  49 public:
  50     Mode::Number number() const override { return Mode::Number::GUIDED; }
  51     bool requires_armed_servos() const override { return true; }
  52     void update() override;
  53
  54     void set_angle(const Quaternion &target_att, bool use_yaw_rate, float yaw_rate_rads) {
  55         _target_att = target_att;
  56         _use_yaw_rate = use_yaw_rate;
  57         _yaw_rate_rads = yaw_rate_rads;
  58     }
  59
  60 private:
  61     Quaternion _target_att;
  62     bool _use_yaw_rate;
  63     float _yaw_rate_rads;
  64 };
  65
  66 class ModeInitialising : public Mode {
  67 public:
  68     Mode::Number number() const override { return Mode::Number::INITIALISING; }
  69     bool requires_armed_servos() const override { return false; }
  70     void update() override {};
  71 };
  72
  73 class ModeManual : public Mode {
  74 public:
  75     Mode::Number number() const override { return Mode::Number::MANUAL; }
  76     bool requires_armed_servos() const override { return true; }
  77     void update() override;
  78 };
  79
  80 class ModeScan : public Mode {
  81 public:
  82     Mode::Number number() const override { return Mode::Number::SCAN; }
  83     bool requires_armed_servos() const override { return true; }
  84     void update() override;
  85 };
  86
  87 class ModeServoTest : public Mode {
  88 public:
  89     Mode::Number number() const override { return Mode::Number::SERVOTEST; }
  90     bool requires_armed_servos() const override { return true; }
  91     void update() override {};
  92
  93     bool set_servo(uint8_t servo_num, uint16_t pwm);
  94 };
  95
  96 class ModeStop : public Mode {
  97 public:
  98     Mode::Number number() const override { return Mode::Number::STOP; }
  99     bool requires_armed_servos() const override { return false; }
 100     void update() override {};
 101 };