Diff: main.cpp

Revision:

7:3b1f44cd4735

Parent:

5:21a8ac83142c

Child:

8:5945d506a872

--- a/main.cpp	Tue May 01 08:34:36 2018 +0000
+++ b/main.cpp	Wed May 09 15:42:43 2018 +0000
@@ -22,6 +22,7 @@
 #include "TS_DISCO_F746NG.h"
 #include "LCD_DISCO_F746NG.h"
 #include <cctype>
+#include <cerrno>
 
 LCD_DISCO_F746NG    lcd;
 TS_DISCO_F746NG     touch_screen;
@@ -34,8 +35,8 @@
 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);  //  another horn
-DigitalOut  I_sink3          (D3);
+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
@@ -56,12 +57,9 @@
 AsyncSerial  com (A4, A5);   //  Com port to opto isolators on Twin BLDC Controller boards
 //AsyncSerial  ir  (D1, D0);    //  Second port does work, but gives the old broken-up display flicker nonsense problem
 
-/*
-Speed now read via opto serial ports
-*/
-
 Servo   servo1    (D6);     //  Now used to adjust Honda speed
 
+extern  uint32_t    odometer_out  ()    ;   //  Read latest total of metres travelled ever
 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
 
@@ -78,6 +76,11 @@
 extern  void    vm_set  ()  ;
 extern  void    update_meters  (double, double, double)  ;
 
+extern  void    setup_pccom ()  ;
+extern  void    setup_lococom ()  ;
+extern  void    clicore (struct parameters & a) ;
+extern  struct  parameters pccom, lococom;
+
 static  const   int
     DAMPER_DECAY        = 42,   //  Small num -> fast 'viscous damper' on dead-mans function with finger removed from panel
     MAF_PTS             = 140,      //  Moving Average Filter points. Filters reduce noise on volatage and current readings
@@ -99,55 +102,80 @@
 class   speed_2018
 {
     static const int    SPEED_AVE_PTS   = 5;    //  AVE_PTS - points in moving average filters
-    uint32_t    speed_maf_mem   [(SPEED_AVE_PTS + 1) * 2][NUMBER_OF_MOTORS],
-                axle_total  [4],
-                mafptr;
-    double      average_rpm_out;
+    uint32_t    speed_maf_mem   [(SPEED_AVE_PTS + 1) * 2][8],   //  Allow for up to 8 axles
+                axle_total  [8],    //  allow up to 8 axles
+                mafptr,
+                board_IDs   [4],    //  allow up to 4 boards
+                board_count,
+                b, reqno;
 public:
     speed_2018  ()  {   //  Constructor
         memset(speed_maf_mem, 0, sizeof(speed_maf_mem));
-        axle_total[0] = axle_total[1] = axle_total[2] = axle_total[3] = 0;
+        for (int i = 0; i < sizeof(axle_total) / sizeof(uint32_t); i++)
+            axle_total[i] = 0;
         mafptr = 0;
-        average_rpm_out = 0.0;
+        board_count = 0;
+        b = 0;
+        reqno = 0;
     }
+    bool    request_rpm ()  ;
     void    rpm_update(struct parameters & a)  ;
+    void    set_board_IDs (uint32_t *)   ;
     double  mph ()  ;
 }
