Merge pull request #23 from hello-robot/features/prince

Features/prince
hello-robot · Oct 7, 2023 · 45bf3b2 · 45bf3b2
2 parents 4c45b68 + cc46f68
commit 45bf3b2
Show file tree

Hide file tree

Showing 52 changed files with 6,033 additions and 295 deletions.
diff --git a/arduino/hardware/hello-robot/samd/variants/hello_pimu/variant.cpp b/arduino/hardware/hello-robot/samd/variants/hello_pimu/variant.cpp
@@ -30,8 +30,8 @@ const PinDescription g_APinDescription[]=
   //{ PORTB, 22, PIO_SERCOM, (PIN_ATTR_DIGITAL), No_ADC_Channel, NOT_ON_PWM, NOT_ON_TIMER, EXTERNAL_INT_10 }, // TX1: SERCOM5/PAD[2]
     //{ PORTB, 23, PIO_DIGITAL, (PIN_ATTR_DIGITAL), No_ADC_Channel, NOT_ON_PWM, NOT_ON_TIMER, EXTERNAL_INT_NONE }, //x
     //{ PORTB, 22, PIO_DIGITAL, (PIN_ATTR_DIGITAL), No_ADC_Channel, NOT_ON_PWM, NOT_ON_TIMER, EXTERNAL_INT_NONE }, //x
-    { PORTA, 11, PIO_SERCOM, (PIN_ATTR_DIGITAL), No_ADC_Channel, NOT_ON_PWM, NOT_ON_TIMER, EXTERNAL_INT_11 }, // RX: SERCOM0/PAD[3]
-  { PORTA, 10, PIO_SERCOM, (PIN_ATTR_DIGITAL), No_ADC_Channel, NOT_ON_PWM, NOT_ON_TIMER, EXTERNAL_INT_10 }, // TX: SERCOM0/PAD[2]
+    { PORTA, 11, PIO_DIGITAL, (PIN_ATTR_DIGITAL), No_ADC_Channel, NOT_ON_PWM, NOT_ON_TIMER, EXTERNAL_INT_NONE }, //Efuse
+  { PORTA, 10, PIO_DIGITAL, (PIN_ATTR_DIGITAL), No_ADC_Channel, NOT_ON_PWM, NOT_ON_TIMER, EXTERNAL_INT_NONE }, //Charge
 
 // 2..8 - Analog pins
   // --------------------
@@ -64,7 +64,7 @@ const PinDescription g_APinDescription[]=
 { PORTA,  9, PIO_TIMER, (PIN_ATTR_DIGITAL|PIN_ATTR_PWM|PIN_ATTR_TIMER), No_ADC_Channel, PWM0_CH1, TCC0_CH1, EXTERNAL_INT_NONE },  //TCC0/WO[1]  PWM0
 { PORTA,  10, PIO_TIMER, (PIN_ATTR_DIGITAL|PIN_ATTR_PWM|PIN_ATTR_TIMER), No_ADC_Channel, PWM1_CH0, TCC1_CH0, EXTERNAL_INT_NONE }, //TCC1/WO[0] PWM1
 { PORTA,  11, PIO_TIMER, (PIN_ATTR_DIGITAL|PIN_ATTR_PWM|PIN_ATTR_TIMER), No_ADC_Channel, PWM1_CH1, TCC1_CH1, EXTERNAL_INT_NONE }, //TCC1/WO[1]  PWM2
-{ PORTA,  13, PIO_TIMER, (PIN_ATTR_DIGITAL|PIN_ATTR_PWM|PIN_ATTR_TIMER), No_ADC_Channel, PWM2_CH1, TCC2_CH1, EXTERNAL_INT_NONE }, //TCC2/WO[1]  PWM3
+{ PORTA,  13, PIO_TIMER, (PIN_ATTR_DIGITAL|PIN_ATTR_PWM|PIN_ATTR_TIMER), No_ADC_Channel, PWM2_CH1, TCC2_CH1, EXTERNAL_INT_13},//EXTERNAL_INT_NONE }, //TCC2/WO[1]  PWM3
 
 // 18..20 - SPI pins (ICSP:MISO,SCK,MOSI)
 //Note: Matches Metro M0 exactly, should be OK

