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

battgo 历史、与参数信息获取与发送

master
yaozb 5 years ago
parent
0d6bb15488
commit
eeeb0d99c2
16 changed files with 172 additions and 132 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 7
      ArduCopter/GCS_Mavlink.cpp
  2. 25
      libraries/AP_BattMonitor/AP_BattMonitor.cpp
  3. 3
      libraries/AP_BattMonitor/AP_BattMonitor.h
  4. 5
      libraries/AP_BattMonitor/AP_BattMonitor_Analog.h
  5. 3
      libraries/AP_BattMonitor/AP_BattMonitor_BLHeliESC.h
  6. 3
      libraries/AP_BattMonitor/AP_BattMonitor_Backend.h
  7. 3
      libraries/AP_BattMonitor/AP_BattMonitor_Bebop.h
  8. 3
      libraries/AP_BattMonitor/AP_BattMonitor_FuelFlow.h
  9. 3
      libraries/AP_BattMonitor/AP_BattMonitor_FuelLevel_PWM.h
  10. 3
      libraries/AP_BattMonitor/AP_BattMonitor_SMBus.h
  11. 198
      libraries/AP_BattMonitor/AP_BattMonitor_Serial_BattGo.cpp
  12. 7
      libraries/AP_BattMonitor/AP_BattMonitor_Serial_BattGo.h
  13. 3
      libraries/AP_BattMonitor/AP_BattMonitor_Sum.h
  14. 3
      libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.h
  15. 3
      libraries/GCS_MAVLink/GCS.h
  16. 32
      libraries/GCS_MAVLink/GCS_Common.cpp

7
ArduCopter/GCS_Mavlink.cpp
Unescape View File

@ -1256,7 +1256,14 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg) @@ -1256,7 +1256,14 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg)
copter.g2.toy_mode.handle_message(msg);
break;
#endif
case MAVLINK_MSG_ID_BATTGO_INFO:
handle_battgo_info(msg);
break;
case MAVLINK_MSG_ID_BATTGO_HISTORY:
handle_battgo_history(msg);
break;
default:
handle_common_message(msg);
break;

25
libraries/AP_BattMonitor/AP_BattMonitor.cpp
Unescape View File

