File content as of revision 12:a25bdf135348:

/*
November 2018  -   Jon Freeman
Cloned from Loco_TS_2018_06 on 23rd November 2018

Touch Screen controller communicates with 1, 2, ... n Brushless STM3 Controller boards via opto-isolated serial port.

9 pid D connector retained but wiring NOT compatible with 2017.
This time pins are : -
1   Not Used on TS2018, connection from Twin BLDC Controller only - Pot wiper
2   Not Used on TS2018, connection from Twin BLDC Controller only - GND
3   Not Used on TS2018, connection from Twin BLDC Controller only - Weak +5 (top of pot)
4   Not Used on TS2018, connection from Twin BLDC Controller only - Fwd / Rev switched between pins 2 and 3 above
5   TS2018 high voltage output - power up signal to Twin BLDC Controllers, +10 to + 70 Volt (full supply via 3k3 0.5W safety resistor)
6   Twin BLDC Rx- <- TS2018 Tx data     ||GND to avoid exposing TS +5v rail to the outside world
7   Twin BLDC Rx+ <- TS2018 +5v         ||Tx\ to avoid exposing TS +5v rail to the outside world, INVERTED Txd idles lo
8   Twin BLDC Tx- <- TS2018 GND
9   Twin BLDC Tx+ <- TS2018 Rx data with 1k pullup to +5, line idles hi
*/
#include "mbed.h"
#include "Electric_Loco.h"
#include "AsyncSerial.hpp"
#include "Servo.h"
#include "TS_DISCO_F746NG.h"
#include "LCD_DISCO_F746NG.h"

char   const_version_string[] = {"1.0.0\0"};  //  Version string, readable from serial ports

LCD_DISCO_F746NG        lcd ;               //  LCD display
TS_DISCO_F746NG         touch_screen    ;   //  Touch Screen
screen_touch_handler    slider  ;           //  Loco driver's slider control

//  see movingcoilmeter.h ffi
moving_coil_meter   Voltmeter   (   LCD_COLOR_BLACK, LCD_COLOR_WHITE, LCD_COLOR_RED, LCD_COLOR_BLUE, LCD_COLOR_MAGENTA,
                                    VOLTMETER_X, VOLTMETER_Y, V_A_SIZE, 22.0, 61.0, 1.25 * PI, -0.25 * PI , 20, "V", ONE_DP, false),  
                    Powermeter  (   LCD_COLOR_BLACK, LCD_COLOR_WHITE, LCD_COLOR_RED, LCD_COLOR_BLUE, LCD_COLOR_BLUE,
                                    AMMETER_X, AMMETER_Y, V_A_SIZE, -1400.0, 1400.0, 1.25 * PI, -0.25 * PI , 14, "Watt", NO_DPS, false),    
                    Speedo      (   SPEEDO_BODY_COLOUR, SPEEDO_DIAL_COLOUR, LCD_COLOR_RED, SPEEDO_TEXT_COLOUR, LCD_COLOR_BLACK,
                                    SPEEDO_X, SPEEDO_Y, SPEEDO_SIZE, 0.0, 12.0, 1.25 * PI, -0.25 * PI , 12, "MPH", ONE_DP, false);
                                //  3 instances of moving coil meter graphic

error_handling_Jan_2019     Controller_Error    ;         //  Provides array usable to store error codes.
STM3_ESC_Interface          My_STM3_ESC_boards   ;

extern  command_line_interpreter_core   pcli, ploco;    //  pcli handles comms with pc, ploco handles comms with STM3_ESC boards

/*
STRANGE BEHAVIOUR WARNING !
This project requires two serial ports.
The following combination of one 'Serial' and one 'AsyncSerial' is the only combination found to work !
MODSERIAL has not been adapted to F746NG, will not compile.
Does compile with BufferedSerial but crashes the whole thing. No startup.
*/

Serial      pc      (USBTX, USBRX);    //  AsyncSerial does not work here. Comms to 'PuTTY' or similar comms programme on pc
AsyncSerial com2escs (A4, A5);   //  Com port to opto isolators on Twin BLDC Controller boards. Only AsyncSerial works here

//DigitalOut  reverse_pin     (D7);   //  These pins no longer used to set mode and direction, now commands issued to com
//DigitalOut  forward_pin     (D9);   //was D6, these two decode to fwd, rev, regen_braking and park

DigitalOut  I_sink1         (D14);  //  a horn
DigitalOut  I_sink2         (D15);  //  lamp
DigitalOut  I_sink3         (D3);  //  lamp
DigitalOut  I_sink4         (D4);
DigitalOut  I_sink5         (D5);
DigitalOut  led_grn         (LED1); //  the only on board user led

