diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.cpp b/libraries/AP_BattMonitor/AP_BattMonitor.cpp index 26b954add6..497a6f4c10 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor.cpp @@ -147,7 +147,7 @@ AP_BattMonitor::init() drivers[instance] = new AP_BattMonitor_FuelLevel_PWM(*this, state[instance], _params[instance]); break; case AP_BattMonitor_Params::BattMonitor_Serial_BattGo: - drivers[instance] = new AP_BattMonitor_Serial_BattGo(*this, state[instance], _params[instance],instance);//TODO serial_instance here is 0 + drivers[instance] = new AP_BattMonitor_Serial_BattGo(*this, state[instance], _params[instance],instance);//TODO break; case AP_BattMonitor_Params::BattMonitor_TYPE_NONE: default: @@ -430,6 +430,8 @@ const AP_BattMonitor::cells & AP_BattMonitor::get_cell_voltages(const uint8_t in if (instance >= AP_BATT_MONITOR_MAX_INSTANCES) { return state[AP_BATT_PRIMARY_INSTANCE].cell_voltages; } else { + + // gcs().send_text(MAV_SEVERITY_INFO,"cell0:%d",state[instance].cell_voltages.cells[0]); return state[instance].cell_voltages; } } @@ -441,7 +443,9 @@ bool AP_BattMonitor::get_temperature(float &temperature, const uint8_t instance) return false; } else { temperature = state[instance].temperature; - return (AP_HAL::millis() - state[instance].temperature_time) <= AP_BATT_MONITOR_TIMEOUT; + + // gcs().send_text(MAV_SEVERITY_INFO,"===now:%lu tt: %lu",AP_HAL::millis(),state[instance].temperature_time); + return ((int64_t)AP_HAL::millis() - (int64_t)state[instance].temperature_time) <= AP_BATT_MONITOR_TIMEOUT; } } diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.h b/libraries/AP_BattMonitor/AP_BattMonitor.h index 300a4a9438..5947182f21 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor.h @@ -69,10 +69,10 @@ public: cells cell_voltages; // battery cell voltages in millivolts, 10 cells matches the MAVLink spec float voltage; // voltage in volts float current_amps; // current in amperes - float consumed_mah; // total current draw in milliamp hours since start-up - float consumed_wh; // total energy consumed in Wh since start-up + float consumed_mah; // total current draw in milliamp hours since start-up + float consumed_wh; // total energy consumed in Wh since start-up uint32_t last_time_micros; // time when voltage and current was last read in microseconds - uint32_t low_voltage_start_ms; // time when voltage dropped below the minimum in milliseconds + uint32_t low_voltage_start_ms; // time when voltage dropped below the minimum in milliseconds uint32_t critical_voltage_start_ms; // critical voltage failsafe start timer in milliseconds float temperature; // battery temperature in degrees Celsius uint32_t temperature_time; // timestamp of the last received temperature message diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Serial_BattGo.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Serial_BattGo.cpp index bab79d984d..e1e15e4747 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Serial_BattGo.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Serial_BattGo.cpp @@ -10,14 +10,15 @@ 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) + 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_Rangefinder, serial_instance)); + uart->begin(serial_manager.find_baudrate(AP_SerialManager::SerialProtocol_Battery, serial_instance)); } }; @@ -28,298 +29,115 @@ AP_BattMonitor_Serial_BattGo::AP_BattMonitor_Serial_BattGo(AP_BattMonitor &mon, */ bool AP_BattMonitor_Serial_BattGo::detect(uint8_t serial_instance) //how to used { - return AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_Rangefinder, serial_instance) != nullptr; + 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_PARAM); - requestBattData(BATTGO_CMD_REQUEST_HISTORY); + gcs().send_text(MAV_SEVERITY_INFO,"into BattGo::init()"); + requestBattData(BATTGO_CMD_REQUEST_ACTUAL_DATA); + requestBattData(BATTGO_CMD_REQUEST_PARAM); + 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; - - bool parsed = false; + 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(); - switch (step) - { - case 0: - if (Head1 == data) - { - payload_counter = 0; - payload_length = 0; - step++; - } - is_read_header = false; - break; - case 1: //addr - if (!is_read_header) - { - if (Head1 != data) - { - addr = data; - check_sum = data; - step++; - } - else - { - is_read_header = true; - } - } - else //this data is 0x55 - { - is_read_header = false; - if (Head1 == data) // is repeat data - { - addr = data; - step++; - check_sum = data; - } - else - { //last data 0x55 is header, this data is addr,next step =2; - gotoStepAddr(data); - } - } - break; - case 2: //payload_length; - if (!is_read_header) - { - if (Head1 != data) - { - payload_length = data; - check_sum += data; - step++; - } - else - { - is_read_header = true; - } - } - else //this data is 0x55 - { - is_read_header = false; - if (Head1 == data) // is repeat data - { - payload_length = data; - check_sum += data; - step++; - } - else - { //last data 0x55 is header, this data is addr,next step =2; - gotoStepAddr(data); - } - } - break; - case 3: //payloaddata - if (!is_read_header) - { - if (Head1 != data) - { - setPayLoadData(data); - } - else - { - is_read_header = true; - } - } - else //this data is 0x55 - { - is_read_header = false; - if (Head1 == data) - { // is repeat data - setPayLoadData(data); - } - else - { //last data 0x55 is header, this data is addr,next step =2; - gotoStepAddr(data); - } - } - break; - case 4: //crc_a - if (!is_read_header) - { - if (Head1 == data) - { - is_read_header = true; - } - else - { - if ((check_sum & 0x00ff) != data) - { - step = 0; - } - else - { - step++; - } - } - } - else //this data is 0x55 - { - is_read_header = false; - if (Head1 == data) // is repeat data - { - if ((check_sum & 0x00ff) != data) - { - step = 0; - } - else - { - step++; - } - } - else - { //last data 0x55 is header, this data is addr,next step =2; - gotoStepAddr(data); - } - } - break; - case 5: //crc_b - if (!is_read_header) - { - if (Head1 == data) - { - is_read_header = true; - } - else - { - if (((check_sum >> 8) & 0x00ff) == data) - { - if (parseBattGo()) - parsed = true; - } - step = 0; - } - } - else //this data is 0x55 - { - is_read_header = false; - if (Head1 == data) // is repeat data - { - if (((check_sum >> 8) & 0x00ff) == data) - { - if (parseBattGo()) - parsed = true; - } - step = 0; - } - else - { //last data 0x55 is header, this data is addr,next step =2; - gotoStepAddr(data); - } - } - break; - default: - break; + rxProcess(data); + if(isparse){ + isparse = true; + return true; } } - return parsed; + return isparse; } -void AP_BattMonitor_Serial_BattGo::gotoStepAddr(uint8_t &data) -{ - step = 2; //next step - addr = data; - check_sum = data; -} +// void AP_BattMonitor_Serial_BattGo::gotoStepAddr(uint8_t &data) +// { +// step = 2; //next step +// addr = data; +// check_sum = data; +// } void AP_BattMonitor_Serial_BattGo::requestBattData(uint8_t data) { //发送data主要 0x40 0x42 0x44 地址0x21 及计算crc 均不会出现0x55，故不做0x55处理 - uint8_t send_data[6]; + uint8_t send_data[13]; send_data[0] = 0x55; - send_data[1] = 0x21; - send_data[2] = 0x01; + send_data[1] = 0x12; + send_data[2] = 0x08; send_data[3] = data; - uint16_t crc = send_data[1] + send_data[2] + send_data[3]; - send_data[4] = crc & 0xff; - send_data[5] = crc >> 8 & 0xff; + 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)); } } -void AP_BattMonitor_Serial_BattGo::setPayLoadData(uint8_t &usart_data) -{ - if (payload_counter < payload_length) - { - data_buf[payload_counter] = usart_data; - payload_counter++; - } - else - { - step++; - } -} - -// void AP_BattMonitor_Serial_BattGo::doWithReadHeaderData(uint8_t &usart_data, uint8_t target_data, uint8_t step, uint8_t &payload_length, uint8_t &payload_count, uint8_t *data_buf, bool &is_read_header) +// void AP_BattMonitor_Serial_BattGo::setPayLoadData(uint8_t &usart_data) // { -// if (!is_read_header) +// if (payload_counter < payload_length) // { -// target_data = usart_data; -// switch (step) -// { -// case 1: //addr -// check_sum = 0; -// step++; -// case 2: //payload_length; -// payload_length = usart_data; -// step++; -// break; -// case 3: //data -// if (payload_count < payload_length) -// { -// data_buf[payload_count] = usart_data; -// payload_count++; -// } -// break; -// default: -// break; -// } -// check_sum += usart_data; +// data_buf[payload_counter] = usart_data; +// payload_counter++; // } // else // { +// step++; // } - // } - mavlink_battgo_info_t battgo_info_t; - mavlink_battgo_history_t battgo_history_t ; + +mavlink_battgo_info_t battgo_info_t; +mavlink_battgo_history_t battgo_history_t; bool AP_BattMonitor_Serial_BattGo::parseBattGo() { - + if (data_buf[0] == 0x43) { //电池实时数据 + gcs().send_text(MAV_SEVERITY_INFO,"into battgo 0x43 battgo realtime data"); 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; + // gcs().send_text(MAV_SEVERITY_INFO," _interim_state.cell_voltages.cells[%d]: %d",i,valtemp); + _interim_state.voltage += valtemp/1000.0; + _has_cell_voltages = true; } - _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); + //gcs().send_text(MAV_SEVERITY_INFO," _interim_state.cell_voltages: %f", _interim_state.voltage); + _interim_state.temperature = (int8_t)data_buf[15] ; + //gcs().send_text(MAV_SEVERITY_INFO," _interim_state.temperature: %f", _interim_state.temperature); + _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; + //gcs().send_text(MAV_SEVERITY_INFO," _interim_state.current_amps: %f", _interim_state.current_amps/1000.0); // update total current drawn since startup if (_interim_state.last_time_micros != 0 && dt < 2000000) { @@ -338,7 +156,7 @@ bool AP_BattMonitor_Serial_BattGo::parseBattGo() 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_software_ver = data_buf[3]; @@ -362,7 +180,7 @@ bool AP_BattMonitor_Serial_BattGo::parseBattGo() 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; + uint16_t year = (data_buf[35] & 0xFC >> 2) + 2017; battgo_info_t.production_time_year = year; battgo_info_t.production_time_mon = month; @@ -393,18 +211,18 @@ bool AP_BattMonitor_Serial_BattGo::parseBattGo() 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.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.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); gcs().update_serial_battgo_info(&battgo_info_t); //requestBattData(BATTGO_CMD_REQUEST_HISTORY); @@ -412,17 +230,20 @@ bool AP_BattMonitor_Serial_BattGo::parseBattGo() } else if (data_buf[0] == 0x45) // { - battgo_history_t.last_charge_cap = (data_buf[7] << 24) | (data_buf[6] << 16) | (data_buf[5] << 8) |data_buf[4]; + // gcs().send_text(MAV_SEVERITY_INFO,"into battgo 0x45 battgo history"); + battgo_history_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_history_t.battery_core_vol[i] = (data_buf[8+2*i+1]<<8)|data_buf[8+2*i]; - } + for (int i = 0; i < 6; i++) + { + battgo_history_t.battery_core_vol[i] = (data_buf[8 + 2 * i + 1] << 8) | data_buf[8 + 2 * i]; + } - for(int i=0;i<6;i++){ - battgo_history_t.Internal_resistance[i] = (data_buf[20+2*i+1]<<8)|data_buf[20+2*i]; - } + for (int i = 0; i < 6; i++) + { + battgo_history_t.Internal_resistance[i] = (data_buf[20 + 2 * i + 1] << 8) | data_buf[20 + 2 * i]; + } battgo_history_t.charge_state = data_buf[32]; - gcs().update_serial_battgo_history(&battgo_history_t); + gcs().update_serial_battgo_history(&battgo_history_t); return true; } return false; @@ -430,9 +251,12 @@ bool AP_BattMonitor_Serial_BattGo::parseBattGo() void AP_BattMonitor_Serial_BattGo::read() { - requestBattData(0x42); + requestBattData(0x42); + if (get_reading()) { + if (parseBattGo()){ + uint32_t tnow = AP_HAL::micros(); // timeout after 5 seconds if ((tnow - _interim_state.last_time_micros) > AP_BATTMONITOR_SERIAL_TIMEOUT_MICROS) @@ -442,13 +266,19 @@ void AP_BattMonitor_Serial_BattGo::read() // Copy over relevant states over to main state // WITH_SEMAPHORE(_sem_battmon); - _state.temperature = _interim_state.temperature; + + _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; + } } } @@ -456,3 +286,91 @@ void AP_BattMonitor_Serial_BattGo::notice_to_send_battgo_param() { gcs().send_message(MSG_BATTGAO_INFO); } + +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; + } + +} diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Serial_BattGo.h b/libraries/AP_BattMonitor/AP_BattMonitor_Serial_BattGo.h index 0aae394466..1ec645ae6b 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Serial_BattGo.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Serial_BattGo.h @@ -1,3 +1,5 @@ +#pragma once + #include #include "AP_BattMonitor.h" #include "AP_BattMonitor_Backend.h" @@ -10,15 +12,25 @@ class AP_BattMonitor_Serial_BattGo : public AP_BattMonitor_Backend { public: - enum BattMonitor_Serial_BattGo_Type - { - SERIAL_BATTGO_INFO = 0 - }; + AP_BattMonitor_Serial_BattGo(AP_BattMonitor &mon, + AP_BattMonitor::BattMonitor_State &mon_state, + AP_BattMonitor_Params ¶ms, + uint8_t serial_instance); - AP_BattMonitor_Serial_BattGo(AP_BattMonitor &mon, - AP_BattMonitor::BattMonitor_State &mon_state, - AP_BattMonitor_Params ¶ms, - uint8_t serial_instance); + // enum BattMonitor_Serial_BattGo_Type + // { + // SERIAL_BATTGO_INFO = 0 + // }; + +typedef enum +{ + rsWaitSync, + rsWaitAddres, + rsWaitLength, + rsWaitData, + rsWaitChkSumL, + rsWaitChkSumH, +}eComunicainot_t; // ~AP_BattMonitor_Serial_BattGo(); @@ -31,18 +43,21 @@ public: /// Read the battery voltage and current. Should be called at 10hz void read() override; - - bool has_cell_voltages() const override { return true; } + bool _has_cell_voltages; + bool has_cell_voltages() const override { return _has_cell_voltages; } // 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 AP_HAL::UARTDriver *uart = nullptr; - AP_BattMonitor::BattMonitor_State _interim_state; + + AP_BattMonitor::BattMonitor_State _interim_state; //TODO may need to fix + char linebuf[10]; uint8_t linebuf_len = 0; const int Head1 = 0x55; //数据包帧头 @@ -53,15 +68,18 @@ private: uint8_t payload_length; uint8_t payload_counter; uint16_t check_sum; - bool is_read_header = false; + + uint16_t m_SyncByteCount = 0; + uint8_t m_eRxStatus = 0; + uint8_t m_u08RXCnt; void requestBattData(uint8_t data); bool get_reading(); uint8_t crc_crc8(const uint8_t *p, uint8_t len); - void gotoStepAddr(uint8_t &data); - void setPayLoadData(uint8_t &usart_data); + //void gotoStepAddr(uint8_t &data); + //void setPayLoadData(uint8_t &usart_data); bool parseBattGo(); - + void rxProcess(uint8_t data); //void AP_BattMonitor_Serial_BattGo::doWithReadHeaderData(uint8_t &usart_data,uint8_t target_data,uint8_t step,uint8_t &payload_length,uint8_t &payload_count,uint8_t * data_buf,bool &is_read_header); }; diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 19d8f0ce17..ac058519eb 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -192,11 +192,16 @@ void GCS_MAVLINK::send_battery_status(const uint8_t instance) const const AP_BattMonitor &battery = AP::battery(); float temp; bool got_temperature = battery.get_temperature(temp, instance); + // gcs().send_text(MAV_SEVERITY_INFO,"instance:%d got_temp_bool:%d temp:%f",instance,got_temperature,temp); +// gcs().send_text(MAV_SEVERITY_INFO,"instance:%d got_cell_bool:%d " ,instance,battery.has_cell_voltages(instance)); + // gcs().send_text(MAV_SEVERITY_INFO,"instance:%d cell0:%d " ,instance,battery.get_cell_voltages(instance).cells[0]); // ensure we always send a voltage estimate to the GCS, because not all battery monitors monitor individual cells // as a work around for this we create a set of fake cells to be used if the backend doesn't provide direct monitoring // the GCS can then recover the pack voltage by summing all non ignored cell values. Because this is looped we can // report a pack up to 655.34 V with this method + //确保我们始终向GCS发送电压估算值，因为并不是所有的电池监视器都会监视单个电池，为此，如果后端不提供直接监视功能，我们将创建一组假电池以供使用，然后GCS可以恢复电池电量。 + // 通过将所有不可忽略的电池值相加来获得电池组电压。 因为这是循环的，所以我们可以用这种方法报告高达655.34 V的电池组 AP_BattMonitor::cells fake_cells; if (!battery.has_cell_voltages(instance)) { float voltage = battery.voltage(instance) * 1e3f;