Line data    Source code

       1             : #pragma once
       2             : #if AP_SCRIPTING_ENABLED
       3             : 
       4             : #include <AP_Common/AP_Common.h>
       5             : #include <AP_Math/AP_Math.h>
       6             : #include <RC_Channel/RC_Channel.h>
       7             : #include "AP_MotorsMatrix.h"
       8             : 
       9             : class AP_MotorsMatrix_6DoF_Scripting : public AP_MotorsMatrix {
      10             : public:
      11             : 
      12             :     /// Constructor
      13           1 :     AP_MotorsMatrix_6DoF_Scripting(uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) :
      14           1 :         AP_MotorsMatrix(speed_hz)
      15             :     {
      16           1 :         if (_singleton != nullptr) {
      17           0 :             AP_HAL::panic("AP_MotorsMatrix 6DoF must be singleton");
      18             :         }
      19           1 :         _singleton = this;
      20           1 :     };
      21             : 
      22             :     // get singleton instance
      23           5 :     static AP_MotorsMatrix_6DoF_Scripting *get_singleton() {
      24           5 :         return _singleton;
      25             :     }
      26             : 
      27             :     // output_to_motors - sends minimum values out to the motors
      28             :     void output_to_motors() override;
      29             : 
      30             :     // sets the roll and pitch offset, this rotates the thrust vector in body frame
      31             :     // these are typically set such that the throttle thrust vector is earth frame up
      32             :     void set_roll_pitch(float roll_deg, float pitch_deg) override;
      33             : 
      34             :     // add_motor using raw roll, pitch, throttle and yaw factors, to be called from scripting
      35             :     void add_motor(int8_t motor_num, float roll_factor, float pitch_factor, float yaw_factor, float throttle_factor, float forward_factor, float right_factor, bool reversible, uint8_t testing_order);
      36             : 
      37             :     // if the expected number of motors have been setup then set as initalized
      38             :     bool init(uint8_t expected_num_motors) override;
      39             : 
      40             : protected:
      41             :     // output - sends commands to the motors
      42             :     void output_armed_stabilizing() override;
      43             : 
      44             :     // nothing to do for setup, scripting will mark as initalized when done
      45           2 :     void setup_motors(motor_frame_class frame_class, motor_frame_type frame_type) override {};
      46             : 
      47           0 :     const char* _get_frame_string() const override { return "6DoF scripting"; }
      48             : 
      49             :     float _forward_factor[AP_MOTORS_MAX_NUM_MOTORS];      // each motors contribution to forward thrust
      50             :     float _right_factor[AP_MOTORS_MAX_NUM_MOTORS];        // each motors contribution to right thrust
      51             : 
      52             :     // true if motor is revesible, it can go from -Spin max to +Spin max, if false motor is can go from Spin min to Spin max
      53             :     bool _reversible[AP_MOTORS_MAX_NUM_MOTORS];
      54             : 
      55             :     // store last values to allow deadzone
      56             :     float _last_thrust_out[AP_MOTORS_MAX_NUM_MOTORS];
      57             : 
      58             :     // Current offset angles, radians
      59             :     float _roll_offset;
      60             :     float _pitch_offset;
      61             : 
      62             : private:
      63             :     static AP_MotorsMatrix_6DoF_Scripting *_singleton;
      64             : 
      65             : };
      66             : 
      67             : #endif // AP_SCRIPTING_ENABLED