-   speed2  ;
+   speed  ;
+
+void    speed_2018::set_board_IDs (uint32_t * a)   {
+    board_count = 0;
+    while   (a[board_count])  {
+        board_IDs[board_count] = a[board_count];
+        board_count++;
+    }
+    pc.printf   ("set_board_IDs %d\r\n", board_count);
+}
 
 double  speed_2018::mph ()  {
-    int t[4] = {0,0,0,0};
+    if  (!board_count)  {
+//        pc.printf   ("No boards\r\n");
+        return  0.0;
+    }
+    int t[8] = {0,0,0,0,0,0,0,0};
     for (int i = 0; i < SPEED_AVE_PTS; i++) {
-        t[0] += speed_maf_mem[i][0];
-        t[1] += speed_maf_mem[i][1];
-        t[2] += speed_maf_mem[i][2];
-        t[3] += speed_maf_mem[i][3];
+        for (int j = 0; j < board_count * 2; j++)
+            t[j] += speed_maf_mem[i][j];
     }
     int j = 0;
-    for (int i = 0; i < 4; i++) {
+    for (int i = 0; i < board_count * 2; i++) {
         j += t[i];
         axle_total[i] = t[i];
     }
-    return  (rpm2mph * ((double) j) / (SPEED_AVE_PTS * 4));
+    return  (rpm2mph * ((double) j) / (SPEED_AVE_PTS * board_count * 2));
+}
+
+bool    speed_2018::request_rpm ()  {   //  Issue "'n'rpm\r" to BLDC board to request RPM
+    if  (board_IDs[0] == 0)
+        return  false;      //  No boards identified
+    if  (board_IDs[reqno] == 0)
+        reqno = 0;
+    com.putc    (board_IDs[reqno++]);
+    com.printf  ("rpm\r");
+    return  true;
 }
 
 void    speed_2018::rpm_update(struct parameters & a)  {    //  Puts new readings into mem
-    if  (a.gp_i == 0)   {
-        speed_maf_mem   [mafptr][0] = (uint32_t)a.dbl[0];
-        speed_maf_mem   [mafptr][1] = (uint32_t)a.dbl[1];
-    }
-    else    {
-        speed_maf_mem   [mafptr][2] = (uint32_t)a.dbl[0];
-        speed_maf_mem   [mafptr][3] = (uint32_t)a.dbl[1];
+    speed_maf_mem   [mafptr][b++] = (uint32_t)a.dbl[0];
+    speed_maf_mem   [mafptr][b++] = (uint32_t)a.dbl[1];
+    if  ((b + 1) >= (board_count * 2))    {
+        b = 0;
         mafptr++;
-        if  (mafptr >= SPEED_AVE_PTS)
+        if  (mafptr >= SPEED_AVE_PTS)   
             mafptr = 0;
     }
 }
 
 
 void    rpm_push    (struct parameters & a)  {  //  Latest RPM reports from Dual BLDC Motor Controllers arrive here
-    speed2.rpm_update   (a);
+    speed.rpm_update   (a);
 //    pc.printf   ("RPM%d %d, mph %.1f\r\n", (int)a.dbl[0], (int)a.dbl[1], speed2.mph());
 }
 
@@ -161,36 +189,27 @@
         p = 1.0;
     last_V = p;
     com.printf  ("v%d\r", (int)(p * 99.0));
-//    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 on pin D12 inverted motor pwm
 }
 
 void    set_I_limit (double p)     //  Sets max motor current
 {
-//    int a;
     if  (p < 0.0)
         p = 0.0;
     if  (p > 1.0)
         p = 1.0;
     last_I = p;     //  New 30/4/2018 ; no use for this yet, included to be consistent with V
     com.printf  ("i%d\r", (int)(p * 99.0));
-//    a = (int)(p * MAX_PWM_TICKS);
-//    if  (a > MAX_PWM_TICKS)
-//        a = MAX_PWM_TICKS;
-//    if  (a < 0)
-//        a = 0;
-//    maxi.pulsewidth_ticks  (a);  //  PWM output on pin D12 inverted motor pwm
 }
 
 double  read_ammeter ()
 {
-    int a = 0;
+    int32_t a = 0;
     for (int b = 0; b < MAF_PTS; b++)
         a += I_maf[b];
     a /= MAF_PTS;
-    double i = (double) a;
-    return  (i * 95.0 / 32768.0) - 95.0 + 0.46;  //  fiddled to suit current module
+    a -= 0x4000;
+    double i = (double) (0 - a);
+    return  i / 200.0;      //  Fiddled to get current reading close enough
 }
 
 double  read_voltmeter   ()
@@ -200,7 +219,6 @@
         a += V_maf[b];
     a /= MAF_PTS;
     double v = (double) a;
-//    return  (v / 940.0) + 0.3;  //  fiddled to suit resistor values
     return  (v / 932.0) + 0.0;  //  fiddled to suit resistor values
 }
 
@@ -245,8 +263,8 @@
     trigger_current_read = false;
     int ch;
     ch++;
-    I_maf[maf_ptr] = ht_amps_ain.read_u16();
-    V_maf[maf_ptr] = ht_volts_ain.read_u16();
+    I_maf[maf_ptr] = ht_amps_ain.read_u16();    //  Read ACS709 ammeter module
+    V_maf[maf_ptr] = ht_volts_ain.read_u16();   //  Read system voltage
     maf_ptr++;
     if  (maf_ptr > MAF_PTS - 1)
         maf_ptr = 0;
@@ -314,20 +332,53 @@
 }
 
 
