File content as of revision 17:cc9b854295d6:

/*
    STM3_ESC    Electronic Speed Controller board, drives Two Brushless Motors, full Four Quadrant Control.
    Jon Freeman  B. Eng Hons
    2015 - 2020
*/
#include "mbed.h"
//#include    "users/mbed_official/code/mbed-dev/file/707f6e361f3e/targets/TARGET_STM/TARGET_STM32F4/TARGET_STM32F401xE/device/stm32f401xe.h"
//#include    "stm32f401xe.h"
//#include    "stm32f411xe.h"
#include "STM3_ESC.h"
#include "BufferedSerial.h"
#include "brushless_motor.h"

extern  eeprom_settings     user_settings     ;
//extern  BufferedSerial pc;            //  The two com ports used. There is also an unused com port, com3 setup @ 1200 baud

/**
brushless_motor::brushless_motor    (PinName iadc, PinName pwv, PinName pwi, 
    const uint16_t * lutptr, PinName px, PinName py, PinName pz, PortName pn, int port_bit_mask, uint32_t rnum) 
    : Motor_I(iadc), maxV(pwv,PWM_PRESECALER_DEFAULT), maxI(pwi,PWM_PRESECALER_DEFAULT), H1(px), H2(py), H3(pz), OP(pn, port_bit_mask)      //  Constructor
*/
brushless_motor::brushless_motor    (PinName iadc, PinName pwv, PinName pwi, 
    const uint16_t * lutptr, PinName px, PinName py, PinName pz, PortName pn, const uint16_t port_bit_mask, const uint32_t rnum) 
    : Motor_I(iadc), maxV(pwv,PWM_PRESECALER_DEFAULT), maxI(pwi,PWM_PRESECALER_DEFAULT), H1(px), H2(py), H3(pz), OP(pn, port_bit_mask)      //  Constructor
{    //  Constructor
    OP = 0;
    H1.mode (PullUp);   //  PullUp resistors enabled on all Hall sensor input pins
    H2.mode (PullUp);
    H3.mode (PullUp);
    H1.rise (callback(this, &brushless_motor::Hall_change));     // Attach handler to the rising interruptIn edge
    H1.fall (callback(this, &brushless_motor::Hall_change));     // Attach handler to the falling interruptIn edge
    H2.rise (callback(this, &brushless_motor::Hall_change));     // Attach handler to the rising interruptIn edge
    H2.fall (callback(this, &brushless_motor::Hall_change));     // Attach handler to the falling interruptIn edge
    H3.rise (callback(this, &brushless_motor::Hall_change));     // Attach handler to the rising interruptIn edge
    H3.fall (callback(this, &brushless_motor::Hall_change));     // Attach handler to the falling interruptIn edge
    Hall_total = 0;  //  mode can be only 0, 8, 16 or 24, lut row select for Handbrake, Forward, Reverse, or Regen Braking
    Hall_previous = 0;
    maxV.period_ticks      (MAX_PWM_TICKS + 1);  //  around 18 kHz
    maxI.period_ticks      (MAX_PWM_TICKS + 1);
    maxV.pulsewidth_ticks  (MAX_PWM_TICKS - 20);    //  Motor voltage pwm is inverted, see MCP1630 data
    maxI.pulsewidth_ticks  (MAX_PWM_TICKS / 30);    //  Motor current pwm is not inverted. Initial values for scope hanging test
    visible_mode    = MOTOR_REGENBRAKE;
    inner_mode      = MOTOR_REGENBRAKE;
    lut = lutptr;                       //  Pointer to motor energisation bit pattern table
    current_sense_rs_offset = rnum;     //  This is position in user_settings.rd(current_sense_rs_offset)
    Hall_index[0] = Hall_index[1]  = read_Halls    ();
    tickleon    = 0;
    direction   = 0;
    angle_cnt   = 0;        //  Incremented or decremented on each Hall event according to actual measured direction of travel
    encoder_error_cnt = 0;  //  Incremented when Hall transition not recognised as either direction
    last_V = last_I = 0.0;
    Idbl = 0.0;
//    numof_current_sense_rs = 1.0;
    RPM_filter  = 0.0;
    dRPM        = 0.0;
    V_clamp =   1.0 ;       //  Used to limit top speed
    motor_poles   = 8;    //  Default to 8 pole motor
}

