Merge pull request #150 from Candas1/master

Improvements and fixes

Inc/config.h

@@ -58,7 +58,13 @@
 #define ADC_TOTAL_CONV_TIME     (ADC_CLOCK_DIV * ADC_CONV_CLOCK_CYCLES) // = ((SystemCoreClock / ADC_CLOCK_HZ) * ADC_CONV_CLOCK_CYCLES), where ADC_CLOCK_HZ = SystemCoreClock/ADC_CLOCK_DIV
 // ########################### END OF  DO-NOT-TOUCH SETTINGS ############################
 
-
+// ############################### BOARD VARIANT ###############################
+/* Board Variant
+ * 0 - Default board type
+ * 1 - Alternate board type with different pin mapping for DCLINK, Buzzer and ON/OFF, Button and Charger
+*/
+#define BOARD_VARIANT           0         // change if board with alternate pin mapping
+// ######################## END OF BOARD VARIANT ###############################
 
 // ############################### BATTERY ###############################
 /* Battery voltage calibration: connect power source.
@@ -293,6 +299,7 @@
     #define DEBUG_SERIAL_USART3           // right sensor board cable, disable if I2C (nunchuk or lcd) is used!
   #endif
 
+  // #define ADC_ALTERNATE_CONNECT           // use to swap ADC inputs
   // #define SUPPORT_BUTTONS_LEFT            // use left sensor board cable for button inputs.  Disable DEBUG_SERIAL_USART2!
   // #define SUPPORT_BUTTONS_RIGHT           // use right sensor board cable for button inputs. Disable DEBUG_SERIAL_USART3!
 #endif
@@ -499,6 +506,7 @@
 
   #define SPEED_COEFFICIENT       16384     // 1.0f
   #define STEER_COEFFICIENT       8192      // 0.5f Only active in Sideboard input
+  // #define ADC_ALTERNATE_CONNECT             // use to swap ADC inputs
   // #define INVERT_R_DIRECTION                // Invert rotation of right motor
   // #define INVERT_L_DIRECTION                // Invert rotation of left motor
   // #define DEBUG_SERIAL_USART3               // right sensor board cable, disable if I2C (nunchuk or lcd) is used!

Inc/defines.h

@@ -102,28 +102,56 @@
 
 // #define DCLINK_ADC ADC3
 // #define DCLINK_CHANNEL
+
+#if BOARD_VARIANT == 0
 #define DCLINK_PIN GPIO_PIN_2
 #define DCLINK_PORT GPIOC
+#elif BOARD_VARIANT == 1
+#define DCLINK_PIN GPIO_PIN_1
+#define DCLINK_PORT GPIOA
+#endif
+
 // #define DCLINK_PULLUP 30000
 // #define DCLINK_PULLDOWN 1000
 
 #define LED_PIN GPIO_PIN_2
 #define LED_PORT GPIOB
 
+#if BOARD_VARIANT == 0
 #define BUZZER_PIN GPIO_PIN_4
 #define BUZZER_PORT GPIOA
+#elif BOARD_VARIANT == 1
+#define BUZZER_PIN GPIO_PIN_13
+#define BUZZER_PORT GPIOC
+#endif
 
-#define SWITCH_PIN GPIO_PIN_1
+// UNUSED/REDUNDANT
-#define SWITCH_PORT GPIOA
+//#define SWITCH_PIN GPIO_PIN_1
+//#define SWITCH_PORT GPIOA
 
+#if BOARD_VARIANT == 0
 #define OFF_PIN GPIO_PIN_5
 #define OFF_PORT GPIOA
+#elif BOARD_VARIANT == 1
+#define OFF_PIN GPIO_PIN_15
+#define OFF_PORT GPIOC
+#endif
 
+#if BOARD_VARIANT == 0
 #define BUTTON_PIN GPIO_PIN_1
 #define BUTTON_PORT GPIOA
+#elif BOARD_VARIANT == 1
+#define BUTTON_PIN GPIO_PIN_9
+#define BUTTON_PORT GPIOB
+#endif
 
+#if BOARD_VARIANT == 0
 #define CHARGER_PIN GPIO_PIN_12
 #define CHARGER_PORT GPIOA
+#elif BOARD_VARIANT == 1
+#define CHARGER_PIN GPIO_PIN_11
+#define CHARGER_PORT GPIOA
+#endif
 
 #if defined(CONTROL_PPM_LEFT)
 #define PPM_PIN             GPIO_PIN_3

Src/bldc.c

@@ -39,13 +39,14 @@ extern RT_MODEL *const rtM_Right;
 extern DW   rtDW_Left;                  /* Observable states */
 extern ExtU rtU_Left;                   /* External inputs */
 extern ExtY rtY_Left;                   /* External outputs */