-extern  void    setup_pccom ()  ;
-extern  void    setup_lococom ()  ;
-extern  void    clicore (struct parameters & a) ;
-extern  struct  parameters pccom, lococom;
+void    lights  (int onoff)  {
+    I_sink2 = onoff;    //  lamp right
+    I_sink3 = onoff;    //  lamp left
+}
 
 int main()
 {
+    errno = 0;
+    int qtr_sec = 0, seconds = 0, minutes = 0;
+    double  electrical_power_Watt = 0.0;
+    ky_bd   kybd_a, kybd_b;
+    memset  (&kybd_a, 0, sizeof(kybd_a));
+    memset  (&kybd_b, 0, sizeof(kybd_b));
+    pc.baud (9600);
+    com.baud (19200);
+    pc.printf   ("\r\n\n\nLoco_TS_2018 Loco Controller starting, testing qspi mem, errno %d\r\n", errno);
+//    ir.baud (1200);
+    pc.printf   ("Ln 352 errno %d\r\n", errno);
+
+    I_sink1 = 0;    //  turn outputs off
+    I_sink2 = 0;    //  lamp right
+    pc.printf   ("Ln 355 errno %d\r\n", errno);
+    I_sink3 = 0;    //  lamp left
+    I_sink4 = 0;
+    I_sink5 = 0;
+    pc.printf   ("Ln 358 errno %d\r\n", errno);
+//    spareio_d8.mode (PullUp); now output driving throttle servo
+//    spareio_d9.mode (PullUp);
+    spareio_d10.mode(PullUp);
+
+    Ticker  tick250us;
+
+    pc.printf   ("Ln 365 errno %d\r\n", errno);
+//  Setup User Interrupt Vectors
+    tick250us.attach_us (&ISR_current_reader, 250);    //  count 125 of these to trig 31.25ms
+//    tick32ms.attach_us  (&ISR_tick_32ms, 32001);
+//    tick32ms.attach_us  (&ISR_tick_32ms, 31250);    //  then count 8 pulses per 250ms
+//    tick250ms.attach_us (&ISR_tick_250ms, 250002);
 #ifdef  QSPI
 
 extern  int qspifindfree    (uint8_t* dest, uint32_t addr)  ;
 extern  int ask_QSPI_OK ()  ;
 extern  bool    qspimemcheck    ()  ;
 extern  int qspiinit    ()  ;
+    pc.printf   ("Before ask_QSPI_OK errno %d\r\n", errno);
     int qspi_ok = ask_QSPI_OK   ();
+    pc.printf   ("After ask_QSPI_OK errno %d\r\n", errno);
 //extern  int qspieraseblock  (uint32_t addr)  ;
 //extern  int qspiwr  (uint8_t* src, uint32_t addr) ;
 //extern  int qspiwr  (uint8_t* src, uint32_t addr, uint32_t len) ;
@@ -352,6 +403,7 @@
 //    pc.printf("\n\nQSPI demo started\r\n");
 
     // Check initialization
+    pc.printf   ("errno %d, qspi mem ", errno);
     if (qspiinit() != qspi_ok)
       error("Initialization FAILED\r\n");
     else
@@ -394,51 +446,22 @@
 */
 //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
 
-    pc.printf   ("\r\r\r\n");
+    pc.printf   ("End of qspi setup, errno %d\r\n", errno);
 #endif
 
-
-    int qtr_sec = 0, seconds = 0, minutes = 0;
-    double  electrical_power_Watt = 0.0;
-    ky_bd   kybd_a, kybd_b;
-    memset  (&kybd_a, 0, sizeof(kybd_a));
-    memset  (&kybd_b, 0, sizeof(kybd_b));
-
-//    spareio_d8.mode (PullUp); now output driving throttle servo
-//    spareio_d9.mode (PullUp);
-    spareio_d10.mode(PullUp);
-
-    Ticker  tick250us;
-
-//  Setup User Interrupt Vectors
-    tick250us.attach_us (&ISR_current_reader, 250);    //  count 125 of these to trig 31.25ms
-//    tick32ms.attach_us  (&ISR_tick_32ms, 32001);
-//    tick32ms.attach_us  (&ISR_tick_32ms, 31250);    //  then count 8 pulses per 250ms
-//    tick250ms.attach_us (&ISR_tick_250ms, 250002);
-    pc.baud (9600);
-    com.baud (19200);
-//    ir.baud (1200);
-    
-    I_sink1 = 0;    //  turn outputs off
-    I_sink2 = 0;
-    I_sink3 = 0;
-    I_sink4 = 0;
-    I_sink5 = 0;
-    if  (f_r_switch)
+    if  (f_r_switch)    {
         slider.direction = FWD; //  make decision from key switch position here
-    else
+        com.printf  ("fw\r");
+    }
+    else    {
         slider.direction = REV; //  make decision from key switch position here
-
+        com.printf  ("re\r");
+    }
 //    max_pwm_ticks   = SystemCoreClock / (2 * PWM_HZ);  //  prescaler min value is 2, or so it would seem. SystemCoreClock returns 216000000 on F746NG board
 //    maxv.period_ticks      (MAX_PWM_TICKS + 1);  //  around 18 kHz
 //    maxi.period_ticks      (MAX_PWM_TICKS + 1);
     set_I_limit (0.0);
     set_V_limit (0.0);
-    if  (slider.direction)
-        com.printf  ("fw\r");
-    else
-        com.printf  ("re\r");
-//    com.printf  ("rvi22 55\rkd\r");
     throttle    (0.0);      //  Set revs to idle. Start engine and warm up before powering up control
     setup_pccom   ();
     setup_lococom ();
@@ -472,13 +495,15 @@
 
     vm_set();   //  Draw 3 analogue meter movements, speedo, voltmeter, ammeter
 
-//    odometer_zero   ();
-//    pc.printf   ("Back from odometer_zero\r\n");
+//    if  (odometer_zero   ())
+//        pc.printf   ("TRUE from odometer_zero\r\n");
+//    else
+//        pc.printf   ("FALSE from odometer_zero\r\n");
     double  torque_req = 0.0;
     bool    toggle32ms = false;
     //  Main loop
 
-    int boards_fitted[8], bfptr = 0;
+    uint32_t boards_fitted[8], bfptr = 0;
     for (int i = 0; i < 8; i++)
         boards_fitted[i] = 0;
     sys_timer_32Hz  = 0;
@@ -506,13 +531,22 @@
             }
         }
     }