/**
*   void    brushless_motor::sniff_current   ()  {          //  Initiate ADC current reading of approx motor average current
*       This to be called in response to ticker timebase interrupt.
*       As designed, called at 200 micro second intervals (Feb 2019)
*       Updates double I.dbl current measured in milliamps
*       Reading not used elsewhere in this code but made available through command line for external controller
*/
void    brushless_motor::sniff_current   ()  {          //  Initiate ADC current reading
    const   double  sampweight  = 0.01 ; /// (double)CURRENT_SAMPLES_AVERAGED    ;
    const   double  shrinkby    = 1.0 - sampweight;
    uint16_t    samp    = Motor_I.read_u16    (); //  CHECK but thought to be called once per 200us for each motor - CORRECT Feb 2019
//    double      dbls    = ((double)samp) * numof_current_sense_rs / 6.0;    //  reading in mA
//    double      dbls    = (((double)samp) / 6.0) * (double)user_settings.rd(current_sense_rs_offset);    //  reading in mA
//    double      dbls    = sampweight * (double)(samp * user_settings.rd(current_sense_rs_offset)) / 6.0;    //  reading in mA
    Idbl *= shrinkby;              //  Jan 2019    New recursive low pass filter
//    Idbl += dbls * sampweight;     //  Current reading available, however not certain this is of any real use
    Idbl += sampweight * (double)(samp * user_settings.rd(current_sense_rs_offset)) / 6.0;     //  Current reading available, however not certain this is of any real use
}

bool    brushless_motor::poles  (uint32_t p)  {
    if  (p == 4 || p == 6 || p == 8)    {
        motor_poles = p;
        return  true;
    }
    return  false;
}

void    brushless_motor::Hall_change   ()  {
    int32_t     delta_theta;
    const   int32_t delta_theta_lut[] = {   //  Looks up -1 for forward move detected, +1 for reverse move detected, 0 for error or unknown
        0, 0, 0, 0, 0, 0, 0, 0,     //  Previous Hindex was 0
        0, 0, 0,-1, 0, 1, 0, 0,     //  Previous Hindex was 1
        0, 0, 0, 1, 0, 0,-1, 0,     //  Previous Hindex was 2
        0, 1,-1, 0, 0, 0, 0, 0,     //  Previous Hindex was 3
        0, 0, 0, 0, 0,-1, 1, 0,     //  Previous Hindex was 4
        0,-1, 0, 0, 1, 0, 0, 0,     //  Previous Hindex was 5
        0, 0, 1, 0,-1, 0, 0, 0,     //  Previous Hindex was 6
        0, 0, 0, 0, 0, 0, 0, 0,     //  Previous Hindex was 7
    }  ;

    Hall_index[0]   = read_Halls    ();
    delta_theta = delta_theta_lut[(Hall_index[1] << 3) | Hall_index[0]];

    if  (delta_theta == 0)  //  Should only ever be +1 in 1 direction, -1 in other direction. 0 indicates invalid Hall sensor transition detected.
        encoder_error_cnt++;    //  Never used Dec 2019
    else
        angle_cnt += delta_theta;
    OP = lut[inner_mode | Hall_index[0]];  //  changed mode to inner_mode 27/04/18
    Hall_total++;
    Hall_index[1] = Hall_index[0];
}

/**
*   void    brushless_motor::set_direction   (int dir)  {
*   Used to set direction according to user_settings data from eeprom
*/
void    brushless_motor::set_direction   (uint32_t dir)
{
    direction = (dir != 0) ? MOTOR_FORWARD | MOTOR_REVERSE : 0; //  bits used in eor, FORWARD = 1<<3, REVERSE = 1<<4
}