diff --git a/arduino/hardware/hello-robot/samd/variants/hello_pimu/variant.h b/arduino/hardware/hello-robot/samd/variants/hello_pimu/variant.h
@@ -60,7 +60,7 @@ extern "C"
  */
 // #define digitalPinToTimer(P)
 
-// LEDs
+
 #define PIN_LED_13           (24u)
 #define PIN_LED              PIN_LED_13
 #define LED_BUILTIN          PIN_LED_13
@@ -79,6 +79,8 @@ extern "C"
 #define PIN_A4               (PIN_A0 + 4)
 #define PIN_A5               (PIN_A0 + 5)
 #define PIN_A6               (PIN_A0 + 6)
+#define PIN_A7               (0ul)
+#define PIN_A8               (1u1)
 
 static const uint8_t A0  = PIN_A0;
 static const uint8_t A1  = PIN_A1;
@@ -88,6 +90,7 @@ static const uint8_t A4  = PIN_A4;
 static const uint8_t A5  = PIN_A5;
 static const uint8_t A6  = PIN_A6 ;
 
+
 #define ADC_RESOLUTION		12
 
 #define PIN_D0               (21ul)

diff --git a/arduino/hardware/hello-robot/samd/variants/hello_stepper/variant.h b/arduino/hardware/hello-robot/samd/variants/hello_stepper/variant.h
@@ -101,6 +101,7 @@ extern "C"
 #define BOARD_ID_1          (31ul) //PB23
 #define BOARD_ID_2          (26ul) //PA27
 
+
 #define PIN_D0               (42ul)
 static const uint8_t D0  = PIN_D0;
 
@@ -113,6 +114,7 @@ static const uint8_t RS0  = PIN_RS0;
 static const uint8_t RS1  = PIN_RS1;
 
 #define PIN_SYNC              (3ul) //PA9
+#define PIN_VOLTAGE           (14ul) //PA2
 
 static const uint8_t A0  = PIN_A0;
 static const uint8_t A1  = PIN_A1;

diff --git a/arduino/hello_pimu/AnalogManager.cpp b/arduino/hello_pimu/AnalogManager.cpp
@@ -10,6 +10,7 @@
   --------------------------------------------------------------
 */
 #include "AnalogManager.h"
+#include "Pimu.h"
 #include "Common.h"
 
 AnalogManager analog_manager;
@@ -19,13 +20,18 @@ AnalogManager analog_manager;
 // Use the SAMD21's ISR to transfer ADC results to buffer array in memory
 
 
