You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
378 lines
13 KiB
378 lines
13 KiB
|
|
#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; |
|
|
|
AP_BattMonitor_Serial_BattGo::AP_BattMonitor_Serial_BattGo(AP_BattMonitor &mon, |
|
AP_BattMonitor::BattMonitor_State &mon_state, |
|
AP_BattMonitor_Params ¶ms, |
|
uint8_t serial_instance) : AP_BattMonitor_Backend(mon, mon_state, params) |
|
{ |
|
|
|
const AP_SerialManager &serial_manager = AP::serialmanager(); |
|
uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Battery, serial_instance); |
|
if (uart != nullptr) |
|
{ |
|
uart->begin(serial_manager.find_baudrate(AP_SerialManager::SerialProtocol_Battery, serial_instance)); |
|
} |
|
last_rev_batt_time = 0; |
|
}; |
|
|
|
/* |
|
detect if a battGo is connected. We'll detect by |
|
trying to take a reading on Serial. If we get a result the sensor is |
|
there. |
|
*/ |
|
bool AP_BattMonitor_Serial_BattGo::detect(uint8_t serial_instance) //how to used |
|
{ |
|
return AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_Battery, serial_instance) != nullptr; |
|
} |
|
|
|
void AP_BattMonitor_Serial_BattGo::init(void) |
|
{ |
|
if (uart != nullptr) |
|
{ |
|
requestBattData(BATTGO_CMD_REQUEST_ACTUAL_DATA); |
|
// requestBattData(BATTGO_CMD_REQUEST_INFO); |
|
// requestBattData(BATTGO_CMD_REQUEST_HISTORY); |
|
} |
|
} |
|
bool isparse = false; |
|
bool AP_BattMonitor_Serial_BattGo::get_reading() //TODO need to Refactor |
|
{ |
|
|
|
if (uart == nullptr) |
|
{ |
|
return false; |
|
} |
|
int16_t numc; |
|
numc = uart->available(); |
|
uint8_t data; |
|
// if (numc == 0) |
|
// gcs().send_text(MAV_SEVERITY_INFO, "uasart num:%d", numc); |
|
isparse = false; |
|
for (int16_t i = 0; i < numc; i++) |
|
{ |
|
data = uart->read(); |
|
rxProcess(data); |
|
if (isparse) |
|
{ |
|
isparse = true; |
|
return true; |
|
} |
|
} |
|
return isparse; |
|
} |
|
|
|
void AP_BattMonitor_Serial_BattGo::requestBattData(uint8_t data) |
|
{ |
|
if (uart == nullptr) |
|
{ |
|
return ; |
|
} |
|
//发送data主要 0x40 0x42 0x44 地址0x21 及计算crc 均不会出现0x55，故不做0x55处理 |
|
uint8_t send_data[13]; |
|
send_data[0] = 0x55; |
|
send_data[1] = 0x12; |
|
send_data[2] = 0x08; |
|
send_data[3] = data; |
|
send_data[4] = 0; |
|
send_data[5] = 0; |
|
send_data[6] = 0; |
|
send_data[7] = 0; |
|
send_data[8] = 0; |
|
send_data[9] = 0; |
|
send_data[10] = 0; |
|
uint16_t crc = send_data[1] + send_data[2] + send_data[3]; //Notice :other data is 0 ,so no need to add to culcalate crc |
|
send_data[11] = crc & 0xff; //64 |
|
send_data[12] = crc >> 8 & 0xff; //00 |
|
if (uart->txspace() > sizeof(send_data)) |
|
{ |
|
uart->write(&send_data[0], sizeof(send_data)); |
|
} |
|
} |
|
|
|
mavlink_battgo_info_t battgo_info_t; |
|
mavlink_battgo_history_t battgo_history_t; |
|
bool AP_BattMonitor_Serial_BattGo::parseBattGo() |
|
{ |
|
uint8_t offset =0; |
|
if (data_buf[0] == 0x43) |
|
{ //电池实时数据 |
|
uint32_t tnow = AP_HAL::micros(); |
|
uint32_t dt = tnow - _interim_state.last_time_micros; |
|
_interim_state.voltage = 0; |
|
for (uint8_t i = 0; i < 6; 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 / 1000.0; |
|
_has_cell_voltages = true; |
|
} |
|
_interim_state.temperature = (int8_t)data_buf[15]; |
|
_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)) / 1000.0; |
|
// update total current drawn since startup |
|
if (_interim_state.last_time_micros != 0 && dt < 2000000) |
|
{ |
|
// .0002778 is 1/3600 (conversion to hours) |
|
float mah = (float)((double)_interim_state.current_amps * (double)dt * (double)0.0000002778f); |
|
_interim_state.consumed_mah += mah; |
|
_interim_state.consumed_wh += 0.001f * mah * _interim_state.voltage; |
|
} |
|
|
|
battgo_history_t.batt_cycled_time = (data_buf[25]<<8)+data_buf[24] ; |
|
battgo_history_t.batt_abnormal_time = (data_buf[27]<<8)+data_buf[26] ; |
|
battgo_history_t.temp_abnormal_time = (data_buf[29]<<8)+data_buf[28] ; |
|
battgo_history_t.over_vol_abnormal_time= (data_buf[31]<<8)+data_buf[30] ; |
|
offset = 32; |
|
battgo_history_t.under_vol_abnormal_time = (data_buf[offset+1]<<8)+data_buf[offset] ; |
|
offset = offset+2; |
|
battgo_history_t.soft_crashes_time = data_buf[offset]; |
|
offset++; |
|
battgo_history_t.charging_current_setted = (data_buf[offset+3] + (data_buf[offset+2] << 8) + (data_buf[offset+1] << 16) + (data_buf[offset] << 24)) ; |
|
offset = offset+4; |
|
battgo_history_t.storage_vol_setted = (data_buf[offset+1]<<8)+data_buf[offset] ; |
|
offset+=2; |
|
battgo_history_t.charging_full_vol_setted = (data_buf[offset+1]<<8)+data_buf[offset] ; |
|
offset+=2; |
|
battgo_history_t.self_discharge_times_setted = data_buf[offset]; |
|
//battgo_history_t.set |
|
// record time |
|
_interim_state.last_time_micros = tnow; |
|
_interim_state.healthy = true; |
|
|
|
|
|
|
|
return true; |
|
} |
|
else if (data_buf[0] == 0x41) |
|
{ |
|
//电池参数 |
|
//gcs().send_text(MAV_SEVERITY_INFO, "into battgo 0x41 battgo info"); |
|
battgo_info_t.channal_num = data_buf[1]; |
|
battgo_info_t.connect_status = data_buf[2]; |
|
battgo_info_t.sub_hardware_ver = data_buf[3]; |
|
battgo_info_t.main_hardware_ver = data_buf[4]; |
|
battgo_info_t.sub_software_ver = data_buf[5]; |
|
battgo_info_t.main_software_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] & 0x3f; |
|
uint8_t min = ((data_buf[32] & 0xc0) >> 6) | ((data_buf[33] & 0x0F) << 2); |
|
uint8_t hour = ((data_buf[33] & 0xF0) >> 4) | ((data_buf[34] & 0x01) << 5); |
|
uint8_t day = (data_buf[34] & 0x3e) >> 1; |
|
uint8_t month = ((data_buf[34] & 0xc0) >> 6) | ((data_buf[35] & 0x03) << 2); |
|
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 + 36]; |
|
} |
|
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]; |
|
battgo_info_t.request_tag = 0; //reset the request |
|
gcs().update_serial_battgo_info(&battgo_info_t); |
|
return true; |
|
} |
|
else if (data_buf[0] == 0x45) // |
|
{ |
|
// gcs().send_text(MAV_SEVERITY_INFO, "into battgo 0x45 battgo history"); |
|
battgo_history_t.channal_num = data_buf[1]; |
|
battgo_history_t.channal_num = data_buf[2]; |
|
battgo_history_t.last_charge_cap = (data_buf[6] << 24) | (data_buf[5] << 16) | (data_buf[4] << 8) | data_buf[3]; |
|
|
|
for (int i = 0; i < 6; i++) |
|
{ |
|
battgo_history_t.battery_core_vol[i] = (data_buf[7 + 2 * i + 1] << 8) | data_buf[7 + 2 * i]; |
|
} |
|
|
|
for (int i = 0; i < 6; i++) |
|
{ |
|
battgo_history_t.Internal_resistance[i] = (data_buf[19 + 2 * i + 1] << 8) | data_buf[19 + 2 * i]; |
|
} |
|
battgo_history_t.charge_state = data_buf[31]; |
|
|
|
gcs().update_serial_battgo_history(&battgo_history_t); |
|
return true; |
|
} |
|
return false; |
|
} |
|
|
|
void AP_BattMonitor_Serial_BattGo::read() |
|
{ |
|
requestBattData(BATTGO_CMD_REQUEST_ACTUAL_DATA); |
|
uint32_t tnow = AP_HAL::micros(); |
|
static uint32_t last_prt; |
|
if (get_reading()) |
|
{ |
|
if (parseBattGo()) |
|
{ |
|
last_rev_batt_time =tnow ; |
|
// timeout after 5 seconds |
|
if ((tnow - _interim_state.last_time_micros) > AP_BATTMONITOR_SERIAL_TIMEOUT_MICROS) |
|
{ |
|
_interim_state.healthy = false; |
|
} |
|
|
|
// Copy over relevant states over to main state |
|
// WITH_SEMAPHORE(_sem_battmon); |
|
|
|
_state.cell_voltages = _interim_state.cell_voltages; |
|
_state.voltage = _interim_state.voltage; |
|
_state.current_amps = _interim_state.current_amps; |
|
_state.consumed_mah = _interim_state.consumed_mah; |
|
_state.consumed_wh = _interim_state.consumed_wh; |
|
_state.last_time_micros = _interim_state.last_time_micros; |
|
//... |
|
_state.temperature = _interim_state.temperature; |
|
_state.temperature_time = _interim_state.temperature_time; |
|
//... |
|
_state.healthy = _interim_state.healthy; |
|
}else{ |
|
if(tnow-last_rev_batt_time>5000000 && tnow - last_prt > 20000000 ){ |
|
_interim_state.healthy = false; |
|
gcs().send_text(MAV_SEVERITY_WARNING,"BATT_ERR:已经%d秒没有接收到智能电池的串口数据！",(tnow-last_rev_batt_time)/1000000); |
|
last_prt = tnow; |
|
} |
|
} |
|
}else{ |
|
if(tnow-last_rev_batt_time>5000000 && tnow - last_prt > 20000000 ){ |
|
_interim_state.healthy = false; |
|
gcs().send_text(MAV_SEVERITY_WARNING,"BATT_ERR:已经%d秒没有接收到智能电池的串口数据！",(tnow-last_rev_batt_time)/1000000); |
|
last_prt = tnow; |
|
} |
|
} |
|
} |
|
|
|
void AP_BattMonitor_Serial_BattGo::notice_to_send_battgo_param() |
|
{ |
|
gcs().send_message(MSG_BATTGAO_INFO); |
|
} |
|
|
|
void AP_BattMonitor_Serial_BattGo::request_info() |
|
{ |
|
requestBattData(BATTGO_CMD_REQUEST_INFO); |
|
} |
|
|
|
void AP_BattMonitor_Serial_BattGo::request_history() |
|
{ |
|
requestBattData(BATTGO_CMD_REQUEST_HISTORY); |
|
} |
|
|
|
void AP_BattMonitor_Serial_BattGo::rxProcess(uint8_t data) |
|
{ |
|
if (data == 0x55) |
|
{ |
|
m_SyncByteCount++; |
|
if (m_SyncByteCount & 0x01) |
|
return; |
|
} |
|
else |
|
{ |
|
if (m_SyncByteCount & 0x01) |
|
m_eRxStatus = rsWaitAddres; |
|
m_SyncByteCount = 0; |
|
} |
|
|
|
switch (m_eRxStatus) |
|
{ |
|
case rsWaitSync: |
|
break; |
|
|
|
case rsWaitAddres: |
|
{ |
|
if (data != 0x21) |
|
{ |
|
m_eRxStatus = rsWaitSync; |
|
break; |
|
} |
|
check_sum = data; |
|
m_eRxStatus = rsWaitLength; |
|
} |
|
break; |
|
|
|
case rsWaitLength: |
|
{ |
|
if (data < 2) |
|
{ |
|
m_eRxStatus = rsWaitSync; |
|
break; |
|
} |
|
m_u08RXCnt = 0; |
|
payload_length = data; |
|
check_sum += data; |
|
m_eRxStatus = rsWaitData; |
|
} |
|
break; |
|
|
|
case rsWaitData: |
|
{ |
|
data_buf[m_u08RXCnt] = data; |
|
m_u08RXCnt++; |
|
check_sum += data; |
|
if (m_u08RXCnt >= payload_length) |
|
m_eRxStatus = rsWaitChkSumL; |
|
} |
|
break; |
|
|
|
case rsWaitChkSumL: |
|
{ |
|
|
|
if (data == (check_sum & 0xFF)) |
|
m_eRxStatus = rsWaitChkSumH; |
|
else |
|
m_eRxStatus = rsWaitSync; |
|
} |
|
break; |
|
|
|
case rsWaitChkSumH: |
|
{ |
|
if (data == ((check_sum >> 8) & 0xFF)) |
|
{ |
|
//m_bMessege = TRUE; |
|
isparse = true; |
|
} |
|
m_eRxStatus = rsWaitSync; |
|
} |
|
break; |
|
|
|
default: |
|
{ |
|
m_eRxStatus = rsWaitSync; |
|
} |
|
break; |
|
} |
|
}
|
|
|