//uint32_t    get_direction   ()  {
//    return  direction;
//}

uint32_t    brushless_motor::read_Halls  ()
{
    uint32_t x = 0;
    if  (H1)    x |= 1;
    if  (H2)    x |= 2;
    if  (H3)    x |= 4;
    return  x;
}

void    brushless_motor::high_side_off   () //  Jan 2019    Only ever called from main when high side gate drive charge might need pumping up
{
    uint16_t    p = OP;
    p &= lut[32];   //  KEEP_L_MASK_A or B
    OP = p;
}
/*
void    brushless_motor::low_side_on   ()
{
    maxV.pulsewidth_ticks  (1);
    OP = lut[31];   //  KEEP_L_MASK_A or B
}
*/


extern  int WatchDog;
#define DRIVING (visible_mode == MOTOR_FORWARD || visible_mode == MOTOR_REVERSE)
#define ESTOP   (WatchDog == 0 && DRIVING)

/**
*   void    brushless_motor::set_V_limit (double p)  //  Sets max motor voltage.
*
*   Set motor voltage limit from zero (p=0.0) to max link voltage (p=1.0)
*/
void    brushless_motor::motor_voltage_refresh ()  //  May alter motor voltage to reflect changes in V_clamp
{
    double  p = last_V;
    if  ((V_clamp < last_V) && DRIVING)    //  Jan 2019 limit top speed when driving
        p = V_clamp;    //  If motor runnable, set voltage limit to lower of last_V and V_clamp

    p *= 0.95;      //  need limit, ffi see MCP1630 data
    p   = 1.0 - p;  //  because pwm is wrong way up
    maxV.pulsewidth_ticks  ((int)(p * MAX_PWM_TICKS));  //  PWM output to MCP1630 inverted motor pwm as MCP1630 inverts
}

void    brushless_motor::set_V_limit (double p)  //  Sets max motor voltage.
{
    if  (p < 0.0 || ESTOP)
        p = 0.0;
    if  (p > 1.0)
        p = 1.0;
    last_V = p;     //  Retains last voltage limit demanded by driver
    motor_voltage_refresh   ()  ;
}


/**void    brushless_motor::set_I_limit (double p)     //  Sets max motor current. pwm integrated to dc ref voltage level
*
*   Set motor current limit from zero (p=0.0) to max determined by current sense resistors (p=1.0)
*       Value sent to pwm with RC integrator acting as AnalogOut.
*       pwm capable of 0.0 <= V_out <= 3.3. This feeds MCP1630 V_Ref, range 0 to 2.7v
*       Therefore (2.7/3.3) = 0.82 factor included.
*   Jan 2019 - As yet uncalibrated, so let's have a go at working it out!
*       Voltage ax current sense resistors amplified by op-amp with gain 5.7 (see EasyPC file 'BrushlessSTM3.sch', U6, R63, R64)
*       This then put through pot divider (10k with 4k7 to ground, gain 0.32) reducing overall gain to 1.8 (accurate enough)
*       This connects to MCP1630 CS (current sense) pin which works over the voltage range 0.0 to 0.9v
*       Therefore 0.5v across current sense resistor is sufficient to turn driver off.
*       0.5v across 0.05 ohm gives 10A per current sense resistor fitted.
*       ** NOTE ** This is fast acting cycle by cycle current limit, the 10A figure is therefore peak T_on current.
*
*   Current flows through current sense reaistor when one high side and one low side switch are on, expect a rising ramp due to motor inductance.
*   When either switch is off, inductor current continues to flow but not through current sense resistors, through a parasitic diode instead.
*   Thus T_on current is measured, T_off current is not measured
*   This means current reading should approximate to current taken from the supply. Motor average current may be considerably higher.
*       During REGEN_BRAKING, current flows the 'wrong' way through sense resistors and can not be measured.
*
*   Board designed to have 1, 2, 3 or 4 0R05 current sense resistors per motor for 10A, 20A, 30A or 40A peak motor currents
*/
//const   double MPR = (double)((MAX_PWM_TICKS * 9) / 11);    //  Scales 3.3v pwm DAC output to 2.7v V_Ref input
const   uint32_t MPR = ((MAX_PWM_TICKS * 9) / 11);    //  Scales 3.3v pwm DAC output to 2.7v V_Ref input

