Merge pull request #2 from MiRoboticsLab/v1.3.0.0

V1.3.0.0

Author: ruheng

cyberdog_bluetooth/cyberdog_bluetooth/bluetooth_node.py

@@ -179,6 +179,7 @@ def __init__(self, node_name: str):
         self.__self_check_status_code = -1
         self.__notification_thread.start()
         self.__joy_polling_thread.start()
+        self.__bt_central.RemoveUnrecordedDevices(self.__getHistoryConnectionInfo())
 
     def __del__(self):
         self.__notification_thread.join()
@@ -305,7 +306,7 @@ def __connect_callback(self, req, res):
                         self.__tryToReleaseMutex(self.__poll_mutex)
                         # res.result = self.__waitForUWBResponse(False)
                         self.__disconnectPeripheral()
-            self.__bt_central.RemoveUnRecordedDevices(self.__getHistoryConnectionInfo())
+            self.__bt_central.RemoveUnrecordedDevices(self.__getHistoryConnectionInfo())
             self.__connect_timeout_timer.reset()
             if self.__bt_central.ConnectToBLE(
                     req.selected_device.mac,
@@ -694,7 +695,7 @@ def __notificationTimerCB(self):
         if self.__connecting or not self.__bt_central.IsConnected():
             self.__tryToReleaseMutex(self.__poll_mutex)
             return
-        notified = self.__bt_central.WaitForNotifications(0.5)
+        notified = self.__bt_central.WaitForNotifications(0.25)
         self.__tryToReleaseMutex(self.__poll_mutex)
         if notified == 3:
             self.__disconnectUnexpectedly()
@@ -917,7 +918,7 @@ def __deleteHistory(self, mac):
         i = 0
         found = False
         for dev_info in history_info_list:
-            if dev_info['mac'] == mac:
+            if dev_info['mac'] == mac or dev_info['mac'] == mac.lower():
                 found = True
                 break
             i += 1
@@ -932,8 +933,10 @@ def __deleteHistory(self, mac):
         return False
 
     def __deleteHistoryCB(self, req, res):
-        res.result = self.__deleteHistory(req.map_url)
-        self.__unpair(req.map_url)
+        res.result = False
+        if not self.__connecting and not self.__dfu_processing:
+            res.result = self.__deleteHistory(req.map_url)
+            self.__unpair(req.map_url)
         return res
 
     def __tryToReleaseMutex(self, mutex):
@@ -1015,7 +1018,7 @@ def __joyPubPolling(self):
             sleep(0.05)
 
     def __activateDFU(self):
-        if not self.__poll_mutex.acquire(blocking=True, timeout=0.75):
+        if not self.__poll_mutex.acquire(blocking=True, timeout=1.0):
             self.__logger.warning('Unable to acquire __poll_mutex')
             return False
         dfu_handle = self.__bt_central.SetNotificationByUUID(  # indicate
@@ -1242,11 +1245,10 @@ def __taskStatusCB(self, msg):
         self.__task_status = msg.task_status
 
     def __unpair(self, mac: str):
-        if not self.__connecting and not self.__dfu_processing:
-            if not self.__bt_central.Unpair(mac):
-                self.__disconnectPeripheral()
-                self.__logger.info('Unpaired current device')
-            self.__logger.info('Unpaired device %s' % mac)
+        if not self.__bt_central.Unpair(mac):
+            self.__disconnectPeripheral()
+            self.__logger.info('Unpaired current device')
+        self.__logger.info('Unpaired device %s' % mac)
 
     def __intervalTimerCB(self):
         self.__logger.info('Intermission is off')

cyberdog_bluetooth/cyberdog_bluetooth/bt_core.py

@@ -129,6 +129,9 @@ def Disconnect(self):
         except AttributeError as e:
             self.__logger.error(
                 'AttributeError: %s Exeption while disconnecting!' % e)
+        except BrokenPipeError as e:
+            self.__logger.error(
+                'BrokenPipeError: %s Exeption while disconnecting!' % e)
         finally:
             self.__connected = False
             self.__peripheral_name = ''
@@ -372,7 +375,8 @@ def SetNotificationByCharacteristicUUID(self, char_uuid: UUID, enable=True, indi
         return characteristic_handle
 
     def Unpair(self, addr: str):
-        if self.__connected and addr == self.__peripheral.addr:
+        if self.__connected and\
+                (addr == self.__peripheral.addr or addr.lower() == self.__peripheral.addr):
             try:
                 self.__peripheral.unpair()
             except BTLEManagementError as e:
@@ -449,26 +453,28 @@ def __runCommand(self, cmd: str):
         tmp = str(output.stdout.read(), encoding='utf-8')
         return tmp
 
-    def RemoveUnRecordedDevices(self, devices):
-        if devices is None or len(devices) == 0:
-            return
+    def RemoveUnrecordedDevices(self, devices):
         mac_list = []
-        for info in devices:
-            mac_list.append(info['mac'].upper())
+        if devices is not None and len(devices) != 0:
+            for info in devices:
+                mac_list.append(info['mac'].upper())
+                self.__logger.info('%s is in history list' % mac_list[-1])
         raw_result = self.__runCommand('echo list | bluetoothctl | grep Device')
         str_list = raw_result.split('\n')
         device_num = len(str_list)
         if device_num < 2:
-            self.__logger.error('No bluetooth devices founded in system')
+            self.__logger.warning('No bluetooth devices found in system')
             return
         device_num -= 1
         i = 0
         while i < device_num:
             found_i = str_list[i].find('Device ')
-            if found_i == -1:
+            cyberdog_i = str_list[i].find('CyberdogRemote')
+            if found_i == -1 or cyberdog_i == -1:
                 i += 1
                 continue
             mac = str_list[i][found_i + 7: found_i + 24]
+            self.__logger.info('%s is a paired device' % mac)
             if mac not in mac_list:
                 self.__logger.warning(
                     '%s is not recorded in known file, remove it from system' % mac)

cyberdog_bms/include/cyberdog_bms/bms_plugin.hpp

@@ -73,7 +73,29 @@ class BMSCarpo : public cyberdog::device::BMSBase
         uint8_t wireless_charging_temp;
         uint16_t batt_loop_number;
         uint8_t batt_health;
-        uint16_t batt_st;
+        // uint16_t batt_st;
+        union {
+          uint16_t batt_st;
+          struct
+          {
+            uint16_t charge_over_current : 1;
+            uint16_t discharge_over_current : 1;
+            uint16_t cell_over_voltage : 1;
+            uint16_t cell_under_voltage : 1;
+            uint16_t cell_volt_abnormal : 1;
+            uint16_t mos_over_temp : 1;
+            uint16_t discharge_short : 1;
+            uint16_t fuse : 1;
+            uint16_t discharge_over_tmp : 1;
+            uint16_t discharge_under_tmp : 1;
+            uint16_t charge_over_temp : 1;
+            uint16_t charge_under_temp : 1;
+            uint16_t charge_mos_state : 1;
+            uint16_t discharge_mos_state : 1;
+            uint16_t chg_mos_fault : 1;
+            uint16_t dsg_mos_fault : 1;
+          };
+        };
         union {
           uint8_t bms_state1;
           struct

cyberdog_bms/src/bms_plugin.cpp

@@ -216,6 +216,24 @@ void BMSCarpo::BatteryMsgCall(EP::DataLabel & label, std::shared_ptr<BatteryMsg>
       message.batt_loop_number = data->batt_loop_number;
       message.batt_health = data->batt_health;
       message.batt_st = data->batt_st;
+
+      message.charge_over_current = data->charge_over_current;
+      message.discharge_over_current = data->discharge_over_current;
+      message.cell_over_voltage = data->cell_over_voltage;
+      message.cell_under_voltage = data->cell_under_voltage;
+      message.cell_volt_abnormal = data->cell_volt_abnormal;
+      message.mos_over_temp = data->mos_over_temp;
+      message.discharge_short = data->discharge_short;
+      message.fuse = data->fuse;
+      message.discharge_over_tmp = data->discharge_over_tmp;
+      message.discharge_under_tmp = data->discharge_under_tmp;
+      message.charge_over_temp = data->charge_over_temp;
+      message.charge_under_temp = data->charge_under_temp;
+      message.charge_mos_state = data->charge_mos_state;
+      message.discharge_mos_state = data->discharge_mos_state;
+      message.chg_mos_fault = data->chg_mos_fault;
+      message.dsg_mos_fault = data->dsg_mos_fault;
+
       message.bms_state_one = data->bms_state1;
       message.power_normal = data->power_normal;
       message.power_wired_charging = data->power_wired_charging;