@ -430,8 +430,6 @@ const AP_BattMonitor::cells & AP_BattMonitor::get_cell_voltages(const uint8_t in @@ -430,8 +430,6 @@ 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;
}
}
@ -443,9 +441,7 @@ bool AP_BattMonitor::get_temperature(float &temperature, const uint8_t instance) @@ -443,9 +441,7 @@ bool AP_BattMonitor::get_temperature(float &temperature, const uint8_t instance)
return false;
} else {
temperature = state[instance].temperature;
// 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;
return (AP_HAL::millis() - state[instance].temperature_time) <= AP_BATT_MONITOR_TIMEOUT;
}
}
@ -484,6 +480,25 @@ void AP_BattMonitor::checkPoweringOff(void) @@ -484,6 +480,25 @@ void AP_BattMonitor::checkPoweringOff(void)
}
}
void AP_BattMonitor::request_info(void)
{
for (uint8_t i=0; i<_num_instances; i++) {
if (drivers[i] != nullptr && _params[i].type() == AP_BattMonitor_Params::BattMonitor_Serial_BattGo) {
drivers[i]->request_info();
}
}
}
void AP_BattMonitor::request_history(void)
{
for (uint8_t i=0; i<_num_instances; i++) {
if (drivers[i] != nullptr && _params[i].type() == AP_BattMonitor_Params::BattMonitor_Serial_BattGo) {
drivers[i]->request_history();
}
}
}
/*
reset battery remaining percentage for batteries that integrate to
calculate percentage remaining

3
libraries/AP_BattMonitor/AP_BattMonitor.h
Unescape View File

@ -164,7 +164,8 @@ public: @@ -164,7 +164,8 @@ public:
void send_serial_batteryGo_param(mavlink_channel_t chan);
void request_info(void) ; //battgo
void request_history(void) ; //battgo
protected:

5
libraries/AP_BattMonitor/AP_BattMonitor_Analog.h
Unescape View File

@ -84,7 +84,7 @@ public: @@ -84,7 +84,7 @@ public:
/// Read the battery voltage and current. Should be called at 10hz
void read() override;
/// returns true if battery monitor provides consumed energy info
bool has_consumed_energy() const override { return has_current(); }
@ -93,6 +93,9 @@ public: @@ -93,6 +93,9 @@ public:
void init(void) override {}
void request_info(void)override {};
void request_history(void)override {};
protected:
AP_HAL::AnalogSource *_volt_pin_analog_source;

3
libraries/AP_BattMonitor/AP_BattMonitor_BLHeliESC.h
Unescape View File

@ -33,7 +33,8 @@ public: @@ -33,7 +33,8 @@ public:
// read the latest battery voltage
void read() override;
void request_info(void)override {};
void request_history(void)override {};
// BLHeliESC provides current info
bool has_current() const override { return have_current; };

3
libraries/AP_BattMonitor/AP_BattMonitor_Backend.h
Unescape View File

@ -34,6 +34,9 @@ public: @@ -34,6 +34,9 @@ public:
// read the latest battery voltage
virtual void read() = 0;
virtual void request_info(void) =0;
virtual void request_history(void) =0;
/// returns true if battery monitor instance provides consumed energy info
virtual bool has_consumed_energy() const { return false; }

3
libraries/AP_BattMonitor/AP_BattMonitor_Bebop.h
Unescape View File

@ -43,6 +43,9 @@ public: @@ -43,6 +43,9 @@ public:
// don't allow reset of remaining capacity for bebop battery
bool reset_remaining(float percentage) override { return false; }
void request_info(void)override {};
void request_history(void)override {};
private:
float _prev_vbat_raw;
float _prev_vbat;

3
libraries/AP_BattMonitor/AP_BattMonitor_FuelFlow.h
Unescape View File

@ -21,6 +21,9 @@ public: @@ -21,6 +21,9 @@ public:
void init(void) override {}
void request_info(void)override {};
void request_history(void)override {};
private:
void irq_handler(uint8_t pin, bool pin_state, uint32_t timestamp);

3
libraries/AP_BattMonitor/AP_BattMonitor_FuelLevel_PWM.h
Unescape View File

@ -21,6 +21,9 @@ public: @@ -21,6 +21,9 @@ public:
void init(void) override {}
void request_info(void)override {};
void request_history(void)override {};;
private:
void irq_handler(uint8_t pin, bool pin_state, uint32_t timestamp);

3
libraries/AP_BattMonitor/AP_BattMonitor_SMBus.h
Unescape View File

@ -47,6 +47,9 @@ public: @@ -47,6 +47,9 @@ public:
void init(void) override;
void request_info(void)override {};
void request_history(void)override {};
protected:
void read(void) override;

198
libraries/AP_BattMonitor/AP_BattMonitor_Serial_BattGo.cpp
Unescape View File

@ -10,8 +10,7 @@ extern const AP_HAL::HAL &hal; @@ -10,8 +10,7 @@ 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 &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();
@ -36,16 +35,15 @@ void AP_BattMonitor_Serial_BattGo::init(void) @@ -36,16 +35,15 @@ void AP_BattMonitor_Serial_BattGo::init(void)
{
if (uart != nullptr)
{
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);
requestBattData(BATTGO_CMD_REQUEST_ACTUAL_DATA);
requestBattData(BATTGO_CMD_REQUEST_INFO);
requestBattData(BATTGO_CMD_REQUEST_HISTORY);
}
}
bool isparse =false;
bool isparse = false;
bool AP_BattMonitor_Serial_BattGo::get_reading() //TODO need to Refactor
{
if (uart == nullptr)
{
return false;
@ -53,14 +51,15 @@ bool AP_BattMonitor_Serial_BattGo::get_reading() //TODO need to Refactor @@ -53,14 +51,15 @@ bool AP_BattMonitor_Serial_BattGo::get_reading() //TODO need to Refactor
int16_t numc;
numc = uart->available();
uint8_t data;
if(numc == 0)
gcs().send_text(MAV_SEVERITY_INFO,"uasart num:%d",numc);
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){
rxProcess(data);
if (isparse)
{
isparse = true;
return true;
}
@ -68,13 +67,6 @@ bool AP_BattMonitor_Serial_BattGo::get_reading() //TODO need to Refactor @@ -68,13 +67,6 @@ bool AP_BattMonitor_Serial_BattGo::get_reading() //TODO need to Refactor
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::requestBattData(uint8_t data)
{
//发送data主要 0x40 0x42 0x44 地址0x21 及计算crc 均不会出现0x55，故不做0x55处理
@ -90,54 +82,35 @@ void AP_BattMonitor_Serial_BattGo::requestBattData(uint8_t data) @@ -90,54 +82,35 @@ void AP_BattMonitor_Serial_BattGo::requestBattData(uint8_t data)
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
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++;
// }
// }
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;
_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;
// 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.voltage += valtemp / 1000.0;
_has_cell_voltages = true;
}
//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 = (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;
//gcs().send_text(MAV_SEVERITY_INFO," _interim_state.current_amps: %f", _interim_state.current_amps/1000.0);
_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)
{
@ -156,13 +129,13 @@ bool AP_BattMonitor_Serial_BattGo::parseBattGo() @@ -156,13 +129,13 @@ bool AP_BattMonitor_Serial_BattGo::parseBattGo()
else if (data_buf[0] == 0x41)
{
//电池参数
// gcs().send_text(MAV_SEVERITY_INFO,"into battgo 0x41 battgo info");
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];
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];
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];
@ -172,15 +145,15 @@ bool AP_BattMonitor_Serial_BattGo::parseBattGo() @@ -172,15 +145,15 @@ bool AP_BattMonitor_Serial_BattGo::parseBattGo()
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];
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;
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;
@ -190,7 +163,7 @@ bool AP_BattMonitor_Serial_BattGo::parseBattGo() @@ -190,7 +163,7 @@ bool AP_BattMonitor_Serial_BattGo::parseBattGo()
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.battery_brand[i] = data_buf[i + 36];
}
battgo_info_t.type = data_buf[52];
battgo_info_t.core_amount = data_buf[53];
@ -206,43 +179,28 @@ bool AP_BattMonitor_Serial_BattGo::parseBattGo() @@ -206,43 +179,28 @@ bool AP_BattMonitor_Serial_BattGo::parseBattGo()
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);
battgo_info_t.request_tag = 0; //reset the request
gcs().update_serial_battgo_info(&battgo_info_t);
//requestBattData(BATTGO_CMD_REQUEST_HISTORY);
return true;
}
else if (data_buf[0] == 0x45) //
{
// 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];
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[8 + 2 * i + 1] << 8) | data_buf[8 + 2 * 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[20 + 2 * i + 1] << 8) | data_buf[20 + 2 * 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[32];
battgo_history_t.charge_state = data_buf[31];
gcs().update_serial_battgo_history(&battgo_history_t);
return true;
}
@ -251,34 +209,35 @@ bool AP_BattMonitor_Serial_BattGo::parseBattGo() @@ -251,34 +209,35 @@ 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)
if (parseBattGo())
{
_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;
}
uint32_t tnow = AP_HAL::micros();
// 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;
}
}
}
@ -287,20 +246,29 @@ void AP_BattMonitor_Serial_BattGo::notice_to_send_battgo_param() @@ -287,20 +246,29 @@ 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) {
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 ;
return;
}
else
{
if (m_SyncByteCount & 0x01)
if (m_SyncByteCount & 0x01)
m_eRxStatus = rsWaitAddres;
m_SyncByteCount = 0;
}
switch (m_eRxStatus)
@ -346,7 +314,7 @@ void AP_BattMonitor_Serial_BattGo::rxProcess(uint8_t data) { @@ -346,7 +314,7 @@ void AP_BattMonitor_Serial_BattGo::rxProcess(uint8_t data) {
case rsWaitChkSumL:
{
if (data == (check_sum & 0xFF))
m_eRxStatus = rsWaitChkSumH;
else
@ -360,7 +328,6 @@ void AP_BattMonitor_Serial_BattGo::rxProcess(uint8_t data) { @@ -360,7 +328,6 @@ void AP_BattMonitor_Serial_BattGo::rxProcess(uint8_t data) {
{
//m_bMessege = TRUE;
isparse = true;
}
m_eRxStatus = rsWaitSync;
}
@ -372,5 +339,4 @@ void AP_BattMonitor_Serial_BattGo::rxProcess(uint8_t data) { @@ -372,5 +339,4 @@ void AP_BattMonitor_Serial_BattGo::rxProcess(uint8_t data) {
}
break;
}
}

7
libraries/AP_BattMonitor/AP_BattMonitor_Serial_BattGo.h
Unescape View File

@ -5,7 +5,7 @@ @@ -5,7 +5,7 @@
#include "AP_BattMonitor_Backend.h"
#define AP_BATTMONITOR_SERIAL_TIMEOUT_MICROS 5000000
#define BATTGO_CMD_REQUEST_PARAM 0x40
#define BATTGO_CMD_REQUEST_INFO 0x40
#define BATTGO_CMD_REQUEST_ACTUAL_DATA 0x42
#define BATTGO_CMD_REQUEST_HISTORY 0x44
class AP_BattMonitor_Serial_BattGo : public AP_BattMonitor_Backend
@ -43,6 +43,10 @@ typedef enum @@ -43,6 +43,10 @@ typedef enum
/// Read the battery voltage and current. Should be called at 10hz
void read() override;
void request_info(void) override;
void request_history(void) override;
bool _has_cell_voltages;
bool has_cell_voltages() const override { return _has_cell_voltages; }
@ -51,7 +55,6 @@ typedef enum @@ -51,7 +55,6 @@ typedef enum
void update_battgo_param_mavlink(mavlink_channel_t chan,mavlink_battgo_info_t &mavlink_battgo_info_t);
void notice_to_send_battgo_param();
private:
AP_HAL::UARTDriver *uart = nullptr;

3
libraries/AP_BattMonitor/AP_BattMonitor_Sum.h
Unescape View File

@ -21,6 +21,9 @@ public: @@ -21,6 +21,9 @@ public:
void init(void) override {}
void request_info(void)override {};
void request_history(void)override {};
private:
uint8_t _instance;
bool _has_current;

3
libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.h
Unescape View File

@ -28,6 +28,9 @@ public: @@ -28,6 +28,9 @@ public:
return true;
}
void request_info(void)override {};
void request_history(void)override {};
static void subscribe_msgs(AP_UAVCAN* ap_uavcan);
static AP_BattMonitor_UAVCAN* get_uavcan_backend(AP_UAVCAN* ap_uavcan, uint8_t node_id);
static void handle_battery_info_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const BattInfoCb &cb);

3
libraries/GCS_MAVLink/GCS.h
Unescape View File

@ -445,7 +445,8 @@ protected: @@ -445,7 +445,8 @@ protected:
virtual bool allow_disarm() const { return true; }
void manual_override(RC_Channel *c, int16_t value_in, uint16_t offset, float scaler, const uint32_t tnow, bool reversed = false);
void handle_battgo_info(const mavlink_message_t &msg);
void handle_battgo_history(const mavlink_message_t &msg);
private:
void log_mavlink_stats();

32
libraries/GCS_MAVLink/GCS_Common.cpp
Unescape View File

@ -192,10 +192,6 @@ void GCS_MAVLINK::send_battery_status(const uint8_t instance) const @@ -192,10 +192,6 @@ 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
@ -3219,6 +3215,7 @@ void GCS_MAVLINK::handle_common_message(const mavlink_message_t &msg) @@ -3219,6 +3215,7 @@ void GCS_MAVLINK::handle_common_message(const mavlink_message_t &msg)
case MAVLINK_MSG_ID_OPTICAL_FLOW:
handle_optical_flow(msg);
break;
}
}
@ -3286,8 +3283,13 @@ void GCS_MAVLINK::handle_send_autopilot_version(const mavlink_message_t &msg) @@ -3286,8 +3283,13 @@ void GCS_MAVLINK::handle_send_autopilot_version(const mavlink_message_t &msg)
{
send_message(MSG_AUTOPILOT_VERSION);
send_banner();
if(!AP::arming().is_armed()){
AP::battery().request_info();
hal.scheduler->delay(500);
AP::battery().request_history();
}
}
/* */
void GCS_MAVLINK::send_banner()
{
#if 0
@ -3966,6 +3968,26 @@ void GCS_MAVLINK::handle_command_int(const mavlink_message_t &msg) @@ -3966,6 +3968,26 @@ void GCS_MAVLINK::handle_command_int(const mavlink_message_t &msg)
hal.util->persistent_data.last_mavlink_cmd = 0;
}
void GCS_MAVLINK::handle_battgo_info(const mavlink_message_t &msg)
{
mavlink_battgo_info_t packet;
mavlink_msg_battgo_info_decode(&msg, &packet);
if (packet. request_tag== 1)
{
AP::battery().request_info();
}
}
void GCS_MAVLINK::handle_battgo_history(const mavlink_message_t &msg){
mavlink_battgo_history_t packet;
mavlink_msg_battgo_history_decode(&msg, &packet);
if (packet.request_tag == 1)
{
AP::battery().request_history();
}
}
void GCS::try_send_queued_message_for_type(MAV_MISSION_TYPE type) {
MissionItemProtocol *prot = get_prot_for_mission_type(type);
if (prot == nullptr) {

Write Preview
Loading…
Cancel
Save