void    brushless_motor::set_I_limit (double p)     //  Sets max motor current. pwm integrated to dc ref voltage level
{
    if  (p < 0.0 || ESTOP)
        p = 0.0;
    if  (p > 1.0)
        p = 1.0;
    last_I = p;     //  Retains last current limit demanded by driver
    if  (DRIVING)    {
        p *= current_scale;
    }
    maxI.pulsewidth_ticks  ((uint32_t)(p * MPR));  //  PWM
}

void    brushless_motor::I_scale (double p)     //  Sets max motor current. pwm integrated to dc ref voltage level
{
    current_scale = p;
    if  (DRIVING)    {
        maxI.pulsewidth_ticks  ((uint32_t)(last_I * p * MPR));  //  PWM
    }
}

/**
*   void    brushless_motor::speed_monitor_and_control   ()       //  ** CALL THIS 32 TIMES PER SECOND **
*   Call this once per 'MAIN_LOOP_REPEAT_TIME_US= 31250' main loop pass to keep RPM and MPH figures correct
*   Tracks total transitions on Hall sensor inputs to determine speed.
*   Sets variables double dRPM of motor RPM, and double dMPH miles per hour
*
*   Speed control - double target_speed as reference input. *
*       **  This is where any speed limit gets applied **
*           Motor voltage reduced when at or over speed. Does NOT apply any braking
*   Scope for further improvement of control algorithm - crude implementation of PID with no I
*/
void    brushless_motor::speed_monitor_and_control   ()       //  call this once per 'MAIN_LOOP_REPEAT_TIME_US= 31250' main loop pass to keep count = edges per sec
{               //
//    const   double  samp_scale  = 0.275;                  //  Tweak this value only to tune filter
    const   double  samp_scale  = 0.6;                  //  Increased May 2020 to improve ssl speed settling time
    const   double  shrink_by   = 1.0 - samp_scale;
    double  rpm, speed_error;
    uint32_t        Hall_tot_copy = Hall_total;     //  Copy value for use throughout function as may get changed at any instant during exec !
    moving_flag  = true;
    if  (Hall_previous == Hall_tot_copy)  {         //  Test for motor turning or not
        moving_flag  = false;                       //  Zero Hall transitions since previous call - motor not moving
        tickleon    = TICKLE_TIMES;                 //  May need to tickle some charge into high side switch bootstrap supplies
    }
    rpm   = (double) (((Hall_tot_copy - Hall_previous) * 640) / motor_poles);  //  (Motor Hall sensor transitions in previous 31250us) * 640
    RPM_filter  *= shrink_by;   //      rpm now true but lumpy 'RPM' during most recent 31250us corrected for number of motor poles
    RPM_filter  += (rpm * samp_scale);  //  filtered RPM
    dRPM = RPM_filter;
    dMPH    =  user_settings.rpm2mph(RPM_filter);     //  Completed updates of RPM and MPH
    Hall_previous = Hall_tot_copy;
    speed_error    = (dMPH - user_settings.top_speed());                //  'P' Proportional contribution to PID control
    bool    clamp_change = false;
    if  (speed_error > 0.0 && (inner_mode == MOTOR_FORWARD || inner_mode == MOTOR_REVERSE)) {  //  Speed control only makes sense when motor runnable
        V_clamp *= (1.0 - (speed_error / 60.0));  //  Driving too fast, so lower clamp voltage a tiny bit
        clamp_change = true;
    }
    else    {   //  Not going too fast, and maybe driving or not
        if  (V_clamp < 0.99)    {
            V_clamp += 0.015;
            if  (V_clamp > 1.0)
                V_clamp = 1.0;
            clamp_change = true;
        }
    }
    if  (clamp_change)  {
        motor_voltage_refresh   ()  ;
    }
/*        d = V_clamp + (speed_error * sdbl[2]) + ((dv_by_dt / 1000.0) * sdbl[4]);  //  Apply P+I+D (with I=0) control
        if  (d > 1.0)   d = 1.0;
        if  (d < 0.0)   d = 0.0;
        V_clamp = d;
        if  (V_clamp < last_V)   //  Jan 2019 limit top speed when driving
        {    
            d *= 0.95;   //  need limit, ffi see MCP1630 data
            d   = 1.0 - d;  //  because pwm is wrong way up
//FUCKETYFUCK            maxV.pulsewidth_ticks  ((int)(d * MAX_PWM_TICKS));  //  PWM output to MCP1630 inverted motor pwm as MCP1630 inverts
            maxV.pulsewidth_ticks  ((int)(d * MAX_PWM_TICKS));  //  PWM output to MCP1630 inverted motor pwm as MCP1630 inverts
        }
    }*/
/*    temp_tick++;
    if  (temp_tick > 50)    {   //  one and a bit second
        temp_tick = 0;
        pc.printf   ("top speed %.1f, mph %.1f, speed_err %.1f, V_clamp%.3f\r\n", user_settings.top_speed(), dMPH, speed_error, V_clamp);
    }
*/
}

