File content as of revision 16:d1e4b9ad3b8b:

#include "mbed.h"
#include "BufferedSerial.h"
#include "Radio_Control_In.h"
#include "STM3_ESC.h"
/**class   RControl_In
    Jon Freeman
    Jan 2019

    Checks for __-__ duration 800-2200us
    Checks repetition rate in range 5-25ms
*/
extern  BufferedSerial  pc;
//extern  eeprom_settings     user_settings     ;

//    RControl_In::RControl_In   ()   {    //  Default Constructor
//        pulse_width_us = period_us = pulse_count = 0;
//        lost_chan_return_value = 0.0;
//    }   ;
//    RControl_In::RControl_In   (PinName inp) : pulse_in(inp)   {    //  Default Constructor
//        pulse_width_us = period_us = pulse_count = 0;
//        lost_chan_return_value = 0.0;
//    }   ;
/**
*/
void        RControl_In::set_lost_chan_return_value  (double d)  {
    lost_chan_return_value = d;
}

uint32_t    RControl_In::pulsewidth   ()
{
    return  pulse_width_us;
}

uint32_t    RControl_In::pulsecount   ()
{
    return  pulse_count;
}

uint32_t    RControl_In::period   ()
{
    return  period_us;
}

bool    RControl_In::validate_rx ()
{   //  Tests for pulse width and repetition rates being believable
    return  !((period_us < 5000) || (period_us > 25000) || (pulse_width_us < 800) || (pulse_width_us > 2200));
}

bool        RControl_In::energise    (struct  RC_stick_info & stick, struct brushless_motor & motor)  {     //  December 2019
    if  (stick.active)   {
        if  (stick.zone == ZONE_DRIVE)   {
            motor.set_mode (stick.stick_implied_motor_direction == 1 ? MOTOR_FORWARD : MOTOR_REVERSE);
            motor.set_V_limit  (stick.drive_effort);
            motor.set_I_limit  (stick.drive_effort);    //  This could be 1.0, or other options
        }
        if  (stick.zone == ZONE_BRAKE)   {
            motor.brake    (stick.brake_effort);
        }
    }
    return  stick.active;
}

bool        RControl_In::read    (class  RC_stick_info & stick)  {      //  December 2019
    double  dtmp;
    uint32_t    old_zone = stick.zone;
    stick.chan_mode = get_chanmode(); //  0 disabled, 1 uni-dir, or 2 bi-dir
    stick.active = validate_rx();     //  True if RC Rx delivering believable pulse duration and timing
    if  (stick.active && (stick.chan_mode < 1 || stick.chan_mode > 2))    {   //  Should signal an error here
        stick.active = false;
    }
    if  (stick.active)    {
        stick.raw = (double) (pulse_width_us - 1000);  //  Read pulse width from Rx, left with -200.0 to + 1200.0 allowing for some margin
        stick.raw /= 1000.0;        //  pulse width varies between typ 1000 to 2000 micro seconds
        stick.raw += range_offset;  //  range now normalised to 0.0 <= raw <= 1.0
        if  (stick.raw > 1.0)        stick.raw = 1.0;
        if  (stick.raw < 0.0)        stick.raw = 0.0;   //  clipped to strict limits 0.0 and 1.0
        if  (stick_sense != 0)
            stick.raw = 1.0 - stick.raw;    //  user setting allows for stick sense reversal
        stick.deflection = stick.raw;
        stick.stick_implied_motor_direction = +1;     //  -1 Reverse, 0 Stopped, +1 Forward
        if  (stick.chan_mode == 2)  {       //  Bi-directional centre zero stick mode selected by user
            stick.deflection = (stick.raw * 2.0) - 1.0;     //  range here -1.0 <= deflection <= +1.0
            if  (stick.deflection < 0.0)    {
                stick.deflection = 0.0 - stick.deflection;  //  range inverted if negative, direction info separated out
                stick.stick_implied_motor_direction = -1;     //  -1 Reverse, 0 Stopped, +1 Forward (almost never 0)
            }   //  endof deflection < 0.0
        }       //  endof if chan_mode == 2
        //  Now find zone from deflection
        stick.zone = ZONE_COAST;
        if  (stick.deflection < (brake_segment - 0.02))     //  size of brake_segment user settable
            stick.zone = ZONE_BRAKE;
        if  (stick.deflection > (brake_segment + 0.02))     //  Tiny 'freewheel' COAST band between drive and brake
            stick.zone = ZONE_DRIVE;
        if  (old_zone != ZONE_COAST && old_zone != stick.zone)    //
            stick.zone = ZONE_COAST;        //  Ensures transitions between BRAKE and DRIVE go via COAST
        switch  (stick.zone)    {
            case    ZONE_COAST:
                stick.drive_effort = 0.0;
                stick.brake_effort = 0.0;
                break;
            case    ZONE_BRAKE:
                stick.brake_effort = (brake_segment - stick.deflection) / brake_segment;    //  1.0 at zero deflection, reducing to 0.0 on boundary with DRIVE
                stick.drive_effort = 0.0;
                break;
            case    ZONE_DRIVE:
                stick.brake_effort = 0.0;
                dtmp = (stick.deflection - brake_segment) / (1.0 - brake_segment);
                if  (dtmp > stick.drive_effort) {       //  Stick has moved in increasing demand direction
                    stick.drive_effort *= (1.0 - stick_attack);     //  Apply 'viscous damping' to demand increases for smoother operation
                    stick.drive_effort += (dtmp * stick_attack);    //  Low pass filter, time constant variable by choosing 'stick_attack' value %age
                }
                else    //  Reduction or no increase in demanded drive effort
                    stick.drive_effort = dtmp;      //  Reduce demand immediately, i.e. no viscous damping on reduced demand
                break;
        }       //  endof switch
    }           //  endof if active
    else    {   //  stick Not active
        stick.zone = ZONE_BRAKE;
        stick.raw = 0.0;
        stick.deflection = 0.0;
    }           //  endof not active
    return  stick.active;
}


void    RControl_In::set_offset (signed char offs, char brake_pcent, char attack)   {   //  Takes user_settings[RCIx_TRIM]
    brake_segment = ((double) brake_pcent) / 100.0;
    stick_attack = ((double) attack) / 100.0;
    range_offset = (double) offs;
    range_offset /= 1000.0;         //  This is where to set range_offset sensitivity
//    pc.printf   ("In RControl_In::set_offset, input signed char = %d, out f %.3f\r\n", offs, range_offset);
}

uint32_t    RControl_In::get_chanmode    () {
    return  chan_mode;
}

void    RControl_In::set_chanmode    (char c, char polarity) {
    chan_mode = ((uint32_t) c);
    stick_sense = (uint32_t) polarity;
}

void    RControl_In::RadC_fall    ()     //  December 2018 - Could not make Servo port bidirectional, fix by using PC_14 and 15 as inputs
{                                       //  30th November 2019 - Swapped _rise and _fall as now using Schmitt inverters on input    
    period_us = t.read_us    ();
    t.reset ();
    t.start ();
}

void    RControl_In::RadC_rise    ()
{
    pulse_width_us = t.read_us   ();
    pulse_count++;
}
//  end of RControl_In class