+extern P    rtP_Left;
 
 extern DW   rtDW_Right;                 /* Observable states */
 extern ExtU rtU_Right;                  /* External inputs */
 extern ExtY rtY_Right;                  /* External outputs */
 // ###############################################################################
 
-static int16_t pwm_margin = 110;        /* This margin allows to always have a window in the PWM signal for proper Phase currents measurement */
+static int16_t pwm_margin;              /* This margin allows to have a window in the PWM signal for proper FOC Phase currents measurement */
 
 extern uint8_t ctrlModReq;
 static int16_t curDC_max = (I_DC_MAX * A2BIT_CONV);
@@ -60,7 +61,7 @@ extern volatile adc_buf_t adc_buffer;
 uint8_t buzzerFreq          = 0;
 uint8_t buzzerPattern       = 0;
 uint8_t buzzerCount         = 0;
-static uint32_t buzzerTimer = 0;
+volatile uint32_t buzzerTimer = 0;
 static uint8_t  buzzerPrev  = 0;
 static uint8_t  buzzerIdx   = 0;
 
@@ -146,6 +147,13 @@ void DMA1_Channel1_IRQHandler(void) {
       buzzerPrev = 0;
   }
 
+  // Adjust pwm_margin depending on the selected Control Type
+  if (rtP_Left.z_ctrlTypSel == FOC_CTRL) {
+    pwm_margin = 110;
+  } else {
+    pwm_margin = 0;
+  }
+
   // ############################### MOTOR CONTROL ###############################
 
   int ul, vl, wl;

Src/comms.c

@@ -59,8 +59,14 @@ extern uint16_t VirtAddVarTab[NB_OF_VAR];
 extern int16_t speedAvg;                      // average measured speed
 extern int16_t speedAvgAbs;                   // average measured speed in absolute
 extern uint8_t ctrlModReqRaw;
-extern adc_buf_t adc_buffer;
+extern int16_t batVoltageCalib;
 extern int16_t board_temp_deg_c;
+extern int16_t left_dc_curr;
+extern int16_t right_dc_curr;
+extern int16_t dc_curr;
+extern int16_t cmdL; 
+extern int16_t cmdR; 
+
 
 
 enum commandTypes {READ,WRITE};
