baytgo param mavlink addded

libraries/AP_BattMonitor/AP_BattMonitor.cpp

@@ -507,6 +507,12 @@ bool AP_BattMonitor::reset_remaining(uint16_t battery_mask, float percentage)
     return ret;
 }
 
+void  AP_BattMonitor::send_serial_batteryGo_param(mavlink_channel_t chan){
+
+
+}
+
+
 namespace AP {
 
 AP_BattMonitor &battery()

libraries/AP_BattMonitor/AP_BattMonitor.h

@@ -162,6 +162,10 @@ public:
 
     static const struct AP_Param::GroupInfo var_info[];
     
+    void send_serial_batteryGo_param(mavlink_channel_t chan);
+
+
+
 protected:
 
     /// parameters

libraries/AP_BattMonitor/AP_BattMonitor_Serial_BattGo.cpp

@@ -2,7 +2,8 @@
 #include <AP_HAL/AP_HAL.h>
 #include "AP_BattMonitor_Serial_BattGo.h"
 #include <AP_SerialManager/AP_SerialManager.h>
-
+#include <GCS_MAVLink/GCS_MAVLink.h>
+#include <GCS_MAVLink/GCS.h>
 #define MAXSONAR_SERIAL_LV_BAUD_RATE 115200
 extern const AP_HAL::HAL &hal;
 
@@ -30,7 +31,8 @@ bool AP_BattMonitor_Serial_BattGo::detect(uint8_t serial_instance)  //how to use
     return AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_Rangefinder, serial_instance) != nullptr;
 }
 
