Line data    Source code

       1             : /// @file       AP_Motors6DOF.h
       2             : /// @brief      Motor control class for ROVs with direct control over 6DOF (or fewer) in movement
       3             : 
       4             : #pragma once
       5             : 
       6             : #include <AP_Common/AP_Common.h>
       7             : #include <AP_Math/AP_Math.h>        // ArduPilot Mega Vector/Matrix math Library
       8             : #include <RC_Channel/RC_Channel.h>     // RC Channel Library
       9             : #include "AP_MotorsMatrix.h"
      10             : 
      11             : /// @class      AP_MotorsMatrix
      12             : class AP_Motors6DOF : public AP_MotorsMatrix {
      13             : public:
      14             : 
      15          83 :     AP_Motors6DOF(uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) :
      16        2739 :         AP_MotorsMatrix(speed_hz) {
      17          83 :         AP_Param::setup_object_defaults(this, var_info);
      18          83 :     };
      19             : 
      20             :     // Supported frame types
      21             :     typedef enum {
      22             :         SUB_FRAME_BLUEROV1,
      23             :         SUB_FRAME_VECTORED,
      24             :         SUB_FRAME_VECTORED_6DOF,
      25             :         SUB_FRAME_VECTORED_6DOF_90DEG,
      26             :         SUB_FRAME_SIMPLEROV_3,
      27             :         SUB_FRAME_SIMPLEROV_4,
      28             :         SUB_FRAME_SIMPLEROV_5,
      29             :         SUB_FRAME_CUSTOM
      30             :     } sub_frame_t;
      31             : 
      32             :     // Override parent
      33             :     void setup_motors(motor_frame_class frame_class, motor_frame_type frame_type) override;
      34             : 
      35             :     // Override parent
      36             :     void output_min() override;
      37             : 
      38             :     // Map thrust input -1~1 to pwm output 1100~1900
      39             :     int16_t calc_thrust_to_pwm(float thrust_in) const;
      40             : 
      41             :     // output_to_motors - sends minimum values out to the motors
      42             :     void output_to_motors() override;
      43             : 
      44      268558 :     void set_max_throttle(float max_throttle) { _max_throttle = max_throttle; }
      45             : 
      46             :     // returns a vector with roll, pitch, and yaw contributions
      47             :     Vector3f get_motor_angular_factors(int motor_number);
      48             : 
      49             :     // returns true if motor is enabled
      50             :     bool motor_is_enabled(int motor_number);
      51             : 
      52             :     bool set_reversed(int motor_number, bool reversed);
      53             : 
      54             :     // var_info for holding Parameter information
      55             :     static const struct AP_Param::GroupInfo        var_info[];
      56             : 
      57             : protected:
      58             :     // return current_limit as a number from 0 ~ 1 in the range throttle_min to throttle_max
      59             :     float               get_current_limit_max_throttle() override;
      60             : 
      61             :     //Override MotorsMatrix method
      62             :     void add_motor_raw_6dof(int8_t motor_num, float roll_fac, float pitch_fac, float yaw_fac, float climb_fac, float forward_fac, float lat_fac, uint8_t testing_order);
      63             : 
      64             :     void output_armed_stabilizing() override;
      65             :     void output_armed_stabilizing_vectored();
      66             :     void output_armed_stabilizing_vectored_6dof();
      67             : 
      68             :     // Parameters
      69             :     AP_Int8             _motor_reverse[AP_MOTORS_MAX_NUM_MOTORS];
      70             :     AP_Float            _forwardVerticalCouplingFactor;
      71             : 
      72             :     float               _forward_factor[AP_MOTORS_MAX_NUM_MOTORS]; // each motors contribution to forward/backward
      73             :     float               _lateral_factor[AP_MOTORS_MAX_NUM_MOTORS];  // each motors contribution to lateral (left/right)
      74             : 
      75             :     float _max_throttle = 1.0f;
      76             :     // current limiting
      77             :     float _output_limited = 1.0f;
      78             :     float _batt_current_last = 0.0f;
      79             : };