@@ -123,15 +129,19 @@ const parameter_entry params[] = {
 #endif  
   // FEEDBACK
   // Type       ,Name                 ,Datatype, ValueL ptr                  ,ValueR                    ,EEPRM Addr ,Init              Int/Ext ,Min    ,Max    ,Div             ,Mul  ,Fix   ,Callback Function  ,Help text
-    {VARIABLE   ,"I_DC_LINK"          ,ADD_PARAM(rtU_Left.i_DCLink)          ,&rtU_Right.i_DCLink       ,0          ,0                 ,0      ,0      ,0      ,A2BIT_CONV      ,100  ,0     ,NULL               ,"DC Link current A *100"},
+    {VARIABLE   ,"DC_CURR"            ,ADD_PARAM(dc_curr)                    ,NULL                      ,0          ,0                 ,0      ,0      ,0      ,0               ,0    ,0     ,NULL               ,"Total DC Link current A *100"},
+    {VARIABLE   ,"RDC_CURR"           ,ADD_PARAM(right_dc_curr)              ,NULL                      ,0          ,0                 ,0      ,0      ,0      ,0               ,0    ,0     ,NULL               ,"Right DC Link current A *100"},
+    {VARIABLE   ,"LDC_CURR"           ,ADD_PARAM(left_dc_curr)               ,NULL                      ,0          ,0                 ,0      ,0      ,0      ,0               ,0    ,0     ,NULL               ,"Left DC Link current A *100"},
+    {VARIABLE   ,"CMDL"               ,ADD_PARAM(cmdL)                       ,NULL                      ,0          ,0                 ,0      ,0      ,0      ,0               ,0    ,0     ,NULL               ,"Left Motor Command"},
+    {VARIABLE   ,"CMDR"               ,ADD_PARAM(cmdR)                       ,NULL                      ,0          ,0                 ,0      ,0      ,0      ,0               ,0    ,0     ,NULL               ,"Right Motor Command"},
     {VARIABLE   ,"SPD_AVG"            ,ADD_PARAM(speedAvg)                   ,NULL                      ,0          ,0                 ,0      ,0      ,0      ,0               ,0    ,0     ,NULL               ,"Motor Measured Avg RPM"},
     {VARIABLE   ,"SPDL"               ,ADD_PARAM(rtY_Left.n_mot)             ,NULL                      ,0          ,0                 ,0      ,0      ,0      ,0               ,0    ,0     ,NULL               ,"Left Motor Measured RPM"},
     {VARIABLE   ,"SPDR"               ,ADD_PARAM(rtY_Right.n_mot)            ,NULL                      ,0          ,0                 ,0      ,0      ,0      ,0               ,0    ,0     ,NULL               ,"Right Motor Measured RPM"},
     {VARIABLE   ,"RATE"               ,0       , NULL                        ,NULL                      ,0          ,RATE              ,0      ,0      ,0      ,0               ,0    ,4     ,NULL               ,"Rate *10"},
     {VARIABLE   ,"SPD_COEF"           ,0       , NULL                        ,NULL                      ,0          ,SPEED_COEFFICIENT ,0      ,0      ,0      ,0               ,10   ,14    ,NULL               ,"Speed Coefficient *10"},
     {VARIABLE   ,"STR_COEF"           ,0       , NULL                        ,NULL                      ,0          ,STEER_COEFFICIENT ,0      ,0      ,0      ,0               ,10   ,14    ,NULL               ,"Steer Coefficient *10"},
-    {VARIABLE   ,"BATV"               ,ADD_PARAM(adc_buffer.batt1)           ,NULL                      ,0          ,0                 ,0      ,0      ,0      ,0               ,0    ,0     ,NULL               ,"Battery voltage *100"},       
+    {VARIABLE   ,"BATV"               ,ADD_PARAM(batVoltageCalib)            ,NULL                      ,0          ,0                 ,0      ,0      ,0      ,0               ,0    ,0     ,NULL               ,"Calibrated Battery voltage *100"},       
-  //{VARIABLE   ,"TEMP"               ,ADD_PARAM(board_temp_deg_c)           ,NULL                      ,0          ,0                 ,0      ,0      ,0      ,0               ,0    ,0     ,NULL               ,"Temperature °C *10"},       
+    {VARIABLE   ,"TEMP"               ,ADD_PARAM(board_temp_deg_c)           ,NULL                      ,0          ,0                 ,0      ,0      ,0      ,0               ,0    ,0     ,NULL               ,"Calibrated Temperature °C *10"},       
 
 };
 

Src/main.c

@@ -61,6 +61,8 @@ extern P    rtP_Left;                   /* Block parameters (auto storage) */
 extern P    rtP_Right;                  /* Block parameters (auto storage) */
 extern ExtY rtY_Left;                   /* External outputs */
 extern ExtY rtY_Right;                  /* External outputs */
