Merge pull request #25 from hello-robot/feature/se3_test

P5 Firmware

arduino/hello_pimu/AnalogManager.cpp

@@ -30,7 +30,11 @@ AnalogManager analog_manager;
 #define IDX_ANA_CURRENT_CHARGE 7//AIN18 MUXPOS 0x12
 #define IDX_ANA_CURRENT_EFUSE 8//AIN19 MUXPOS 0x13
 
-
+//Default LPF Values
+#define FACTORY_VOLTAGE_LPF 1
+#define FACTORY_CURRENT_LPF 10
+#define FACTORY_CLIFF_LPF 10
+#define FACTORY_TEMP_LPF 1
 
 
 
@@ -59,6 +63,31 @@ AnalogManager::AnalogManager(){
   mux_map[IDX_ANA_CURRENT_EFUSE]=0x13;
   }
 
+//TODO: Write gains to Flash instead of hardcoding them here////
+void AnalogManager::factory_config()
+{
+  Pimu_Config factory_config;
+  factory_config.voltage_LPF = FACTORY_VOLTAGE_LPF;
+  factory_config.current_LPF = FACTORY_CURRENT_LPF;
+
+  factory_config.cliff_LPF = FACTORY_CLIFF_LPF;
+  factory_config.temp_LPF = FACTORY_TEMP_LPF;
+
+
+  cliff_LPFa = exp(factory_config.cliff_LPF*-2*3.14159/FS); // z = e^st pole mapping
+  cliff_LPFb = (1.0-cliff_LPFa);
+
+  voltage_LPFa = exp(factory_config.voltage_LPF*-2*3.14159/FS); // z = e^st pole mapping
+  voltage_LPFb = (1.0-voltage_LPFa);
+
+  current_LPFa = exp(factory_config.current_LPF*-2*3.14159/FS); // z = e^st pole mapping
+  current_LPFb = (1.0-current_LPFa);
+
+  temp_LPFa = exp(factory_config.temp_LPF*-2*3.14159/FS); // z = e^st pole mapping
+  temp_LPFb = (1.0-temp_LPFa);
+
+}
+
 void AnalogManager::update_config(Pimu_Config * cfg_new, Pimu_Config * cfg_old)
 {
   if (cfg_new->cliff_LPF!=cfg_old->cliff_LPF) 

arduino/hello_pimu/AnalogManager.h

@@ -23,6 +23,7 @@ class AnalogManager {
     void update_config(Pimu_Config * cfg_new, Pimu_Config * cfg_old);
     void step(Pimu_Status * stat, Pimu_Config * cfg);
     void setupADC();
+    void factory_config();
 
     float cliff_LPFa; 
     float cliff_LPFb;

arduino/hello_pimu/ChargerManager.cpp

@@ -0,0 +1,146 @@
+#include "ChargerManager.h"
+#include "TimeManager.h"
+#include "Common.h"
+
+
+unsigned long t_charger_last=0;
+
+float v1 = 0.0;
+float current_change = 0.0;
+float voltage_change = 0.0;
+
+float sys_curr_array[10] = {};
+float sys_volt_array[10] = {};
+int curr_cnt = 0;
+
+//charging_sts_flag is if charger is connected 
+
+void ChargerManager::hotplug_check(float vd)
+{
+	//hotplug condition 
+	if (vd >= 0.07 && hotplug_sts_flag)
+	{	
+		hotplug_sts_flag = false;
+		charging_sts_flag = true;
+
+	}
+	if (vd <= 0.05 && hotplug_sts_flag)
+	{
+		charging_sts_flag = false;
+	}
+}
+
+void ChargerManager::unplug_check(float v, float c, float cc)
+{
+	sys_curr_array[curr_cnt] = c;
+	sys_volt_array[curr_cnt] = v;
+
+
+	if (curr_cnt == 9)
+	{	
+		//Only do check if charging current is low and sys current > 2
+		if (cc < 0.15 && c > 2 && hotplug_sts_flag == false)
+		{
+			float voltage_change = sys_volt_array[9] - sys_volt_array[0];
+
+			float max_c = sys_curr_array[0];
+			float min_c = sys_curr_array[0];
+			for (int x = 1; x < 10; ++x)
+			{
+				if(sys_curr_array[x] > max_c)
+				{
+					max_c = sys_curr_array[x];
+				}
+				if(sys_curr_array[x] < min_c)
+				{
+					min_c = sys_curr_array[x];
+				}
+			}
+
+
+			if (voltage_change < -0.15 && (max_c - min_c) < 1 && unplug_sts_flag == false)
+			{
+				charging_sts_flag = false;
+				unplug_sts_flag = true;
+			}
+			if (voltage_change > 0.15 && (max_c - min_c) < 1 && unplug_sts_flag == true)
+			{
+				charging_sts_flag = true;
+				unplug_sts_flag = false;
+			}
+		}
+
+
+
+		sys_curr_array[0] = sys_curr_array[9];
+		sys_volt_array[0] = sys_volt_array[9];
+		for (int i = 1; i < 10; i++)
+		{
+			sys_curr_array[i] = 0;
+			sys_volt_array[i] = 0;
+		}
+		curr_cnt = 0;
+	}
+	curr_cnt++;
+
+}
+
+bool ChargerManager::step(float vbat, float sys_current, float charge_current, int board_variant)
+{
+
+	unsigned long t = time_manager.get_elapsed_time_ms();
+
+	if (t -  t_charger_last > CHARGER_SAMPLE_RATE)
+	{
+		t_charger_last = t;
+
+		//Charger Connected Pin LOW Charger Not Connected
+		if (digitalRead(CHARGER_CONNECTED) == LOW)
+		{
+			v1 = vbat;
+			hotplug_sts_flag = true;
+			charging_sts_flag = false;
+			charger_plugged_in_flag = false;
+			unplug_sts_flag = true;
+		}
+
+		//Charger Connected Pin HIGH Charger Connected
+		if (digitalRead(CHARGER_CONNECTED) == HIGH)
+		{	
+			charger_plugged_in_flag = true;
+			float v2 = vbat;
+			float vdiff = v2 - v1;
+
+			if (board_variant >= 3)
+			{
+				if (charge_current >= 0.15)
+				{
+					charging_sts_flag = true;
+					unplug_sts_flag = false;
+					hotplug_sts_flag = false;
+				}
+
+				if (charge_current < 0.15 && sys_current <= 2)
+				{
+					charging_sts_flag = false;
+					unplug_sts_flag = true;
+					hotplug_sts_flag = false;
+				}
+
+				unplug_check(vbat, sys_current, charge_current);
+				hotplug_check(vdiff);
+			}
+
+			if (board_variant < 3)
+			{
+				hotplug_check(vdiff);
+			}
+
+		}
+
+	}
+	return charging_sts_flag;
+
+}
+
+

arduino/hello_pimu/ChargerManager.h

@@ -0,0 +1,19 @@
+#ifndef __CHARGER_MANAGER_H__
+#define  __CHARGER_MANAGER_H__
+#include "Common.h"
+
+#define CHARGER_SAMPLE_RATE 500
+
+
+class ChargerManager{
+  public:
+    bool step(float vbat, float sys_current, float charge_current, int board_variant);
+    void hotplug_check(float vd);
+    void unplug_check(float v, float c, float cc);
+    bool charging_sts_flag = true;
+    bool hotplug_sts_flag = false;
+    bool unplug_sts_flag = false;
+    bool charger_plugged_in_flag = true;
+};
+
+#endif

arduino/hello_pimu/Common.h

@@ -33,8 +33,9 @@
 // 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. (BOARD VARIANT=3 for new IMU)
 // Version 0.6.2: added interrupts for imu data, added writing to system orientation record for IMU orientation to match older robots
-
-#define FIRMWARE_VERSION "Pimu.v0.6.2p4"
+// Version 0.6.3: incorporting charging detection class
+// Version 0.7.0: added is_charger_charging state to pimu status
+#define FIRMWARE_VERSION "Pimu.v0.7.0p5"
 
 #define FS 100 //Loop rate in Hz for TC5
 
@@ -109,6 +110,7 @@
 #define STATE_CHARGER_CONNECTED 2048
 #define STATE_BOOT_DETECTED 4096
 #define STATE_IS_TRACE_ON 8192       //Is trace recording
+#define STATE_IS_CHARGER_CHARGING 16384
 
 #define TRIGGER_BOARD_RESET  1
 #define TRIGGER_RUNSTOP_RESET  2

arduino/hello_pimu/LightBarManager.h

@@ -14,6 +14,7 @@
 #define  __LIGHT_BAR_MANAGER_H__
 
 #include "Common.h"
+#include "ChargerManager.h"
 #include <Adafruit_NeoPixel_ZeroDMA.h>
 
 enum  lightbar_mode {OFF,BOOTING, CHARGING_REQUIRED, CHARGING_RUNSTOP_ON, CHARGING_RUNSTOP_OFF, NORMAL_RUNSTOP_OFF, NORMAL_RUNSTOP_ON };

arduino/hello_pimu/Pimu.cpp

@@ -24,12 +24,17 @@
 #include "LightBarManager.h"
 #include "TraceManager.h"
 #include "IMU_BNO085.h"
+#include "ChargerManager.h"
 
 
 #define V_TO_RAW(v) v*1024/20.0 //per circuit
 #define RAW_TO_V(r) (float)r*0.01953125 //20.0/1024.0
 #define I_TO_RAW(i) (i*1000)*0.408*1024.0/3300 //per circuit
 
+#define RAW_TO_I(r) (float)r*0.01438686
+
+#define RAW_TO_CHRG_I(r) (float)r*0.01498910
+
 float low_voltage_alert=V_TO_RAW(10.5);
 int low_voltage_alert_cnt=0;
 float high_current_alert= I_TO_RAW(6.0);
@@ -45,13 +50,18 @@ bool state_buzzer_on=false;
 bool state_low_voltage_alert=false;
 bool state_high_current_alert=false;
 bool state_over_tilt_alert=false;
-bool state_charger_connected=false;
+
 bool state_boot_detected=false;
 
 RunstopManager runstop_manager;
 SyncManager sync_manager(&runstop_manager);
 LightBarManager light_bar_manager;
 
+ChargerManager charger_manager;
+
+bool state_charger_connected = false;
+bool state_charger_is_charging = false;
+
 
 
 
@@ -175,6 +185,7 @@ void setupPimu() {
   sprintf(board_info.board_variant, "Pimu.%d", BOARD_VARIANT);
   memcpy(&(board_info.firmware_version),FIRMWARE_VERSION,min(20,strlen(FIRMWARE_VERSION)));
   analog_manager.setupADC();
+  analog_manager.factory_config();
   setupTimer4_and_5();
   setupWDT(WDT_TIMEOUT_PERIOD);
   time_manager.clock_zero();
@@ -189,7 +200,7 @@ void stepPimuController()
   runstop_manager.step(&cfg);
   beep_manager.step();
   analog_manager.step(&stat, &cfg);
-  light_bar_manager.step(state_boot_detected, runstop_manager.state_runstop_event, state_charger_connected, state_low_voltage_alert, runstop_manager.runstop_led_on, RAW_TO_V(analog_manager.voltage));
+  light_bar_manager.step(state_boot_detected, runstop_manager.state_runstop_event, state_charger_is_charging, state_low_voltage_alert, runstop_manager.runstop_led_on, RAW_TO_V(analog_manager.voltage));
   update_fan();  
   update_imu();
   update_board_reset();
@@ -426,10 +437,12 @@ void update_imu()
 ////////////////////////////
 void update_voltage_monitor()
 {
-  if (BOARD_VARIANT>=1)
+  if (BOARD_VARIANT >= 1)
   {
-    //For Variant 1, indicate charging required on the Neopixel
-    state_charger_connected=digitalRead(CHARGER_CONNECTED);
+    state_charger_is_charging = charger_manager.step(RAW_TO_V(analog_manager.voltage), RAW_TO_I(analog_manager.current_efuse), RAW_TO_CHRG_I(analog_manager.current_charge) , BOARD_VARIANT);
+    state_charger_connected = charger_manager.charger_plugged_in_flag;
+
+
     if(analog_manager.voltage<low_voltage_alert) //dropped below
       {
         state_low_voltage_alert=true;
@@ -441,7 +454,7 @@ void update_voltage_monitor()
         state_low_voltage_alert=false;
       }
   }
-  
+
   if (BOARD_VARIANT==0)
   {
     //For Variant 0, do the double beep at low voltage and trigger the runstop
@@ -550,6 +563,7 @@ void update_status()
   stat.state= state_boot_detected ? stat.state|STATE_BOOT_DETECTED : stat.state;
   stat.state= state_over_tilt_alert ? stat.state|STATE_OVER_TILT_ALERT : stat.state;
   stat.state = trace_manager.trace_on ?     stat.state | STATE_IS_TRACE_ON: stat.state;
+  stat.state= state_charger_is_charging ? stat.state|STATE_IS_CHARGER_CHARGING : stat.state;
   memcpy((uint8_t *) (&stat_out),(uint8_t *) (&stat),sizeof(Pimu_Status));
 
   if(TRACE_TYPE==TRACE_TYPE_DEBUG)