zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

实时数据基本ok

master
yaozb 5 years ago
parent
1c18717448
commit
0d6bb15488
5 changed files with 228 additions and 283 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 8
      libraries/AP_BattMonitor/AP_BattMonitor.cpp
  2. 6
      libraries/AP_BattMonitor/AP_BattMonitor.h
  3. 442
      libraries/AP_BattMonitor/AP_BattMonitor_Serial_BattGo.cpp
  4. 50
      libraries/AP_BattMonitor/AP_BattMonitor_Serial_BattGo.h
  5. 5
      libraries/GCS_MAVLink/GCS_Common.cpp

8
libraries/AP_BattMonitor/AP_BattMonitor.cpp
Unescape View File

@ -147,7 +147,7 @@ AP_BattMonitor::init()
drivers[instance] = new AP_BattMonitor_FuelLevel_PWM(*this, state[instance], _params[instance]); drivers[instance] = new AP_BattMonitor_FuelLevel_PWM(*this, state[instance], _params[instance]);
break; break;
case AP_BattMonitor_Params::BattMonitor_Serial_BattGo: 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; break;
case AP_BattMonitor_Params::BattMonitor_TYPE_NONE: case AP_BattMonitor_Params::BattMonitor_TYPE_NONE:
default: 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) { if (instance >= AP_BATT_MONITOR_MAX_INSTANCES) {
return state[AP_BATT_PRIMARY_INSTANCE].cell_voltages; return state[AP_BATT_PRIMARY_INSTANCE].cell_voltages;
} else { } else {
// gcs().send_text(MAV_SEVERITY_INFO,"cell0:%d",state[instance].cell_voltages.cells[0]);
return state[instance].cell_voltages; return state[instance].cell_voltages;
} }
} }
@ -441,7 +443,9 @@ bool AP_BattMonitor::get_temperature(float &temperature, const uint8_t instance)
return false; return false;
} else { } else {
temperature = state[instance].temperature; 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;
} }
} }

6
libraries/AP_BattMonitor/AP_BattMonitor.h
Unescape View File

@ -69,10 +69,10 @@ public:
cells cell_voltages; // battery cell voltages in millivolts, 10 cells matches the MAVLink spec cells cell_voltages; // battery cell voltages in millivolts, 10 cells matches the MAVLink spec
float voltage; // voltage in volts float voltage; // voltage in volts
float current_amps; // current in amperes float current_amps; // current in amperes
float consumed_mah; // total current draw in milliamp hours 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 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 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 uint32_t critical_voltage_start_ms; // critical voltage failsafe start timer in milliseconds
float temperature; // battery temperature in degrees Celsius float temperature; // battery temperature in degrees Celsius
uint32_t temperature_time; // timestamp of the last received temperature message uint32_t temperature_time; // timestamp of the last received temperature message

442
libraries/AP_BattMonitor/AP_BattMonitor_Serial_BattGo.cpp
Unescape View File