DigitalIn   f_r_switch      (D2);   //  was D0, Reads position of centre-off ignition switch
//DigitalIn   spareio_d8      (D8);
//DigitalIn   spareio_d9      (D9);
DigitalIn   spareio_d10     (D10);  //  D8, D9, D10 wired to jumper on pcb - not used to Apr 2017

AnalogIn    ht_volts_ain    (A0);  //  Jan 2017
AnalogIn    ht_amps_ain     (A1);  //  Jan 2017
//AnalogIn    spare_ain2      (A2);
//AnalogIn    spare_ain3      (A3);
//AnalogIn    spare_ain4      (A4);   //  hardware on pcb for these 3 spare analogue inputs - not used to Apr 2017
//AnalogIn    spare_ain5      (A5); //  causes display flicker !

Servo   servo1    (D6);     //  Model control servo used to adjust Honda engine speed

extern  bool    test_qspi   ()  ;
extern  bool    odometer_zero   ()  ;   //  Returns true on success
extern  bool    odometer_update  (uint32_t pulsetot, uint16_t pow, uint16_t volt)  ;   //  Hall pulse total updated once per sec and saved in blocks of 4096 bytes on QSPI onboard memory

extern  void    setup_buttons    ()  ;
extern  void    rewrite_odometer    ()  ;

static  const   int
    MAF_PTS             = 140,      //  Moving Average Filter points. Filters reduce noise on volatage and current readings
    FWD                 = 0,
    REV                 = ~FWD;

int32_t     V_maf[MAF_PTS + 2],    I_maf[MAF_PTS + 2],  maf_ptr = 0;    //
volatile    uint32_t    sys_timer_32Hz = 0;
double      recent_distance = 0.0;

bool        qtrsec_trig             = false;
bool        trigger_current_read    = false;
volatile    bool    trigger_32ms    = false;

double  read_voltmeter   ()
{
    int32_t a = 0;
    for (int b = 0; b < MAF_PTS; b++)
        a += V_maf[b];
    a /= MAF_PTS;
    double v = (double) a;
    return  (v / 932.0) + 0.0;  //  fiddled to suit resistor values
}

double  read_ammeter ()     //  Returns amps taken by STM3escs - amps dumped due to over-voltage
{                           //  Could make sense to read this at up to 32 times per second
    int32_t a = 0;          //  MAF data almost completely renewed at this rate
    for (int b = 0; b < MAF_PTS; b++)   //  MAF updated every 250us, MAF size = MAF_PTS (once set to 140, probably still is)
        a += I_maf[b];
    a /= MAF_PTS;
    a -= 0x4000;
    double i = (double) (0 - a);
    return  i / 200.0;      //  Fiddled to get current reading close enough
}

const   int BIGMAFSIZ = 8;

class   ammeter_reading  {
    double  bigImaf[BIGMAFSIZ];
    int     bigImafptr;
    double  amps_longave,   //  internal use only
            amps_latest,    //  update() called @ 32Hz. Value stored here is average over most recent 3125us
            amps_filtered;  //  Average of the BIGMAFSIZ most recent samples stored in latest
public:    
    ammeter_reading ()  {   //  constructor
        bigImafptr = 0;
        amps_longave = amps_latest = amps_filtered = 0.0;
        for (int i = 0; i < BIGMAFSIZ; i++)
            bigImaf[i] = 0.0;
    }
    void    update  ()  ;   //  Read ammeter core, store most recent 32ms or so worth in amps_latest, 250ms average in amps_filtered
    double  latest  ()  ;
    double  filtered()  ;
}   Ammeter ;

double  ammeter_reading::latest     ()  {
    return  amps_latest;
}

double  ammeter_reading::filtered   ()  {
    return  amps_filtered;
}

void  ammeter_reading::update   ()  {
    amps_latest = read_ammeter();
    amps_longave -= bigImaf[bigImafptr];
    bigImaf[bigImafptr] = amps_latest;
    amps_longave += amps_latest;
    bigImafptr++;
    if  (bigImafptr >= BIGMAFSIZ)
        bigImafptr = 0;
    amps_filtered = amps_longave / BIGMAFSIZ;
}


//  Interrupt Service Routines

