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
Unified 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)
copter.g2.toy_mode.handle_message(msg); copter.g2.toy_mode.handle_message(msg);
break; break;
#endif #endif
case MAVLINK_MSG_ID_BATTGO_INFO:
handle_battgo_info(msg);
break;
case MAVLINK_MSG_ID_BATTGO_HISTORY:
handle_battgo_history(msg);
break;
default: default:
handle_common_message(msg); handle_common_message(msg);
break; 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
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;
} }
} }
@ -443,9 +441,7 @@ 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;
} }
} }
@ -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 reset battery remaining percentage for batteries that integrate to
calculate percentage remaining calculate percentage remaining

3
libraries/AP_BattMonitor/AP_BattMonitor.h
Unescape View File

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

5
libraries/AP_BattMonitor/AP_BattMonitor_Analog.h
Unescape View File

@ -84,7 +84,7 @@ 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;
/// returns true if battery monitor provides consumed energy info /// returns true if battery monitor provides consumed energy info
bool has_consumed_energy() const override { return has_current(); } bool has_consumed_energy() const override { return has_current(); }
@ -93,6 +93,9 @@ public:
void init(void) override {} void init(void) override {}
void request_info(void)override {};
void request_history(void)override {};
protected: protected:
AP_HAL::AnalogSource *_volt_pin_analog_source; AP_HAL::AnalogSource *_volt_pin_analog_source;

3
libraries/AP_BattMonitor/AP_BattMonitor_BLHeliESC.h
Unescape View File

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

3
libraries/AP_BattMonitor/AP_BattMonitor_Backend.h
Unescape View File

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

3
libraries/AP_BattMonitor/AP_BattMonitor_Bebop.h
Unescape View File

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

3
libraries/AP_BattMonitor/AP_BattMonitor_FuelFlow.h
Unescape View File