@ -10,14 +10,15 @@ extern const AP_HAL::HAL &hal;
AP_BattMonitor_Serial_BattGo::AP_BattMonitor_Serial_BattGo(AP_BattMonitor &mon, AP_BattMonitor_Serial_BattGo::AP_BattMonitor_Serial_BattGo(AP_BattMonitor &mon,
AP_BattMonitor::BattMonitor_State &mon_state, AP_BattMonitor::BattMonitor_State &mon_state,
AP_BattMonitor_Params &params, AP_BattMonitor_Params &params,
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(); const AP_SerialManager &serial_manager = AP::serialmanager();
uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Battery, serial_instance); uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Battery, serial_instance);
if (uart != nullptr) 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 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) void AP_BattMonitor_Serial_BattGo::init(void)
{ {
if (uart != nullptr) if (uart != nullptr)
{ {
requestBattData(BATTGO_CMD_REQUEST_ACTUAL_DATA); gcs().send_text(MAV_SEVERITY_INFO,"into BattGo::init()");
requestBattData(BATTGO_CMD_REQUEST_PARAM); requestBattData(BATTGO_CMD_REQUEST_ACTUAL_DATA);
requestBattData(BATTGO_CMD_REQUEST_HISTORY); requestBattData(BATTGO_CMD_REQUEST_PARAM);
requestBattData(BATTGO_CMD_REQUEST_HISTORY);
} }
} }
bool isparse =false;
bool AP_BattMonitor_Serial_BattGo::get_reading() //TODO need to Refactor bool AP_BattMonitor_Serial_BattGo::get_reading() //TODO need to Refactor
{ {
if (uart == nullptr) if (uart == nullptr)
{ {
return false; return false;
} }
int16_t numc; int16_t numc;
numc = uart->available(); numc = uart->available();
uint8_t data; uint8_t data;
if(numc == 0)
bool parsed = false; gcs().send_text(MAV_SEVERITY_INFO,"uasart num:%d",numc);
isparse = false;
for (int16_t i = 0; i < numc; i++) for (int16_t i = 0; i < numc; i++)
{ {
data = uart->read(); data = uart->read();
switch (step) rxProcess(data);
{ if(isparse){
case 0: isparse = true;
if (Head1 == data) return true;
{
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;
} }
} }
return parsed; return isparse;
} }
void AP_BattMonitor_Serial_BattGo::gotoStepAddr(uint8_t &data) // void AP_BattMonitor_Serial_BattGo::gotoStepAddr(uint8_t &data)
{ // {
step = 2; //next step // step = 2; //next step
addr = data; // addr = data;
check_sum = data; // check_sum = data;
} // }
void AP_BattMonitor_Serial_BattGo::requestBattData(uint8_t data) void AP_BattMonitor_Serial_BattGo::requestBattData(uint8_t data)
{ {
//发送data主要 0x40 0x42 0x44 地址0x21 及计算crc 均不会出现0x55，故不做0x55处理 //发送data主要 0x40 0x42 0x44 地址0x21 及计算crc 均不会出现0x55，故不做0x55处理
uint8_t send_data[6]; uint8_t send_data[13];
send_data[0] = 0x55; send_data[0] = 0x55;
send_data[1] = 0x21; send_data[1] = 0x12;
send_data[2] = 0x01; send_data[2] = 0x08;
send_data[3] = data; send_data[3] = data;
uint16_t crc = send_data[1] + send_data[2] + send_data[3]; send_data[4] = 0;
send_data[4] = crc & 0xff; send_data[5] = 0;
send_data[5] = crc >> 8 & 0xff; 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)) if (uart->txspace() > sizeof(send_data))
{ {
uart->write(&send_data[0], sizeof(send_data)); uart->write(&send_data[0], sizeof(send_data));
} }
} }
void AP_BattMonitor_Serial_BattGo::setPayLoadData(uint8_t &usart_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)
// { // {
// if (!is_read_header) // if (payload_counter < payload_length)
// { // {
// target_data = usart_data; // data_buf[payload_counter] = usart_data;
// switch (step) // payload_counter++;
// {
// 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;
// } // }
// else // 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() bool AP_BattMonitor_Serial_BattGo::parseBattGo()
{ {
if (data_buf[0] == 0x43) 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 tnow = AP_HAL::micros();
uint32_t dt = tnow - _interim_state.last_time_micros; uint32_t dt = tnow - _interim_state.last_time_micros;
_interim_state.voltage =0;
for (uint8_t i = 0; i < 6; i++) 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.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; //gcs().send_text(MAV_SEVERITY_INFO," _interim_state.cell_voltages: %f", _interim_state.voltage);
_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.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 // update total current drawn since startup
if (_interim_state.last_time_micros != 0 && dt < 2000000) 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) 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.channal_num = data_buf[1];
battgo_info_t.connect_status = data_buf[2]; battgo_info_t.connect_status = data_buf[2];
battgo_info_t.sub_software_ver = data_buf[3]; 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 hour = (data_buf[33] & 0xF0 << 4) | (data_buf[34] & 0x01 << 1);
uint8_t day = data_buf[34] & 0x3e >> 1; uint8_t day = data_buf[34] & 0x3e >> 1;
uint8_t month = (data_buf[34] & 0xc0 >> 6) | (data_buf[35] & 0x03); 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_year = year;
battgo_info_t.production_time_mon = month; 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.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.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_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.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_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.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.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.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.under_vol_abr_times = (data_buf[105] << 8) | data_buf[104];
battgo_info_t.soft_crash_times = data_buf[106]; 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.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_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.vol_stored_full_set = (data_buf[114] << 8) | data_buf[113];
battgo_info_t.self_discharge_times = (data_buf[115] << 8); battgo_info_t.self_discharge_times = (data_buf[115] << 8);
gcs().update_serial_battgo_info(&battgo_info_t); gcs().update_serial_battgo_info(&battgo_info_t);
//requestBattData(BATTGO_CMD_REQUEST_HISTORY); //requestBattData(BATTGO_CMD_REQUEST_HISTORY);
@ -412,17 +230,20 @@ bool AP_BattMonitor_Serial_BattGo::parseBattGo()
} }
else if (data_buf[0] == 0x45) // 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++){ 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]; {
} 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++){ 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.Internal_resistance[i] = (data_buf[20 + 2 * i + 1] << 8) | data_buf[20 + 2 * i];
}
battgo_history_t.charge_state = data_buf[32]; 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 true;
} }
return false; return false;
@ -430,9 +251,12 @@ bool AP_BattMonitor_Serial_BattGo::parseBattGo()
void AP_BattMonitor_Serial_BattGo::read() void AP_BattMonitor_Serial_BattGo::read()
{ {
requestBattData(0x42); requestBattData(0x42);
if (get_reading()) if (get_reading())
{ {
if (parseBattGo()){
uint32_t tnow = AP_HAL::micros(); uint32_t tnow = AP_HAL::micros();
// timeout after 5 seconds // 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)
@ -442,13 +266,19 @@ void AP_BattMonitor_Serial_BattGo::read()
// Copy over relevant states over to main state // Copy over relevant states over to main state
// WITH_SEMAPHORE(_sem_battmon); // WITH_SEMAPHORE(_sem_battmon);
_state.temperature = _interim_state.temperature;
_state.cell_voltages = _interim_state.cell_voltages;
_state.voltage = _interim_state.voltage; _state.voltage = _interim_state.voltage;
_state.current_amps = _interim_state.current_amps; _state.current_amps = _interim_state.current_amps;
_state.consumed_mah = _interim_state.consumed_mah; _state.consumed_mah = _interim_state.consumed_mah;
_state.consumed_wh = _interim_state.consumed_wh; _state.consumed_wh = _interim_state.consumed_wh;
_state.last_time_micros = _interim_state.last_time_micros; _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; _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); 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;
}
}

50
libraries/AP_BattMonitor/AP_BattMonitor_Serial_BattGo.h
Unescape View File

@ -1,3 +1,5 @@
#pragma once
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
#include "AP_BattMonitor.h" #include "AP_BattMonitor.h"
#include "AP_BattMonitor_Backend.h" #include "AP_BattMonitor_Backend.h"
@ -10,15 +12,25 @@ class AP_BattMonitor_Serial_BattGo : public AP_BattMonitor_Backend
{ {
public: public:
enum BattMonitor_Serial_BattGo_Type AP_BattMonitor_Serial_BattGo(AP_BattMonitor &mon,
{ AP_BattMonitor::BattMonitor_State &mon_state,
SERIAL_BATTGO_INFO = 0 AP_BattMonitor_Params &params,
}; uint8_t serial_instance);
AP_BattMonitor_Serial_BattGo(AP_BattMonitor &mon, // enum BattMonitor_Serial_BattGo_Type
AP_BattMonitor::BattMonitor_State &mon_state, // {
AP_BattMonitor_Params &params, // SERIAL_BATTGO_INFO = 0
uint8_t serial_instance); // };
typedef enum
{
rsWaitSync,
rsWaitAddres,
rsWaitLength,
rsWaitData,
rsWaitChkSumL,
rsWaitChkSumH,
}eComunicainot_t;
// ~AP_BattMonitor_Serial_BattGo(); // ~AP_BattMonitor_Serial_BattGo();
@ -31,18 +43,21 @@ public:
/// Read the battery voltage and current. Should be called at 10hz /// Read the battery voltage and current. Should be called at 10hz
void read() override; void read() override;
bool _has_cell_voltages;
bool has_cell_voltages() const override { return true; } bool has_cell_voltages() const override { return _has_cell_voltages; }
// static detection function // static detection function
static bool detect(uint8_t serial_instance); 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 update_battgo_param_mavlink(mavlink_channel_t chan,mavlink_battgo_info_t &mavlink_battgo_info_t);
void notice_to_send_battgo_param(); void notice_to_send_battgo_param();
private: private:
// get a reading
AP_HAL::UARTDriver *uart = nullptr; 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]; char linebuf[10];
uint8_t linebuf_len = 0; uint8_t linebuf_len = 0;
const int Head1 = 0x55; //数据包帧头 const int Head1 = 0x55; //数据包帧头
@ -53,15 +68,18 @@ private:
uint8_t payload_length; uint8_t payload_length;
uint8_t payload_counter; uint8_t payload_counter;
uint16_t check_sum; 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); void requestBattData(uint8_t data);
bool get_reading(); bool get_reading();
uint8_t crc_crc8(const uint8_t *p, uint8_t len); uint8_t crc_crc8(const uint8_t *p, uint8_t len);
void gotoStepAddr(uint8_t &data); //void gotoStepAddr(uint8_t &data);
void setPayLoadData(uint8_t &usart_data); //void setPayLoadData(uint8_t &usart_data);
bool parseBattGo(); 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); //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);
}; };

5
libraries/GCS_MAVLink/GCS_Common.cpp
Unescape View File

@ -192,11 +192,16 @@ void GCS_MAVLINK::send_battery_status(const uint8_t instance) const
const AP_BattMonitor &battery = AP::battery(); const AP_BattMonitor &battery = AP::battery();
float temp; float temp;
bool got_temperature = battery.get_temperature(temp, instance); 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 // 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 // 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 // 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 // report a pack up to 655.34 V with this method
//确保我们始终向GCS发送电压估算值，因为并不是所有的电池监视器都会监视单个电池，为此，如果后端不提供直接监视功能，我们将创建一组假电池以供使用，然后GCS可以恢复电池电量。
// 通过将所有不可忽略的电池值相加来获得电池组电压。 因为这是循环的，所以我们可以用这种方法报告高达655.34 V的电池组
AP_BattMonitor::cells fake_cells; AP_BattMonitor::cells fake_cells;
if (!battery.has_cell_voltages(instance)) { if (!battery.has_cell_voltages(instance)) {
float voltage = battery.voltage(instance) * 1e3f; float voltage = battery.voltage(instance) * 1e3f;

Write Preview
Loading…
Cancel
Save