+extern ExtU rtU_Left;                   /* External inputs */
+extern ExtU rtU_Right;                  /* External inputs */
 //---------------
 
 extern uint8_t     inIdx;               // input index used for dual-inputs
@@ -101,7 +103,15 @@ extern volatile uint16_t pwm_captured_ch2_value;
 // Global variables set here in main.c
 //------------------------------------------------------------------------
 uint8_t backwardDrive;
+extern volatile uint32_t buzzerTimer;
 volatile uint32_t main_loop_counter;
+int16_t batVoltageCalib;         // global variable for calibrated battery voltage
+int16_t board_temp_deg_c;        // global variable for calibrated temperature in degrees Celsius
+int16_t left_dc_curr;            // global variable for Left DC Link current 
+int16_t right_dc_curr;           // global variable for Right DC Link current
+int16_t dc_curr;                 // global variable for Total DC Link current 
+int16_t cmdL;                    // global variable for Left Command 
+int16_t cmdR;                    // global variable for Right Command 
 
 //------------------------------------------------------------------------
 // Local variables
@@ -148,10 +158,10 @@ static int16_t    speed;                // local variable for speed. -1000 to 10
   static int32_t  speedFixdt;           // local fixed-point variable for speed low-pass filter
 #endif
 
+static uint32_t    buzzerTimer_prev = 0;
 static uint32_t    inactivity_timeout_counter;
 static MultipleTap MultipleTapBrake;    // define multiple tap functionality for the Brake pedal
 
