APMotors_6DOF: Implement motor_is_enabled(), get_motor_angular_factors() and set_reversed();

libraries/AP_Motors/AP_Motors6DOF.cpp

@@ -548,3 +548,29 @@ void AP_Motors6DOF::output_armed_stabilizing_vectored_6dof()
         }
     }
 }
+
+Vector3f AP_Motors6DOF::get_motor_angular_factors(int motor_number) {
+     if (motor_number < 0 || motor_number >= AP_MOTORS_MAX_NUM_MOTORS) {
+        return Vector3f(0,0,0);
+    }
+    return Vector3f(_roll_factor[motor_number], _pitch_factor[motor_number], _yaw_factor[motor_number]);
+}
+
+bool AP_Motors6DOF::motor_is_enabled(int motor_number) {
+    if (motor_number < 0 || motor_number >= AP_MOTORS_MAX_NUM_MOTORS) {
+        return false;
+    }
+    return motor_enabled[motor_number];
+}
+
+bool AP_Motors6DOF::set_reversed(int motor_number, bool reversed) {
+    if (motor_number < 0 || motor_number >= AP_MOTORS_MAX_NUM_MOTORS) {
+        return false;
+    }
+    if (reversed) {
+        _motor_reverse[motor_number].set_and_save(-1);
+    } else {
+        _motor_reverse[motor_number].set_and_save(1);
+    }
+    return true;
+}

libraries/AP_Motors/AP_Motors6DOF.h

@@ -44,6 +44,14 @@ public:
     // This allows us to read back the output of the altidude controllers
     // The controllers are in charge of the throttle input, so this gives vehicle access/visibility to the output of those controllers
     float get_throttle_in_bidirectional() const { return constrain_float(2*(_throttle_in - 0.5f), -1.0f, 1.0f); }
+    
+    // returns a vector with roll, pitch, and yaw contributions
+    Vector3f get_motor_angular_factors(int motor_number);
+
+    // returns true if motor is enabled
+    bool motor_is_enabled(int motor_number);
+
+    bool set_reversed(int motor_number, bool reversed);
 
     // var_info for holding Parameter information
     static const struct AP_Param::GroupInfo        var_info[];