diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_FuelLevel_PWM.h b/libraries/AP_BattMonitor/AP_BattMonitor_FuelLevel_PWM.h index 7a67b325e2..e04d96414e 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_FuelLevel_PWM.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_FuelLevel_PWM.h @@ -22,7 +22,7 @@ public: void init(void) override {} void request_info(void)override {}; - void request_history(void)override {};; + void request_history(void)override {}; private: void irq_handler(uint8_t pin, bool pin_state, uint32_t timestamp); diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Serial_BattGo.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Serial_BattGo.cpp index 8fd694fc57..b816218c13 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Serial_BattGo.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Serial_BattGo.cpp @@ -12,13 +12,14 @@ AP_BattMonitor_Serial_BattGo::AP_BattMonitor_Serial_BattGo(AP_BattMonitor &mon, AP_BattMonitor_Params ¶ms, uint8_t serial_instance) : AP_BattMonitor_Backend(mon, mon_state, params) { - + const AP_SerialManager &serial_manager = AP::serialmanager(); uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Battery, serial_instance); if (uart != nullptr) { - uart->begin(serial_manager.find_baudrate(AP_SerialManager::SerialProtocol_Battery, serial_instance)); + uart->begin(serial_manager.find_baudrate(AP_SerialManager::SerialProtocol_Battery, serial_instance)); } + last_rev_batt_time = 0; }; /* @@ -36,8 +37,8 @@ void AP_BattMonitor_Serial_BattGo::init(void) if (uart != nullptr) { requestBattData(BATTGO_CMD_REQUEST_ACTUAL_DATA); - requestBattData(BATTGO_CMD_REQUEST_INFO); - requestBattData(BATTGO_CMD_REQUEST_HISTORY); + // requestBattData(BATTGO_CMD_REQUEST_INFO); + // requestBattData(BATTGO_CMD_REQUEST_HISTORY); } } bool isparse = false; @@ -51,8 +52,8 @@ 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++) { @@ -95,7 +96,7 @@ mavlink_battgo_info_t battgo_info_t; mavlink_battgo_history_t battgo_history_t; bool AP_BattMonitor_Serial_BattGo::parseBattGo() { - + uint8_t offset =0; if (data_buf[0] == 0x43) { //电池实时数据 uint32_t tnow = AP_HAL::micros(); @@ -119,17 +120,36 @@ bool AP_BattMonitor_Serial_BattGo::parseBattGo() _interim_state.consumed_mah += mah; _interim_state.consumed_wh += 0.001f * mah * _interim_state.voltage; } - + + battgo_history_t.batt_cycled_time = (data_buf[25]<<8)+data_buf[24] ; + battgo_history_t.batt_abnormal_time = (data_buf[27]<<8)+data_buf[26] ; + battgo_history_t.temp_abnormal_time = (data_buf[29]<<8)+data_buf[28] ; + battgo_history_t.over_vol_abnormal_time= (data_buf[31]<<8)+data_buf[30] ; + offset = 32; + battgo_history_t.under_vol_abnormal_time = (data_buf[offset+1]<<8)+data_buf[offset] ; + offset = offset+2; + battgo_history_t.soft_crashes_time = data_buf[offset]; + offset++; + battgo_history_t.charging_current_setted = (data_buf[offset+3] + (data_buf[offset+2] << 8) + (data_buf[offset+1] << 16) + (data_buf[offset] << 24)) ; + offset = offset+4; + battgo_history_t.storage_vol_setted = (data_buf[offset+1]<<8)+data_buf[offset] ; + offset+=2; + battgo_history_t.charging_full_vol_setted = (data_buf[offset+1]<<8)+data_buf[offset] ; + offset+=2; + battgo_history_t.self_discharge_times_setted = data_buf[offset]; + //battgo_history_t.set // record time _interim_state.last_time_micros = tnow; _interim_state.healthy = true; + + return true; } else if (data_buf[0] == 0x41) { //电池参数 - gcs().send_text(MAV_SEVERITY_INFO, "into battgo 0x41 battgo info"); + //gcs().send_text(MAV_SEVERITY_INFO, "into battgo 0x41 battgo info"); battgo_info_t.channal_num = data_buf[1]; battgo_info_t.connect_status = data_buf[2]; battgo_info_t.sub_hardware_ver = data_buf[3]; @@ -185,7 +205,7 @@ bool AP_BattMonitor_Serial_BattGo::parseBattGo() } 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.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]; @@ -209,20 +229,19 @@ bool AP_BattMonitor_Serial_BattGo::parseBattGo() void AP_BattMonitor_Serial_BattGo::read() { - requestBattData(0x42); - + requestBattData(BATTGO_CMD_REQUEST_ACTUAL_DATA); + uint32_t tnow = AP_HAL::micros(); if (get_reading()) { if (parseBattGo()) - { - - uint32_t tnow = AP_HAL::micros(); + { + last_rev_batt_time =tnow ; // timeout after 5 seconds if ((tnow - _interim_state.last_time_micros) > AP_BATTMONITOR_SERIAL_TIMEOUT_MICROS) { _interim_state.healthy = false; } - + // Copy over relevant states over to main state // WITH_SEMAPHORE(_sem_battmon); @@ -237,7 +256,17 @@ void AP_BattMonitor_Serial_BattGo::read() _state.temperature_time = _interim_state.temperature_time; //... _state.healthy = _interim_state.healthy; + }else{ + if(tnow-last_rev_batt_time>5000000){ + _interim_state.healthy = false; + gcs().send_text(MAV_SEVERITY_INFO,"BATT_ERR:已经%d秒没有接收到智能电池 的串口数据！",(tnow-last_rev_batt_time)/1000000); + } } + }else{ + if(tnow-last_rev_batt_time>5000000){ + _interim_state.healthy = false; + gcs().send_text(MAV_SEVERITY_INFO,"BATT_ERR:已经%d秒没有接收到智能电池 的串口数据！",(tnow-last_rev_batt_time)/1000000); + } } } diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Serial_BattGo.h b/libraries/AP_BattMonitor/AP_BattMonitor_Serial_BattGo.h index 8b5375ae34..28de9cc28a 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Serial_BattGo.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Serial_BattGo.h @@ -51,7 +51,7 @@ typedef enum bool has_cell_voltages() const override { return _has_cell_voltages; } // 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 notice_to_send_battgo_param(); @@ -61,13 +61,13 @@ private: AP_BattMonitor::BattMonitor_State _interim_state; //TODO may need to fix - char linebuf[10]; - uint8_t linebuf_len = 0; + // char linebuf[10]; + // uint8_t linebuf_len = 0; const int Head1 = 0x55; //数据包帧头 - uint8_t data_buf[116]; + uint8_t data_buf[76]; - uint8_t step; - uint8_t addr; + //uint8_t step; + //uint8_t addr; uint8_t payload_length; uint8_t payload_counter; uint16_t check_sum; @@ -76,10 +76,11 @@ private: uint8_t m_eRxStatus = 0; uint8_t m_u08RXCnt; + uint32_t last_rev_batt_time; void requestBattData(uint8_t data); 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 setPayLoadData(uint8_t &usart_data); bool parseBattGo(); diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index e040674a1c..f86068396b 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -67,8 +67,8 @@ extern const AP_HAL::HAL& hal; uint32_t GCS_MAVLINK::last_radio_status_remrssi_ms; -uint8_t GCS_MAVLINK::mavlink_active = 0; -uint8_t GCS_MAVLINK::chan_is_streaming = 0; +uint8_t GCS_MAVLINK::mavlink_active = 0; +uint8_t GCS_MAVLINK::chan_is_streaming = 0; uint32_t GCS_MAVLINK::reserve_param_space_start_ms; // private channels are ones used for point-to-point protocols, and @@ -3283,11 +3283,11 @@ 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(); - } + // if(!AP::arming().is_armed()){ + // AP::battery().request_info(); + // hal.scheduler->delay(500); + // AP::battery().request_history(); + // } } /* */ void GCS_MAVLINK::send_banner() @@ -3972,7 +3972,7 @@ 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) + if (packet. request_tag== 0x5a) { AP::battery().request_info(); } @@ -3981,7 +3981,7 @@ void GCS_MAVLINK::handle_battgo_info(const mavlink_message_t &msg) 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) + if (packet.request_tag == 0x5a) { AP::battery().request_history(); }