-
 int main(void) {
 
   HAL_Init();
@@ -192,18 +202,14 @@ int main(void) {
   poweronMelody();
   HAL_GPIO_WritePin(LED_PORT, LED_PIN, GPIO_PIN_SET);
   
-  int16_t cmdL      = 0, cmdR      = 0;
-  int16_t cmdL_prev = 0, cmdR_prev = 0;
-
   int32_t board_temp_adcFixdt = adc_buffer.temp << 16;  // Fixed-point filter output initialized with current ADC converted to fixed-point
   int16_t board_temp_adcFilt  = adc_buffer.temp;
-  int16_t board_temp_deg_c;
 
   // Loop until button is released
   while(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { HAL_Delay(10); }
 
   while(1) {
-    HAL_Delay(DELAY_IN_MAIN_LOOP);        // delay in ms
+    if (buzzerTimer - buzzerTimer_prev > 16*DELAY_IN_MAIN_LOOP) {   // 1 ms = 16 ticks buzzerTimer
 
     readCommand();                        // Read Command: input1[inIdx].cmd, input2[inIdx].cmd
     calcAvgSpeed();                       // Calculate average measured speed: speedAvg, speedAvgAbs
@@ -293,7 +299,6 @@ int main(void) {
       mixerFcn(speed << 4, steer << 4, &cmdR, &cmdL);   // This function implements the equations above
 
       // ####### SET OUTPUTS (if the target change is less than +/- 100) #######
-      if ((cmdL > cmdL_prev-100 && cmdL < cmdL_prev+100) && (cmdR > cmdR_prev-100 && cmdR < cmdR_prev+100)) {
       #ifdef INVERT_R_DIRECTION
         pwmr = cmdR;
       #else
@@ -304,7 +309,6 @@ int main(void) {
       #else
         pwml = cmdL;
       #endif
-      }
     #endif
 
     #ifdef VARIANT_TRANSPOTTER
@@ -315,7 +319,6 @@ int main(void) {
       if (nunchuk_connected == 0) {
         cmdL = cmdL * 0.8f + (CLAMP(distanceErr + (steering*((float)MAX(ABS(distanceErr), 50)) * ROT_P), -850, 850) * -0.2f);
         cmdR = cmdR * 0.8f + (CLAMP(distanceErr - (steering*((float)MAX(ABS(distanceErr), 50)) * ROT_P), -850, 850) * -0.2f);
-        if ((cmdL < cmdL_prev + 50 && cmdL > cmdL_prev - 50) && (cmdR < cmdR_prev + 50 && cmdR > cmdR_prev - 50)) {
         if (distanceErr > 0) {
           enable = 1;
         }
@@ -341,7 +344,6 @@ int main(void) {
         } else {
           enable = 0;
         }
-        }
         timeoutCntGen = 0;
         timeoutFlgGen = 0;
       }
@@ -421,6 +423,14 @@ int main(void) {
     board_temp_adcFilt  = (int16_t)(board_temp_adcFixdt >> 16);  // convert fixed-point to integer
     board_temp_deg_c    = (TEMP_CAL_HIGH_DEG_C - TEMP_CAL_LOW_DEG_C) * (board_temp_adcFilt - TEMP_CAL_LOW_ADC) / (TEMP_CAL_HIGH_ADC - TEMP_CAL_LOW_ADC) + TEMP_CAL_LOW_DEG_C;
 
+    // ####### CALC CALIBRATED BATTERY VOLTAGE #######
+    batVoltageCalib = batVoltage * BAT_CALIB_REAL_VOLTAGE / BAT_CALIB_ADC;
+
+    // ####### CALC DC LINK CURRENT #######
+    left_dc_curr  = -(rtU_Left.i_DCLink * 100) / A2BIT_CONV;   // Left DC Link Current * 100 
+    right_dc_curr = -(rtU_Right.i_DCLink * 100) / A2BIT_CONV;  // Right DC Link Current * 100
+    dc_curr       = left_dc_curr + right_dc_curr;            // Total DC Link Current * 100
+
     // ####### DEBUG SERIAL OUT #######
     #if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
       if (main_loop_counter % 25 == 0) {    // Send data periodically every 125 ms      
@@ -433,7 +443,7 @@ int main(void) {
             cmdL,                     // 3: output command: [-1000, 1000]
             cmdR,                     // 4: output command: [-1000, 1000]
             adc_buffer.batt1,         // 5: for battery voltage calibration
-            batVoltage * BAT_CALIB_REAL_VOLTAGE / BAT_CALIB_ADC, // 6: for verifying battery voltage calibration
+            batVoltageCalib,          // 6: for verifying battery voltage calibration
             board_temp_adcFilt,       // 7: for board temperature calibration
             board_temp_deg_c);        // 8: for verifying board temperature calibration
         #endif
@@ -448,7 +458,7 @@ int main(void) {
         Feedback.cmd2           = (int16_t)input2[inIdx].cmd;
         Feedback.speedR_meas	  = (int16_t)rtY_Right.n_mot;
         Feedback.speedL_meas	  = (int16_t)rtY_Left.n_mot;
-        Feedback.batVoltage	    = (int16_t)(batVoltage * BAT_CALIB_REAL_VOLTAGE / BAT_CALIB_ADC);
+        Feedback.batVoltage	    = (int16_t)batVoltageCalib;
         Feedback.boardTemp	    = (int16_t)board_temp_deg_c;
 
         #if defined(FEEDBACK_SERIAL_USART2)
@@ -512,14 +522,15 @@ int main(void) {
       poweroff();
     }
 
+
     // HAL_GPIO_TogglePin(LED_PORT, LED_PIN);                 // This is to measure the main() loop duration with an oscilloscope connected to LED_PIN
     // Update states
     inIdx_prev = inIdx;
-    cmdL_prev  = cmdL;
+    buzzerTimer_prev = buzzerTimer;
-    cmdR_prev  = cmdR;
     main_loop_counter++;
     }
   }
+}
 
 
 // ===========================================================

Src/setup.c

@@ -638,7 +638,11 @@ void MX_ADC1_Init(void) {
   sConfig.Rank    = 3;
   HAL_ADC_ConfigChannel(&hadc1, &sConfig);
 
+  #if BOARD_VARIANT == 0
   sConfig.Channel = ADC_CHANNEL_12;  // pc2 vbat
+  #elif BOARD_VARIANT == 1
+  sConfig.Channel = ADC_CHANNEL_1;   // pa1 vbat
+  #endif
   sConfig.Rank    = 4;
   HAL_ADC_ConfigChannel(&hadc1, &sConfig);
 

Src/util.c

@@ -802,8 +802,13 @@ void calcInputCmd(InputStruct *in, int16_t out_min, int16_t out_max) {
 void readInputRaw(void) {
     #ifdef CONTROL_ADC
     if (inIdx == CONTROL_ADC) {
+      #ifdef ADC_ALTERNATE_CONNECT
+        input1[inIdx].raw = adc_buffer.l_rx2;
+        input2[inIdx].raw = adc_buffer.l_tx2;
+      #else
         input1[inIdx].raw = adc_buffer.l_tx2;
         input2[inIdx].raw = adc_buffer.l_rx2;
+      #endif
     }
     #endif
 
@@ -1065,20 +1070,16 @@ void usart2_rx_check(void)
   #endif
 
   #if defined(DEBUG_SERIAL_USART2)
-  uint8_t *ptr;
+  uint8_t ptr[SERIAL_BUFFER_SIZE];
   if (pos != old_pos) {                                                 // Check change in received data
     if (pos > old_pos) {                                                // "Linear" buffer mode: check if current position is over previous one
       usart_process_debug(&rx_buffer_L[old_pos], pos - old_pos);        // Process data
     } else {                                                            // "Overflow" buffer mode
-      ptr = (uint8_t *) malloc(sizeof(uint8_t) * (rx_buffer_L_len - old_pos + pos));
+      memcpy(&ptr[0], &rx_buffer_L[old_pos], rx_buffer_L_len - old_pos);    // First copy data from the end of buffer
-      memcpy(ptr, &rx_buffer_L[old_pos], rx_buffer_L_len - old_pos);    // First copy data from the end of buffer
       if (pos > 0) {                                                    // Check and continue with beginning of buffer
-        ptr += rx_buffer_L_len - old_pos;
+        memcpy(&ptr[rx_buffer_L_len - old_pos], &rx_buffer_L[0], pos);                              // Copy remaining data
-        memcpy(ptr, &rx_buffer_L[0], pos);                              // Copy remaining data
       }
-      ptr -= rx_buffer_L_len - old_pos;
       usart_process_debug(ptr, rx_buffer_L_len - old_pos + pos);        // Process data
-      free(ptr);
     }
   }
   #endif // DEBUG_SERIAL_USART2
@@ -1141,20 +1142,17 @@ void usart3_rx_check(void)
   #endif
 
   #if defined(DEBUG_SERIAL_USART3)
-  uint8_t *ptr;
+  uint8_t ptr[SERIAL_BUFFER_SIZE];
+
   if (pos != old_pos) {                                                 // Check change in received data
     if (pos > old_pos) {                                                // "Linear" buffer mode: check if current position is over previous one
       usart_process_debug(&rx_buffer_R[old_pos], pos - old_pos);        // Process data
     } else {                                                            // "Overflow" buffer mode
-      ptr = (uint8_t *) malloc(sizeof(uint8_t) * (rx_buffer_R_len - old_pos + pos));
+      memcpy(&ptr[0], &rx_buffer_R[old_pos], rx_buffer_R_len - old_pos);    // First copy data from the end of buffer
-      memcpy(ptr, &rx_buffer_R[old_pos], rx_buffer_R_len - old_pos);    // First copy data from the end of buffer
       if (pos > 0) {                                                    // Check and continue with beginning of buffer
-        ptr += rx_buffer_R_len - old_pos;
+        memcpy(&ptr[rx_buffer_R_len - old_pos], &rx_buffer_R[0], pos);                              // Copy remaining data
-        memcpy(ptr, &rx_buffer_R[0], pos);                              // Copy remaining data
       }
-      ptr -= rx_buffer_R_len - old_pos;
       usart_process_debug(ptr, rx_buffer_R_len - old_pos + pos);        // Process data
-      free(ptr);
     }
   }
   #endif // DEBUG_SERIAL_USART3