Diff: FlightController/MotorMixer/MotorMixer.cpp

Revision:

0:c6a85bb2a827

Child:

2:969dfa4f2436

diff -r 000000000000 -r c6a85bb2a827 FlightController/MotorMixer/MotorMixer.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FlightController/MotorMixer/MotorMixer.cpp	Wed Mar 04 18:50:37 2015 +0000
@@ -0,0 +1,84 @@
+#include "MotorMixer.h"
+
+MotorMixer::MotorMixer(){}
+
+MotorMixer::~MotorMixer(){}
+
+bool MotorMixer::initialise(PinName motor1, PinName motor2, PinName motor3, PinName motor4)
+{
+    _motor1 = new PwmOut(motor1);
+    _motor2 = new PwmOut(motor2);
+    _motor3 = new PwmOut(motor3);
+    _motor4 = new PwmOut(motor4);
+    
+    //Set frequency
+    float period = 1.0 / FLIGHT_CONTROLLER_FREQUENCY;
+    _motor1->period(period);
+    _motor2->period(period);
+    _motor3->period(period);
+    _motor4->period(period);
+    
+    //Disable
+    _motor1 = MOTORS_OFF;
+    _motor2 = MOTORS_OFF;
+    _motor2 = MOTORS_OFF;
+    _motor2 = MOTORS_OFF;
+    
+    DEBUG("Motor power initialised");
+    return true; 
+}
+
+void MotorMixer::computePower(PidWrapper::PidOutputs pidOutputs, float throttle)
+{
+    //Calculate base power to apply from throttle - returns 1060 at min, 1860 at max
+    float basePower = MOTORS_MIN + (throttle * 800);
+    
+    //Map motor power - each PID returns -100 <-> 100
+    _motorPower.motor1 = basePower + pidOutputs.pitch + pidOutputs.roll + pidOutputs.yaw;
+    _motorPower.motor2 = basePower + pidOutputs.pitch - pidOutputs.roll - pidOutputs.yaw;
+    _motorPower.motor3 = basePower - pidOutputs.pitch - pidOutputs.roll + pidOutputs.yaw;
+    _motorPower.motor4 = basePower - pidOutputs.pitch + pidOutputs.roll - pidOutputs.yaw;
+    
+    //Specify intial motor power limits
+    float motorFix = 0;
+    float motorMin = _motorPower.motor1;
+    float motorMax = _motorPower.motor1;
+    
+    //Check motor power is within limits - if not add/remove constant to all motors to keep motor ratio the same
+    if(_motorPower.motor1 < motorMin) motorMin = _motorPower.motor1;
+    if(_motorPower.motor1 > motorMax) motorMax = _motorPower.motor1;
+    if(_motorPower.motor2 < motorMin) motorMin = _motorPower.motor2;
+    if(_motorPower.motor2 > motorMax) motorMax = _motorPower.motor2;
+    if(_motorPower.motor3 < motorMin) motorMin = _motorPower.motor3;
+    if(_motorPower.motor3 > motorMax) motorMax = _motorPower.motor3;
+    if(_motorPower.motor4 < motorMin) motorMin = _motorPower.motor4;
+    if(_motorPower.motor4 > motorMax) motorMax = _motorPower.motor4;
+        
+    //Check if min or max is outside of the limits
+    if(motorMin < MOTORS_MIN) motorFix = MOTORS_MIN - motorMin;
+    else if(motorMax > MOTORS_MAX) motorFix = MOTORS_MAX - motorMax;
+    
+    //Add/remove constant
+    _motorPower.motor1 += motorFix;
+    _motorPower.motor2 += motorFix;
+    _motorPower.motor3 += motorFix;
+    _motorPower.motor4 += motorFix;
+}
+
+void MotorMixer::setPower(float motor1Power, float motor2Power, float motor3Power, float motor4Power)
+{
+    _motor1->pulsewidth_us(motor1Power);
+    _motor2->pulsewidth_us(motor2Power);
+    _motor3->pulsewidth_us(motor3Power);
+    _motor4->pulsewidth_us(motor4Power);
+}
+
+void MotorMixer::setPower(float motorPower)
+{
+    setPower(motorPower, motorPower, motorPower, motorPower);
+}
+
+MotorMixer::MotorPower MotorMixer::getMotorPower()
+{
+    return _motorPower;
+}
\ No newline at end of file