Joseph Roberts / Mbed 2 deprecated Quadcopter_mk2

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed MODSERIAL filter mbed-rtos ConfigFile PID PPM FreeIMU_external_magnetometer TinyGPS

FlightController/MotorMixer/MotorMixer.cpp

Committer:
joe4465
Date:
2015-03-04
Revision:
0:c6a85bb2a827
Child:
2:969dfa4f2436

File content as of revision 0:c6a85bb2a827:

#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;
}