+
+//    boards_fitted[0] = '4';
+//    boards_fitted[1] = '5';
+
     while   (boards_fitted[bfptr])    { //  This works, identified BLDC Motor Controller board ID chars '0' to '9' listed in boards_fitted[]
         pc.printf   ("Board %c found\r\n", boards_fitted[bfptr++]);
     }
+    speed.set_board_IDs   (boards_fitted);    //  store number and IDs of Dual BLDC boards identified
+    bfptr = 0;
     clicore (pccom);
     pc.printf   ("pcmenuitems %d, commenuitems %d\r\n", pccom.numof_menu_items, lococom.numof_menu_items);
+//    perror  (strerror(errno));
     //  Done setup, time to roll !
     
+    lights  (1);    //  Headlights ON!
+
     while (1) {
 
         struct ky_bd * present_kybd, * previous_kybd;
@@ -524,7 +558,7 @@
 
         if  (trigger_32ms == true)  {       //  Stuff to do every 32 milli secs
             trigger_32ms = false;
-        clicore (lococom);
+            clicore (lococom);
             toggle32ms = !toggle32ms;
             if  (toggle32ms)    {
                 present_kybd = &kybd_a;
@@ -595,7 +629,7 @@
                                 break;
                         }   //  endof switch (k)
                     }       //  endof if (inlist(*present2, k)) {
-                    if  (inlist(*present_kybd, k) && !inlist(*previous_kybd, k))    {
+                    if  (inlist(*present_kybd, k) && !inlist(*previous_kybd, k))    {   //  New key press detected
                         pc.printf   ("Handle Press %d\r\n", k);
                         draw_button_hilight     (k, LCD_COLOR_YELLOW)  ;
                         switch  (k)    {    //  Handle new touch screen button presses here - single action per press, not autorepeat
@@ -603,10 +637,12 @@
                                 pc.printf   ("Speedometer key pressed %d\r\n", k);
                                 break;
                             case    VMETER_BUT:  //
-                                pc.printf   ("Voltmeter key pressed %d\r\n", k);
+//                                pc.printf   ("Voltmeter key pressed %d\r\n", k);
+                                I_sink5 = 1;    //  Turn on hi-horn
                                 break;
                             case    AMETER_BUT:  //
-                                pc.printf   ("Ammeter key pressed %d\r\n", k);
+//                                pc.printf   ("Ammeter key pressed %d\r\n", k);
+                                I_sink4 = 1;    //  Turn on lo-horn
                                 break;
                             default:
                                 pc.printf   ("Unhandled keypress %d\r\n", k);
@@ -620,6 +656,22 @@
                     if  (inlist(*previous_kybd, k) && !inlist(*present_kybd, k))    {
                         pc.printf   ("Handle Release %d\r\n", k);
                         draw_button_hilight     (k, LCD_COLOR_DARKBLUE)  ;
+                        switch  (k)    {    //  Handle new touch screen button RELEASes here - single action per press, not autorepeat
+                            case    SPEEDO_BUT:  //
+                                pc.printf   ("Speedometer key released %d\r\n", k);
+                                break;
+                            case    VMETER_BUT:  //
+                                I_sink5 = 0;    //  Turn hi-tone horn off
+//                                pc.printf   ("Voltmeter key released %d\r\n", k);
+                                break;
+                            case    AMETER_BUT:  //
+                                I_sink4 = 0;    //  Turn lo-tone horn off
+//                                pc.printf   ("Ammeter key released %d\r\n", k);
+                                break;
+                            default:
+                                pc.printf   ("Unhandled keyreleas %d\r\n", k);
+                                break;
+                        }       //  endof   switch  (button)
                     }
                 }       //  endof while - handle new key releases
             }   //  endof at least one key pressed this time or last time
@@ -635,7 +687,6 @@
             if  (slider.recalc_run) {   //  range of slider.position in RUN mode is min_pos_() to NEUTRAL_VAL - 1
                 slider.recalc_run = false;  //  All RUN power and pwm calcs done here
                 int b = slider.position;
-//                double  torque_req;   //  now declared above to be used as parameter for throttle
                 if  (b > NEUTRAL_VAL)
                     b = NEUTRAL_VAL;
                 if  (b < MIN_POS)   //  if finger position is above top of slider limit
@@ -645,12 +696,12 @@
                 torque_req /= (NEUTRAL_VAL - MIN_POS);  //  in range 0.0 to 1.0
                 pc.printf   ("torque_rec = %.3f, last_V = %.3f\r\n", torque_req, last_V);
                 set_I_limit (torque_req);
-                
-                throttle    (torque_req);   //  Think about moderating this and including speed related element
-                
-                if  (torque_req < 0.05)
+                if  (torque_req < 0.05) {
                     set_V_limit (last_V / 2.0);
+                    throttle    (torque_req * 6.0);
+                }
                 else    {
+                    throttle    (0.3 + (torque_req / 2.0));
                     if  (last_V < 0.99)
                         set_V_limit (last_V + 0.05);   //  ramp voltage up rather than slam to max
                 }
@@ -659,16 +710,7 @@
 
         if  (qtrsec_trig == true)  {    //  do every quarter second stuff here
             qtrsec_trig = false;
-//            speed.qtr_sec_update  ();
-//            double  speedmph = speed.MPH(), amps = 0.0 - read_ammeter(), volts = read_voltmeter();
-            double  speedmph = speed2.mph(), amps = 0.0 - read_ammeter(), volts = read_voltmeter();
-//static const double mph_2_mm_per_sec = 447.04;  //  exact
-//            double  mm_travelled_in_qtrsec = speedmph * mph_2_mm_per_sec / 4.0;
-//  1.0 mph = 0.44704 metre per sec
-//          = 0.11176 metre per quarter sec
-
-            recent_distance += (speedmph * 111.76);    //  Convert mph to distance mm travelled in quarter second
-
+            double  speedmph = speed.mph(), amps = read_ammeter(), volts = read_voltmeter();
             slider.loco_speed = speedmph;
             electrical_power_Watt = volts * amps;   //  visible throughout main
             update_meters  (speedmph, electrical_power_Watt, volts)  ;   //  displays speed, volts and power (volts times amps)
@@ -687,21 +729,9 @@
                     pc.printf   ("Handbrake not slipping, effort %.2f\r\n", slider.handbrake_effort);
                 }
             }
-            switch  (qtr_sec)   {   //  Can do sequential stuff quarter second apart here
-                case    0:
-                case    2:
-                    com.putc    (boards_fitted[0]);
-                    com.printf  ("rpm\r");
-                    break;
-                case    1:
-                case    3:
-                    com.putc    (boards_fitted[1]);
-                    com.printf  ("rpm\r");
-                    break;
-                default:
-                    qtr_sec = 0;
-                    break;
-            }   //  End of switch qtr_sec
+            speed.request_rpm   (); //  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) {
@@ -714,10 +744,23 @@
                     //  do once per minute stuff here
                 }   //  fall back into once per second
 #ifdef  QSPI
+
+//            recent_distance += 300;
+
+            recent_distance += (speedmph * (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");
+            char    dist[20];
+            sprintf (dist, "%05d m", odometer_out());
+            displaytext (236, 226, 2, dist);
+
 /*
     Odometer Update stuff now needs fixing to take updagte in form of mm travelled in previous period
     
 //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
+bool    distance_measurement::update  (uint32_t new_metres_travelled, uint16_t powr, uint16_t volt)  {
                 if  (!odometer_update (historic_distance, (uint16_t)electrical_power_Watt, (uint16_t)(volts * 500.0)))
                     pc.printf   ("Probs with odometer_update");
                 char    dist[20];