void    ISR_current_reader  (void)      //  FIXED at 250us
{
    static  int ms32 = 0, ms250 = 0;
    trigger_current_read    = true; //  every 250us, i.e. 4kHz  NOTE only sets trigger here, readings taken in main loop
    ms32++;
    if  (ms32 >= 125)    {   //  31.25ms, not 32ms, is 32Hz
        ms32 = 0;
        sys_timer_32Hz++;   //  , usable anywhere as general measure of elapsed time
        trigger_32ms = true;
        ms250++;
        if  (ms250 >= 8) {
            ms250 = 0;
            qtrsec_trig = true;
        }
    }
}

//  End of Interrupt Service Routines

void    throttle    (double p)  {            // New Apr 2018 ; servo adjusts throttle lever on Honda GX120
    const   double  THR_MAX = 0.92;     //  Values tweaked to suit servo and linkage fitted to loco power unit
    const   double  THR_MIN = 0.09;
    const   double  RANGE = (THR_MAX - THR_MIN);
    if  (p > 1.0)
        p = 1.0;
    if  (p < 0.0)
        p = 0.0;
    //  p = 1.0 - p;    //  if direction needs swapping
    p *= RANGE;
    p += THR_MIN;
    servo1 = p;
}


void    horn    (int which, int onoff)  {
    if  (which == HI_HORN)
        I_sink5 = onoff;
    else
        I_sink4 = onoff;
}


void    lights  (int onoff)  {
    I_sink2 = onoff;    //  lamp right
    I_sink3 = onoff;    //  lamp left
}


void    draw_normal_run_screen  ()  {
    lcd.Clear(LCD_COLOR_LIGHTGRAY);
    setup_buttons(); //  draws buttons
    slider.DrawSlider ();
    //  Draw 3 analogue meter movements, speedo, voltmeter, ammeter
    Speedo.redraw();
    Voltmeter.redraw();
    Powermeter.redraw();
}