@ -21,6 +21,9 @@ public:
void init(void) override {} void init(void) override {}
void request_info(void)override {};
void request_history(void)override {};
private: private:
void irq_handler(uint8_t pin, bool pin_state, uint32_t timestamp); 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:
void init(void) override {} void init(void) override {}
void request_info(void)override {};
void request_history(void)override {};;
private: private:
void irq_handler(uint8_t pin, bool pin_state, uint32_t timestamp); 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:
void init(void) override; void init(void) override;
void request_info(void)override {};
void request_history(void)override {};
protected: protected:
void read(void) override; 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;
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) : uint8_t serial_instance) : AP_BattMonitor_Backend(mon, mon_state, params)
AP_BattMonitor_Backend(mon, mon_state, params)
{ {
const AP_SerialManager &serial_manager = AP::serialmanager(); const AP_SerialManager &serial_manager = AP::serialmanager();
@ -36,16 +35,15 @@ void AP_BattMonitor_Serial_BattGo::init(void)
{ {
if (uart != nullptr) if (uart != nullptr)
{ {
gcs().send_text(MAV_SEVERITY_INFO,"into BattGo::init()"); requestBattData(BATTGO_CMD_REQUEST_ACTUAL_DATA);
requestBattData(BATTGO_CMD_REQUEST_ACTUAL_DATA); requestBattData(BATTGO_CMD_REQUEST_INFO);
requestBattData(BATTGO_CMD_REQUEST_PARAM); requestBattData(BATTGO_CMD_REQUEST_HISTORY);
requestBattData(BATTGO_CMD_REQUEST_HISTORY);
} }
} }
bool isparse =false; 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;
@ -53,14 +51,15 @@ bool AP_BattMonitor_Serial_BattGo::get_reading() //TODO need to Refactor
int16_t numc; int16_t numc;
numc = uart->available(); numc = uart->available();
uint8_t data; uint8_t data;
if(numc == 0) if (numc == 0)
gcs().send_text(MAV_SEVERITY_INFO,"uasart num:%d",numc); gcs().send_text(MAV_SEVERITY_INFO, "uasart num:%d", numc);
isparse = false; isparse = false;
for (int16_t i = 0; i < numc; i++) for (int16_t i = 0; i < numc; i++)
{ {
data = uart->read(); data = uart->read();
rxProcess(data); rxProcess(data);
if(isparse){ if (isparse)
{
isparse = true; isparse = true;
return true; return true;
} }
@ -68,13 +67,6 @@ bool AP_BattMonitor_Serial_BattGo::get_reading() //TODO need to Refactor
return isparse; 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) 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处理
@ -90,54 +82,35 @@ void AP_BattMonitor_Serial_BattGo::requestBattData(uint8_t data)
send_data[8] = 0; send_data[8] = 0;
send_data[9] = 0; send_data[9] = 0;
send_data[10] = 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 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[11] = crc & 0xff; //64
send_data[12] = crc >> 8 & 0xff; //00 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)
// {
// if (payload_counter < payload_length)
// {
// data_buf[payload_counter] = usart_data;
// payload_counter++;
// }
// else
// {
// step++;
// }
// }
mavlink_battgo_info_t battgo_info_t; mavlink_battgo_info_t battgo_info_t;
mavlink_battgo_history_t battgo_history_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; _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;
// gcs().send_text(MAV_SEVERITY_INFO," _interim_state.cell_voltages.cells[%d]: %d",i,valtemp); _interim_state.voltage += valtemp / 1000.0;
_interim_state.voltage += valtemp/1000.0; _has_cell_voltages = true;
_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];
_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.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; _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)
{ {
@ -156,13 +129,13 @@ 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"); 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_hardware_ver = data_buf[3];
battgo_info_t.main_software_ver = data_buf[4]; battgo_info_t.main_hardware_ver = data_buf[4];
battgo_info_t.sub_hardware_ver = data_buf[5]; battgo_info_t.sub_software_ver = data_buf[5];
battgo_info_t.main_hardware_ver = data_buf[6]; battgo_info_t.main_software_ver = data_buf[6];
for (int i = 0; i < 8; i++) for (int i = 0; i < 8; i++)
{ {
battgo_info_t.equipment_id[i] = data_buf[7 + i]; battgo_info_t.equipment_id[i] = data_buf[7 + i];
@ -172,15 +145,15 @@ bool AP_BattMonitor_Serial_BattGo::parseBattGo()
battgo_info_t.slave_id[i] = data_buf[15 + i]; battgo_info_t.slave_id[i] = data_buf[15 + i];
} }
battgo_info_t.station_id = data_buf[25]; battgo_info_t.station_id = data_buf[25];
battgo_info_t.customer_id = data_buf[27] << 8 & data_buf[26]; 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.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 sec = data_buf[32] & 0x3f;
uint8_t min = (data_buf[32] & 0xe0 >> 5) | (data_buf[33] & 0x0F << 4); 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 << 1); uint8_t hour = ((data_buf[33] & 0xF0) >> 4) | ((data_buf[34] & 0x01) << 5);
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) << 2);
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;
@ -190,7 +163,7 @@ bool AP_BattMonitor_Serial_BattGo::parseBattGo()
battgo_info_t.production_time_sec = sec; battgo_info_t.production_time_sec = sec;
for (int i = 0; i < 16; i++) 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.type = data_buf[52];
battgo_info_t.core_amount = data_buf[53]; battgo_info_t.core_amount = data_buf[53];
@ -206,43 +179,28 @@ bool AP_BattMonitor_Serial_BattGo::parseBattGo()
battgo_info_t.temp_store_nin = data_buf[72]; battgo_info_t.temp_store_nin = data_buf[72];
battgo_info_t.temp_store_max = data_buf[73]; battgo_info_t.temp_store_max = data_buf[73];
battgo_info_t.enable_auto_store = data_buf[74]; battgo_info_t.enable_auto_store = data_buf[74];
for (int i = 0; i < 6; i++) battgo_info_t.request_tag = 0; //reset the request
{
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);
gcs().update_serial_battgo_info(&battgo_info_t); gcs().update_serial_battgo_info(&battgo_info_t);
//requestBattData(BATTGO_CMD_REQUEST_HISTORY);
return true; return true;
} }
else if (data_buf[0] == 0x45) // else if (data_buf[0] == 0x45) //
{ {
// gcs().send_text(MAV_SEVERITY_INFO,"into battgo 0x45 battgo history"); 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]; 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++) 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++) 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); gcs().update_serial_battgo_history(&battgo_history_t);
return true; return true;
} }
@ -251,34 +209,35 @@ 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()){ if (parseBattGo())
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 uint32_t tnow = AP_HAL::micros();
// WITH_SEMAPHORE(_sem_battmon); // timeout after 5 seconds
if ((tnow - _interim_state.last_time_micros) > AP_BATTMONITOR_SERIAL_TIMEOUT_MICROS)
_state.cell_voltages = _interim_state.cell_voltages; {
_state.voltage = _interim_state.voltage; _interim_state.healthy = false;
_state.current_amps = _interim_state.current_amps; }
_state.consumed_mah = _interim_state.consumed_mah;
_state.consumed_wh = _interim_state.consumed_wh; // Copy over relevant states over to main state
_state.last_time_micros = _interim_state.last_time_micros; // WITH_SEMAPHORE(_sem_battmon);
//...
_state.temperature = _interim_state.temperature; _state.cell_voltages = _interim_state.cell_voltages;
_state.temperature_time = _interim_state.temperature_time; _state.voltage = _interim_state.voltage;
//... _state.current_amps = _interim_state.current_amps;
_state.healthy = _interim_state.healthy; _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()
gcs().send_message(MSG_BATTGAO_INFO); 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) if (data == 0x55)
{ {
m_SyncByteCount++; m_SyncByteCount++;
if (m_SyncByteCount & 0x01) if (m_SyncByteCount & 0x01)
return ; return;
} }
else else
{ {
if (m_SyncByteCount & 0x01) if (m_SyncByteCount & 0x01)
m_eRxStatus = rsWaitAddres; m_eRxStatus = rsWaitAddres;
m_SyncByteCount = 0; m_SyncByteCount = 0;
} }
switch (m_eRxStatus) switch (m_eRxStatus)
@ -346,7 +314,7 @@ void AP_BattMonitor_Serial_BattGo::rxProcess(uint8_t data) {
case rsWaitChkSumL: case rsWaitChkSumL:
{ {
if (data == (check_sum & 0xFF)) if (data == (check_sum & 0xFF))
m_eRxStatus = rsWaitChkSumH; m_eRxStatus = rsWaitChkSumH;
else else
@ -360,7 +328,6 @@ void AP_BattMonitor_Serial_BattGo::rxProcess(uint8_t data) {
{ {
//m_bMessege = TRUE; //m_bMessege = TRUE;
isparse = true; isparse = true;
} }
m_eRxStatus = rsWaitSync; m_eRxStatus = rsWaitSync;
} }
@ -372,5 +339,4 @@ void AP_BattMonitor_Serial_BattGo::rxProcess(uint8_t data) {
} }
break; break;
} }
} }