-void AP_BattMonitor_Serial_BattGo::init(void ){
+void AP_BattMonitor_Serial_BattGo::init(void)
+{
     if (uart != nullptr)
     {
         requestBattData(BATTGO_CMD_REQUEST_ACTUAL_DATA);
@@ -198,7 +200,7 @@ bool AP_BattMonitor_Serial_BattGo::get_reading() //TODO need to Refactor
                 {
                     if (((check_sum >> 8) & 0x00ff) == data)
                     {
-                     if(parseBattGo())
+                        if (parseBattGo())
                             parsed = true;
                     }
                     step = 0;
@@ -211,14 +213,14 @@ bool AP_BattMonitor_Serial_BattGo::get_reading() //TODO need to Refactor
                 {
                     if (((check_sum >> 8) & 0x00ff) == data)
                     {
-                        if(parseBattGo())
+                        if (parseBattGo())
                             parsed = true;
                     }
                     step = 0;
                 }
                 else
                 { //last data  0x55 is header, this data is addr,next step =2;
-                    gotoStepAddr( data);
+                    gotoStepAddr(data);
                 }
             }
             break;
@@ -229,7 +231,7 @@ bool AP_BattMonitor_Serial_BattGo::get_reading() //TODO need to Refactor
     return parsed;
 }
 
-void AP_BattMonitor_Serial_BattGo::gotoStepAddr( uint8_t &data)
+void AP_BattMonitor_Serial_BattGo::gotoStepAddr(uint8_t &data)
 {
     step = 2; //next step
     addr = data;
@@ -238,7 +240,6 @@ void AP_BattMonitor_Serial_BattGo::gotoStepAddr( uint8_t &data)
 
 void AP_BattMonitor_Serial_BattGo::requestBattData(uint8_t data)
 {
-
     //发送data主要   0x40  0x42  0x44  地址0x21  及计算crc 均不会出现0x55，故不做0x55处理
     uint8_t send_data[6];
     send_data[0] = 0x55;
@@ -248,7 +249,8 @@ void AP_BattMonitor_Serial_BattGo::requestBattData(uint8_t data)
     uint16_t crc = send_data[1] + send_data[2] + send_data[3];
     send_data[4] = crc & 0xff;
     send_data[5] = crc >> 8 & 0xff;
-    if (uart->txspace() > sizeof(send_data)) {
+    if (uart->txspace() > sizeof(send_data))
+    {
         uart->write(&send_data[0], sizeof(send_data));
     }
 }
@@ -297,9 +299,10 @@ void AP_BattMonitor_Serial_BattGo::setPayLoadData(uint8_t &usart_data)
 //     }
 
 // }
-
+ mavlink_battgo_info_t battgo_info_t;
 bool AP_BattMonitor_Serial_BattGo::parseBattGo()
 {
+    
     if (data_buf[0] == 0x43)
     { //电池实时数据
         uint32_t tnow = AP_HAL::micros();
@@ -307,13 +310,13 @@ bool AP_BattMonitor_Serial_BattGo::parseBattGo()
 
         for (uint8_t i = 0; i < 6; i++)
         {
-            uint16_t valtemp = ((data_buf[4 + 2 * i] << 8) & 0xff00 )+data_buf[3 + 2 * i];
+            uint16_t valtemp = ((data_buf[4 + 2 * i] << 8) & 0xff00) + data_buf[3 + 2 * i];
             _interim_state.cell_voltages.cells[i] = valtemp;
             _interim_state.voltage += valtemp;
         }
         _interim_state.temperature = data_buf[15] - 128;
         _interim_state.temperature_time = AP_HAL::millis();
-        _interim_state.current_amps = data_buf[16] +(data_buf[17] << 8 )+ (data_buf[18] << 16)+ (data_buf[19] << 24);
+        _interim_state.current_amps = data_buf[16] + (data_buf[17] << 8) + (data_buf[18] << 16) + (data_buf[19] << 24);
 
         // update total current drawn since startup
         if (_interim_state.last_time_micros != 0 && dt < 2000000)
@@ -333,10 +336,86 @@ bool AP_BattMonitor_Serial_BattGo::parseBattGo()
     else if (data_buf[0] == 0x41)
     {
         //电池参数
+       
+        battgo_info_t.channal_num = data_buf[1];
+        battgo_info_t.connect_status = data_buf[2];
+        battgo_info_t.sub_software_ver = data_buf[3];
+        battgo_info_t.main_software_ver = data_buf[4];
+        battgo_info_t.sub_hardware_ver = data_buf[5];
+        battgo_info_t.main_hardware_ver = data_buf[6];
+        for (int i = 0; i < 8; i++)
+        {
+            battgo_info_t.equipment_id[i] = data_buf[7 + i];
+        }
+        for (int i = 0; i < 10; i++)
+        {
+            battgo_info_t.slave_id[i] = data_buf[15 + i];
+        }
+        battgo_info_t.station_id = data_buf[25];
+        battgo_info_t.customer_id = data_buf[27] << 8 & data_buf[26];
+        battgo_info_t.battery_id = data_buf[31] << 24 & data_buf[30] << 16 & data_buf[29] << 8 & data_buf[28];
+
+        uint8_t sec = data_buf[32] & 0x1f;
+        uint8_t min = (data_buf[32] & 0xe0 >> 5) | (data_buf[33] & 0x0F << 4);
+        uint8_t hour = (data_buf[33] & 0xF0 << 4) | (data_buf[34] & 0x01 << 1);
+        uint8_t day = data_buf[34] & 0x3e >> 1;
+        uint8_t month = (data_buf[34] & 0xc0 >> 6) | (data_buf[35] & 0x03);
+        uint16_t year = (data_buf[35] & 0xFC >> 2 )+ 2017;
+
+        battgo_info_t.production_time_year = year;
+        battgo_info_t.production_time_mon = month;
+        battgo_info_t.production_time_mday = day;
+        battgo_info_t.production_time_hour = hour;
+        battgo_info_t.production_time_min = min;
+        battgo_info_t.production_time_sec = sec;
+        for (int i = 0; i < 16; i++)
+        {
+            battgo_info_t.battery_brand[i] = data_buf[i];
+        }
+        battgo_info_t.type = data_buf[52];
+        battgo_info_t.core_amount = data_buf[53];
+        battgo_info_t.over_discharge_vol = (data_buf[55] << 8) | data_buf[54];
+        battgo_info_t.warning_vol = (data_buf[57] << 8) | data_buf[56];
+        battgo_info_t.storage_vol = (data_buf[59] << 8) | data_buf[58];
+        battgo_info_t.full_charge_vol = (data_buf[61] << 8) | data_buf[60];
+        battgo_info_t.battery_capacity = (data_buf[65] << 24) | (data_buf[64] << 16) | (data_buf[63] << 8) | data_buf[62];
+        battgo_info_t.charge_c_value = (data_buf[67] << 8) | data_buf[66];
+        battgo_info_t.discharge_c_value = (data_buf[69] << 8) | data_buf[68];
+        battgo_info_t.temp_using_min = data_buf[70];
+        battgo_info_t.temp_using_max = data_buf[71];
+        battgo_info_t.temp_store_nin = data_buf[72];
+        battgo_info_t.temp_store_max = data_buf[73];
+        battgo_info_t.enable_auto_store = data_buf[74];
+        for (int i = 0; i < 6; i++)
+        {
+            battgo_info_t.battery_core_vol[i] = (data_buf[75 + 2 * i + 1] << 8) | (data_buf[75 + 2 * i]);
+        }
+        battgo_info_t.current_temp = data_buf[87];
+        battgo_info_t.actual_discharge_vol = (data_buf[91] << 24) | (data_buf[90] << 16) | (data_buf[89] << 8) |data_buf[88];
+        battgo_info_t.actual_charge_vol = (data_buf[95] << 24) | (data_buf[94] << 16) | (data_buf[93] << 8) |data_buf[92];
+        battgo_info_t.batt_cycle_times = (data_buf[97] << 8) |data_buf[96];
+        battgo_info_t.batt_abr_times = (data_buf[99] << 8) |data_buf[98];
+        battgo_info_t.temp_abr_times = (data_buf[101] << 8) |data_buf[100];
+        battgo_info_t.temp_abr_times = (data_buf[101] << 8) |data_buf[100];
+        battgo_info_t.over_vol_abr_times = (data_buf[103] << 8) |data_buf[102];
+        battgo_info_t.under_vol_abr_times = (data_buf[105] << 8) |data_buf[104];
+        battgo_info_t.soft_crash_times = data_buf[106];
+        battgo_info_t.current_charged_set = (data_buf[110] << 24) | (data_buf[109] << 16) | (data_buf[108] << 8) |data_buf[107];
+        battgo_info_t.vol_stored_set = (data_buf[112] << 8) |data_buf[111];
+        battgo_info_t.vol_stored_full_set = (data_buf[114] << 8) |data_buf[113];
+        battgo_info_t.self_discharge_times = (data_buf[115] << 8);
+        
+        requestBattData(BATTGO_CMD_REQUEST_HISTORY);
         return true;
     }
     else if (data_buf[0] == 0x45) //
     {
+         battgo_info_t.last_charge_cap = (data_buf[7] << 24) | (data_buf[6] << 16) | (data_buf[5] << 8) |data_buf[4];
+         for(int i=0;i<6;i++){
+             battgo_info_t.Internal_resistance[i]  =  (data_buf[20+2*i+1]<<8)|data_buf[20+2*i];
+         }
+         battgo_info_t.charge_state = data_buf[32];
+        gcs().update_serial_battgo(&battgo_info_t);
         return true;
     }
     return false;
@@ -345,11 +424,12 @@ bool AP_BattMonitor_Serial_BattGo::parseBattGo()
 void AP_BattMonitor_Serial_BattGo::read()
 {
     requestBattData(0x42);
-  if( get_reading())
+    if (get_reading())
     {
         uint32_t tnow = AP_HAL::micros();
         // timeout after 5 seconds
-    if ((tnow - _interim_state.last_time_micros) > AP_BATTMONITOR_SERIAL_TIMEOUT_MICROS) {
+        if ((tnow - _interim_state.last_time_micros) > AP_BATTMONITOR_SERIAL_TIMEOUT_MICROS)
+        {
             _interim_state.healthy = false;
         }
 
@@ -364,3 +444,8 @@ void AP_BattMonitor_Serial_BattGo::read()
         _state.healthy = _interim_state.healthy;
     }
 }
+
+void AP_BattMonitor_Serial_BattGo::notice_to_send_battgo_param()
+{
+    gcs().send_message(MSG_BATTGAO_PARAM);
+}

libraries/AP_BattMonitor/AP_BattMonitor_Serial_BattGo.h

@@ -39,7 +39,8 @@ public:
 
   // static detection function
   static bool detect(uint8_t serial_instance);
-
+void update_battgo_param_mavlink(mavlink_channel_t chan,mavlink_battgo_info_t &mavlink_battgo_info_t);
+void notice_to_send_battgo_param();
 private:
   // get a reading
 
@@ -48,7 +49,7 @@ private:
   char linebuf[10];
   uint8_t linebuf_len = 0;
   const int Head1 = 0x55; //数据包帧头
-  uint8_t data_buf[88];
+  uint8_t data_buf[116];
 
   uint8_t step;
   uint8_t addr;

libraries/GCS_MAVLink/GCS.h

@@ -217,7 +217,8 @@ public:
     void send_sys_status();
     void send_set_position_target_global_int(uint8_t target_system, uint8_t target_component, const Location& loc);
     void send_rpm() const;
-
+    bool send_battgo_param(const mavlink_battgo_info_t *mavlink_battgo_info_t);
+    //void send_serial_battgo(mavlink_battgo_info_t &battgo_info_t );
     bool locked() const;
 
     // return a bitmap of active channels. Used by libraries to loop
@@ -831,6 +832,8 @@ public:
     void update_send();
     void update_receive();
 
+   void   update_serial_battgo(mavlink_battgo_info_t *battgo_info_t );
+
     // minimum amount of time (in microseconds) that must remain in
     // the main scheduler loop before we are allowed to send any
     // mavlink messages.  We want to prioritise the main flight

libraries/GCS_MAVLink/GCS_Common.cpp

@@ -239,6 +239,7 @@ void GCS_MAVLINK::send_battery_status(const uint8_t instance) const
                                     MAV_BATTERY_CHARGE_STATE_UNDEFINED);
 }
 
+
 // returns true if all battery instances were reported
 bool GCS_MAVLINK::send_battery_status() const
 {
@@ -252,7 +253,10 @@ bool GCS_MAVLINK::send_battery_status() const
     }
     return true;
 }
-
+bool GCS_MAVLINK::send_battgo_param(const mavlink_battgo_info_t *battgo_info_t){
+             mavlink_msg_battgo_info_send_struct(chan,battgo_info_t);
+             return true;
+}
 void GCS_MAVLINK::send_distance_sensor(const AP_RangeFinder_Backend *sensor, const uint8_t instance) const
 {
     if (!sensor->has_data()) {
@@ -1896,6 +1900,15 @@ void GCS::update_receive(void)
     update_passthru();
 }
 
+void GCS::update_serial_battgo(mavlink_battgo_info_t *battgo_info_t )
+{
+    for (uint8_t i=0; i<num_gcs(); i++) {
+    //  mavlink_msg_battgo_info_send_struct(chan,&battgo_info_t);
+    chan(i)->send_battgo_param(battgo_info_t);
+    }
+           
+}
+
 void GCS::send_mission_item_reached_message(uint16_t mission_index)
 {
     for (uint8_t i=0; i<num_gcs(); i++) {
@@ -2133,7 +2146,6 @@ void GCS_MAVLINK::send_autopilot_version() const
     );
 }
 
-
 /*
   send LOCAL_POSITION_NED message
  */
@@ -4739,7 +4751,6 @@ uint64_t GCS_MAVLINK::capabilities() const
     return ret;
 }
 
-
 void GCS_MAVLINK::manual_override(RC_Channel *c, int16_t value_in, const uint16_t offset, const float scaler, const uint32_t tnow, const bool reversed)
 {
     if (c == nullptr) {

libraries/GCS_MAVLink/ap_message.h

@@ -73,5 +73,6 @@ enum ap_message : uint8_t {
     MSG_NAMED_FLOAT,
     MSG_EXTENDED_SYS_STATE,
     MSG_AUTOPILOT_VERSION,
+    MSG_BATTGAO_PARAM,
     MSG_LAST // MSG_LAST must be the last entry in this enum
 };