bool    brushless_motor::exists ()  {
    return  ((Hall_index[0] > 0) && (Hall_index[0] < 7))    ;
}

bool    brushless_motor::is_moving ()
{
    return  moving_flag;
}

/**
bool    brushless_motor::set_mode (int m)
Use to set motor to one mode of HANDBRAKE, FORWARD, REVERSE. Used to also be used to set REGENBRAKE, replaced by brake(x) function.
If this causes change of mode, also sets V and I to zero.
*/
bool    brushless_motor::set_mode (uint32_t m)
{
    if  ((m != MOTOR_HANDBRAKE) && (m != MOTOR_FORWARD) && (m != MOTOR_REVERSE) && (m != MOTOR_REGENBRAKE))  {
//        pc.printf   ("Error in set_mode, invalid mode %d\r\n", m);
        return  false;
    }
    if  (visible_mode != m) {   //  Mode change, kill volts and amps to be safe
        set_V_limit (0.0);
        set_I_limit (0.0);
        visible_mode = m;
    }
    if  (m == MOTOR_FORWARD || m == MOTOR_REVERSE)  //  8 or 16 - these are effectively address bits of motor pattern lut
        m ^= direction; //  direction set to 0 or (MOTOR_FORWARD | MOTOR_REVERSE), so has zero or two bits set
    inner_mode = m;     //  idea is to use inner_mode only in lut addressing, keep 'visible_mode' true regardless of setup data in eeprom
    return  true;
}

void    brushless_motor::brake   (double brake_effort)    {
//    pc.printf   ("In motor::brake, user_settings.brake_effectiveness = %3f\r\n", user_settings.brake_effectiveness());
    set_mode    (MOTOR_REGENBRAKE);
    if  (brake_effort > 1.0)
        brake_effort = 1.0;
    if  (brake_effort < 0.0)
        brake_effort = 0.0;
    brake_effort *= user_settings.brake_effectiveness();    //  set upper limit, this is essential - May2020 fixed, was 100 times too big
    brake_effort = sqrt  (brake_effort);  //  to linearise effect
    set_V_limit (brake_effort)  ;
    set_I_limit (1.0);
    V_clamp = 1.0;
}

uint32_t    brushless_motor::get_mode ()    {
    return  visible_mode;
}

void    brushless_motor::motor_set  ()  //  Energise Port with data determined by Hall sensors
{
    Hall_index[0]  = read_Halls    ();
    OP = lut[inner_mode | Hall_index[0]];
}