File content as of revision 2:858781a5bbd1:

#include <algorithm>
#include "GMD.hpp"

const float GMD::_frequency_to_tick_coeff = 3300.0f;
const uint32_t GMD::_default_frequency_kHz = 50;
const float GMD::_duty_lower_limit = 0.0f;
const float GMD::_duty_upper_limit = 1.0f;

GMD::GMD(PinName p0, PinName p1, PinName shut_down) : _shut_down(shut_down, 1) {
    _pwm[0] = new FastPWM(p0);
    _pwm[1] = new FastPWM(p1);
    
    set_frequency_kHz(_default_frequency_kHz);
}

void GMD::set_frequency_kHz(float f_kHz) {
    _pwm[0]->period_ticks(_frequency_to_tick_coeff / f_kHz);
    _pwm[1]->period_ticks(_frequency_to_tick_coeff / f_kHz);
}

float sign(float n) {
    if (n < 0.0f) {
        return -1.0f;
    }
    return 1.0f;
}

float GMD::set(float p) {
    p = std::max(-1.0f, std::min(p, 1.0f));
    p = (abs(p) * (_duty_upper_limit - _duty_lower_limit) + _duty_lower_limit) * sign(p);
    
    if (p < 0.0f) {
        _pwm[0]->write(-p);
        _pwm[1]->write(0.0f);
    } else {
        _pwm[0]->write(0.0f);
        _pwm[1]->write(p);
    }
    _shut_down = 1;
    return p;
}

void GMD::release() {
    _shut_down = 0;
}

float GMD::operator=(float p) {
    return set(p);
}