-#define IDX_ANA_V_BATT 0
-#define IDX_ANA_CLIFF_0 2
-#define IDX_ANA_CLIFF_1 3
-#define IDX_ANA_CLIFF_2 4
-#define IDX_ANA_CLIFF_3 5
-#define IDX_ANA_TEMP 6
-#define IDX_ANA_CURRENT 7
+#define IDX_ANA_V_BATT 0 //AIN0 MUXPOS 0x00
+#define IDX_ANA_CLIFF_0 1 //AIN2 MUXPOS 0x02
+#define IDX_ANA_CLIFF_1 2 //AIN3 MUXPOS 0x03
+#define IDX_ANA_CLIFF_2 3 //AIN4 MUXPOS 0x04
+#define IDX_ANA_CLIFF_3 4 //AIN5 MUXPOS 0x05
+#define IDX_ANA_TEMP 5 //AIN6 MUXPOS 0x06
+#define IDX_ANA_CURRENT 6 //AIN7 MUXPOS 0x07
+#define IDX_ANA_CURRENT_CHARGE 7//AIN18 MUXPOS 0x12
+#define IDX_ANA_CURRENT_EFUSE 8//AIN19 MUXPOS 0x13
+
+
+
 
 
 AnalogManager::AnalogManager(){
@@ -41,6 +47,16 @@ AnalogManager::AnalogManager(){
   first_read_done=0;
   first_config=1;
   adc_input_id=0;
+
+  mux_map[IDX_ANA_V_BATT]=0x00;
+  mux_map[IDX_ANA_CLIFF_0]=0x02;
+  mux_map[IDX_ANA_CLIFF_1]=0x03;
+  mux_map[IDX_ANA_CLIFF_2]=0x04;
+  mux_map[IDX_ANA_CLIFF_3]=0x05;
+  mux_map[IDX_ANA_TEMP]=0x06;
+  mux_map[IDX_ANA_CURRENT]=0x07;
+  mux_map[IDX_ANA_CURRENT_CHARGE]=0x12;
+  mux_map[IDX_ANA_CURRENT_EFUSE]=0x13;
   }
 
 void AnalogManager::update_config(Pimu_Config * cfg_new, Pimu_Config * cfg_old)
@@ -69,6 +85,16 @@ void AnalogManager::update_config(Pimu_Config * cfg_new, Pimu_Config * cfg_old)
   {
     voltage = adcResult[IDX_ANA_V_BATT];
     current = adcResult[IDX_ANA_CURRENT];
+    if (BOARD_VARIANT>=3)
+    {
+        current_efuse = adcResult[IDX_ANA_CURRENT_EFUSE];
+        current_charge = adcResult[IDX_ANA_CURRENT_CHARGE];
+    }
+    else
+    {
+        current_efuse = 0;
+        current_charge = 0;
+    }
     temp =    adcResult[IDX_ANA_TEMP];
     cliff[0] = adcResult[IDX_ANA_CLIFF_0];
     cliff[1] = adcResult[IDX_ANA_CLIFF_1];
@@ -93,11 +119,26 @@ void AnalogManager::step(Pimu_Status * stat, Pimu_Config * cfg)
     cliff[1] = adcResult[IDX_ANA_CLIFF_1];
     cliff[2] = adcResult[IDX_ANA_CLIFF_2];
     cliff[3] = adcResult[IDX_ANA_CLIFF_3];
+    if (BOARD_VARIANT>=3)
+    {
+        current_charge = adcResult[IDX_ANA_CURRENT];
+        current = adcResult[IDX_ANA_CURRENT_EFUSE];
+    }
+    else
+    {
+        current_efuse = 0;
+        current_charge = 0;
+    }
     first_filter=false;
   }
 
   voltage = voltage * voltage_LPFa +  voltage_LPFb* adcResult[IDX_ANA_V_BATT];
   current = current * current_LPFa +  current_LPFb* adcResult[IDX_ANA_CURRENT];
+  if (BOARD_VARIANT>=3)
+    {
+    current_efuse = current_efuse * current_LPFa +  current_LPFb* adcResult[IDX_ANA_CURRENT_EFUSE];
+    current_charge = current_charge * current_LPFa +  current_LPFb* adcResult[IDX_ANA_CURRENT_CHARGE];
+    }
   temp =    temp *    temp_LPFa +     temp_LPFb*    adcResult[IDX_ANA_TEMP];
   cliff[0]= cliff_LPFa*cliff[0] +  cliff_LPFb*adcResult[IDX_ANA_CLIFF_0];
   cliff[1]= cliff_LPFa*cliff[1] +  cliff_LPFb*adcResult[IDX_ANA_CLIFF_1];
@@ -119,7 +160,7 @@ void AnalogManager::step(Pimu_Status * stat, Pimu_Config * cfg)
 void AnalogManager::setupADC()
 {
   //ADC ISR setup
-  ADC->INPUTCTRL.bit.MUXPOS = adc_input_id;                   // Set the analog input to A0
+  ADC->INPUTCTRL.bit.MUXPOS = mux_map[adc_input_id];                
   while(ADC->STATUS.bit.SYNCBUSY);                   // Wait for synchronization
   //SAMPLEN 5:0 bits, sample time =(samplen+1)*(clk_adc/2)
   ADC->SAMPCTRL.bit.SAMPLEN = 0x07;                  // Set max Sampling Time Length to 4x ADC clock pulse (42us)
@@ -155,7 +196,7 @@ void ADC_Handler()
     }
     ADC->CTRLA.bit.ENABLE = 0;                     // Disable the ADC
     while(ADC->STATUS.bit.SYNCBUSY);               // Wait for synchronization
-    ADC->INPUTCTRL.bit.MUXPOS = analog_manager.adc_input_id;         // Set the analog input channel
+    ADC->INPUTCTRL.bit.MUXPOS = analog_manager.mux_map[analog_manager.adc_input_id];         // Set the analog input channel
     while(ADC->STATUS.bit.SYNCBUSY);                   // Wait for synchronization
     ADC->CTRLA.bit.ENABLE = 1;                         // Enable the ADC
     while(ADC->STATUS.bit.SYNCBUSY);                   // Wait for synchronization

diff --git a/arduino/hello_pimu/AnalogManager.h b/arduino/hello_pimu/AnalogManager.h
@@ -15,7 +15,7 @@
 
 #include "Common.h"
 
-#define NUM_ADC_INPUTS 8
+#define NUM_ADC_INPUTS 9
 
 class AnalogManager {    
   public: 
@@ -36,8 +36,11 @@ class AnalogManager {
     bool at_cliff[4];
     float voltage;
     float current;
+    float current_charge;
+    float current_efuse;
     float temp;
     volatile uint16_t adcResult[NUM_ADC_INPUTS] = {};         // ADC results buffer
+    uint8_t mux_map[NUM_ADC_INPUTS];
     uint8_t adc_input_id;
     bool first_read_done;
   private:

diff --git a/arduino/hello_pimu/Common.h b/arduino/hello_pimu/Common.h
@@ -31,8 +31,9 @@
 // Version 0.4.0: Move to fast motor sync, status_aux, and P3
 // Version 0.5.0: Move to support for Transport V1
 // Version 0.5.1: Fix trace rollover issue
+// Version 0.6.0: Production release for S3 and P4. Move to new IMU and current monitoring.
 
-#define FIRMWARE_VERSION "Pimu.v0.5.1p3"
+#define FIRMWARE_VERSION "Pimu.v0.6.0p4"
 
 #define FS 100 //Loop rate in Hz for TC5
 
@@ -58,13 +59,15 @@
 
 /////////////////Map Pins////////////////////////////////////////////////
 //From hello_pimu/variants.h
-#define ANA_V_BATT A0
-#define ANA_CLIFF_0 A1
-#define ANA_CLIFF_1 A2
-#define ANA_CLIFF_2 A3
-#define ANA_CLIFF_3 A4
-#define ANA_TEMP A5
-#define ANA_CURRENT A6
+#define ANA_V_BATT PIN_A0
+#define ANA_CLIFF_0 PIN_A1
+#define ANA_CLIFF_1 PIN_A2
+#define ANA_CLIFF_2 PIN_A3
+#define ANA_CLIFF_3 PIN_A4
+#define ANA_TEMP PIN_A5
+#define ANA_CURRENT PIN_A6 //Stretch 2 and earlier, shunt on battery supply
+#define ANA_CURRENT_EFUSE PIN_A7//Stretch 3 and later, e-fuse measurement: PA11
+#define ANA_CURRENT_CHARGE PIN_A8//Stretch 3 and later, current supplied by charger : PA10
 
 #define RUNSTOP_SW D0
 #define LED D3
@@ -126,7 +129,6 @@
 
 
 struct __attribute__ ((packed)) IMU_Status{
-
   float ax;
   float ay;
   float az;
@@ -183,6 +185,8 @@ struct __attribute__ ((packed)) Pimu_Status{
   uint64_t timestamp; //us
   uint16_t bump_event_cnt;
   float debug;
+  float current_charge;
+
 };
 
 //Dummy struct for now, for future expansion