int main()  //  Programme entry point
{
    int     qtr_sec = 0, seconds = 0, minutes = 0;
    double  electrical_power_Watt = 0.0, volts = 0.0;

    pc.baud (9600);
    com2escs.baud (19200);

    I_sink1 = 0;    //  turn outputs off
    I_sink2 = 0;    //  lamp right
    I_sink3 = 0;    //  lamp left
    I_sink4 = 0;    //  low horn
    I_sink5 = 0;    //  high horn
    spareio_d10.mode(PullUp);

    Ticker  tick250us;      //  Master 4kHz interrupt timebase

//  Setup User Interrupt Vectors
    tick250us.attach_us (&ISR_current_reader, 250);    //  count 125 interrupts to trig 31.25ms

//  QSPI memory is now in constant use for odometer
    if  (!test_qspi())
        Controller_Error.set    (FAULT_QSPI, -1);   //        pc.printf   ("Problem with qspimemcheck\r\n");

    slider.direction = f_r_switch ? REV : FWD;      //  Only place in the code where direction gets set. Centre-Off power switch REV-OFF-FWD.

    My_STM3_ESC_boards.set_I_limit (0.0);
    My_STM3_ESC_boards.set_V_limit (0.0);
    My_STM3_ESC_boards.message  ("rb\r");
    throttle    (0.0);      //  Set revs to idle. Start engine and warm up before powering up control
    pc.printf   ("\r\n\n\nJon's Loco_TS_2018 Loco Controller ver %s starting, direction %s\r\n", const_version_string, slider.direction ? "Forward":"Reverse");

    uint8_t lcd_status = touch_screen.Init(lcd.GetXSize(), lcd.GetYSize());
    if (lcd_status != TS_OK) {
        Controller_Error.set    (FAULT_TS, -1);
    } 
    lcd.Clear(LCD_COLOR_DARKBLUE);
    lcd.SetBackColor(LCD_COLOR_GREEN);
    lcd.SetTextColor(LCD_COLOR_WHITE);
    lcd.DisplayStringAt(0, LINE(5), (uint8_t *)"TOUCHSCREEN INIT OK", CENTER_MODE);

//    if  (odometer_zero   ())
//        pc.printf   ("TRUE from odometer_zero\r\n");
//    else
//        pc.printf   ("FALSE from odometer_zero\r\n");

    lights  (1);    //  Headlights ON!

    My_STM3_ESC_boards.search_for_escs ();  //  Build list of connected STM3_ESC IDs

/*    Controller_Error.set    (3, 99);
    pc.printf   ("%lx red\r\n", LCD_COLOR_RED);     //LCD_COLOR is 0xffrrggbb
    pc.printf   ("%lx grn\r\n", LCD_COLOR_GREEN);
    pc.printf   ("%lx blu\r\n", LCD_COLOR_BLUE);
    pc.printf   ("%lx blk\r\n", LCD_COLOR_BLACK);
    pc.printf   ("%lx white\r\n", LCD_COLOR_WHITE);
*/
    draw_normal_run_screen  ();

    pc.printf   ("Controller_Error.all_good() ret'd %s\r\n", Controller_Error.all_good() ? "true" : "false");

    while (1) {     //  main prog loop

        pcli.sniff  ();   //  Do any actions from command line serial port via usb link

        if  (trigger_current_read)  {     //  flag set in interrupt handler every 250us
            trigger_current_read = false;
            I_maf[maf_ptr] = ht_amps_ain.read_u16();    //  Read raw ACS709 ammeter module
            V_maf[maf_ptr] = ht_volts_ain.read_u16();   //  Read raw system voltage
            maf_ptr++;
            if  (maf_ptr > MAF_PTS - 1)
                maf_ptr = 0;
        }                       //  endof stuff to do every 250us

        if  (trigger_32ms == true)  {       //  Stuff to do every 31.25 milli secs (32Hz)
            trigger_32ms = false;
            ploco.sniff ();         //  Only call within main loop, checks message responses from STM3_ESC boards
            Ammeter.update ();      //  This updates Ammeter 'latest' and 'filtered' variables every 31.25ms
            slider.HandleFingerInput   ();  //  Do everything concerning fingers on touch screen
        }                                  //  endof doing 32Hz stuff

        if  (qtrsec_trig == true)  {    //  do every quarter second stuff here
            qtrsec_trig = false;
            volts = read_voltmeter();    //  voltage and current readings updated @ 250us, these are averaged over 35ms or so
            electrical_power_Watt = volts * Ammeter.filtered();   //  visible throughout main
            //  Update meters
            Powermeter.set_value(electrical_power_Watt);
            Voltmeter.set_value (volts);
            Speedo.set_value    (My_STM3_ESC_boards.mph);

            led_grn = !led_grn;
/*
Handbrake more sensibly implemented on STM3_ESC boards ?

            if  (slider.state == PARK)   {
                if  (My_STM3_ESC_boards.mph > LOCO_HANDBRAKE_ESCAPE_SPEED / 4.0)    {
                    slider.handbrake_effort *= 1.1;
                    if  (slider.handbrake_effort > 0.55)    slider.handbrake_effort = 0.55;
                    set_run_mode    (PARK);
                    pc.printf   ("Handbrake slipping, effort %.2f\r\n", slider.handbrake_effort);
                }
                if  (My_STM3_ESC_boards.mph < 0.02)    {
                    slider.handbrake_effort *= 0.9;
                    if  (slider.handbrake_effort < 0.05)    slider.handbrake_effort = 0.05;
                    set_run_mode    (PARK);
                    pc.printf   ("Handbrake not slipping, effort %.2f\r\n", slider.handbrake_effort);
                }
            }
*/
            My_STM3_ESC_boards.request_mph   (); //  issues "'n'rpm\r", takes care of cycling through available boards in sequence
//            switch  (qtr_sec)   {   //  Can do sequential stuff quarter second apart here
//            }   //  End of switch qtr_sec
            qtr_sec++;
            //  Can do stuff once per second here
            if(qtr_sec > 3) {
                qtr_sec = 0;
                seconds++;
                if  (seconds > 59)  {
                    seconds = 0;
                    minutes++;
                    //  do once per minute stuff here
                    Controller_Error.report_any ();
                }   //  fall back into once per second
                if  (seconds & 1)
                    Speedo.LED  (0, LCD_COLOR_DARKGRAY);
                else
                    Speedo.LED  (0, LCD_COLOR_RED);
                My_STM3_ESC_boards.message  ("kd\r");       //  Kick the WatchDog timers in the Twin BLDC drive boards
                recent_distance += (My_STM3_ESC_boards.mph * (111.76 * 4.0));    //  Convert mph to distance mm travelled in one second
                uint32_t    new_metres = ((uint32_t)recent_distance) / 1000;
                recent_distance -= (double)(new_metres * 1000);
                if  (!odometer_update (new_metres, (uint16_t)electrical_power_Watt, (uint16_t)(volts * 500.0))) {
                    pc.printf   ("Probs with odometer_update");
                    Controller_Error.set    (FAULT_ODOMETER, 1);
                }
                rewrite_odometer    ()  ;   //  Update text on speedo dial face
            }   //  endof if(qtr_sec > 3
        }       //  endof if  (qtrsec_trig == true)  {
    }           //  endof while(1) main programme loop
}               //  endof main ()