7
libraries/AP_BattMonitor/AP_BattMonitor_Serial_BattGo.h
Unescape View File

@ -5,7 +5,7 @@
#include "AP_BattMonitor_Backend.h" #include "AP_BattMonitor_Backend.h"
#define AP_BATTMONITOR_SERIAL_TIMEOUT_MICROS 5000000 #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_ACTUAL_DATA 0x42
#define BATTGO_CMD_REQUEST_HISTORY 0x44 #define BATTGO_CMD_REQUEST_HISTORY 0x44
class AP_BattMonitor_Serial_BattGo : public AP_BattMonitor_Backend class AP_BattMonitor_Serial_BattGo : public AP_BattMonitor_Backend
@ -43,6 +43,10 @@ typedef enum
/// 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;
void request_info(void) override;
void request_history(void) override;
bool _has_cell_voltages; bool _has_cell_voltages;
bool has_cell_voltages() const override { return _has_cell_voltages; } bool has_cell_voltages() const override { return _has_cell_voltages; }
@ -51,7 +55,6 @@ typedef enum
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:
AP_HAL::UARTDriver *uart = nullptr; AP_HAL::UARTDriver *uart = nullptr;

3
libraries/AP_BattMonitor/AP_BattMonitor_Sum.h
Unescape View File

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

3
libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.h
Unescape View File

@ -28,6 +28,9 @@ public:
return true; return true;
} }
void request_info(void)override {};
void request_history(void)override {};
static void subscribe_msgs(AP_UAVCAN* ap_uavcan); static void subscribe_msgs(AP_UAVCAN* ap_uavcan);
static AP_BattMonitor_UAVCAN* get_uavcan_backend(AP_UAVCAN* ap_uavcan, uint8_t node_id); 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); 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:
virtual bool allow_disarm() const { return true; } 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 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: private:
void log_mavlink_stats(); 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
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
@ -3219,6 +3215,7 @@ void GCS_MAVLINK::handle_common_message(const mavlink_message_t &msg)
case MAVLINK_MSG_ID_OPTICAL_FLOW: case MAVLINK_MSG_ID_OPTICAL_FLOW:
handle_optical_flow(msg); handle_optical_flow(msg);
break; break;
} }
} }
@ -3286,8 +3283,13 @@ void GCS_MAVLINK::handle_send_autopilot_version(const mavlink_message_t &msg)
{ {
send_message(MSG_AUTOPILOT_VERSION); send_message(MSG_AUTOPILOT_VERSION);
send_banner(); send_banner();
if(!AP::arming().is_armed()){
AP::battery().request_info();
hal.scheduler->delay(500);
AP::battery().request_history();
}
} }
/* */
void GCS_MAVLINK::send_banner() void GCS_MAVLINK::send_banner()
{ {
#if 0 #if 0
@ -3966,6 +3968,26 @@ void GCS_MAVLINK::handle_command_int(const mavlink_message_t &msg)
hal.util->persistent_data.last_mavlink_cmd = 0; 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) { void GCS::try_send_queued_message_for_type(MAV_MISSION_TYPE type) {
MissionItemProtocol *prot = get_prot_for_mission_type(type); MissionItemProtocol *prot = get_prot_for_mission_type(type);
if (prot == nullptr) { if (prot == nullptr) {

Write Preview
Loading…
Cancel
Save