From 1c187174483ae1e48a3150e60014b04277f87965 Mon Sep 17 00:00:00 2001 From: yaozb Date: Mon, 27 Apr 2020 15:03:57 +0800 Subject: [PATCH] battgo history and info mavlink separated (didnt test) --- .../AP_BattMonitor_Serial_BattGo.cpp | 23 ++++++++++------ .../AP_BattMonitor_Serial_BattGo.h | 5 +--- libraries/GCS_MAVLink/GCS.h | 7 ++--- libraries/GCS_MAVLink/GCS_Common.cpp | 27 ++++++++++++++----- libraries/GCS_MAVLink/ap_message.h | 2 +- 5 files changed, 41 insertions(+), 23 deletions(-) diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Serial_BattGo.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Serial_BattGo.cpp index 993b834325..bab79d984d 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Serial_BattGo.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Serial_BattGo.cpp @@ -37,6 +37,7 @@ void AP_BattMonitor_Serial_BattGo::init(void) { requestBattData(BATTGO_CMD_REQUEST_ACTUAL_DATA); requestBattData(BATTGO_CMD_REQUEST_PARAM); + requestBattData(BATTGO_CMD_REQUEST_HISTORY); } } @@ -300,6 +301,7 @@ void AP_BattMonitor_Serial_BattGo::setPayLoadData(uint8_t &usart_data) // } mavlink_battgo_info_t battgo_info_t; + mavlink_battgo_history_t battgo_history_t ; bool AP_BattMonitor_Serial_BattGo::parseBattGo() { @@ -404,18 +406,23 @@ bool AP_BattMonitor_Serial_BattGo::parseBattGo() 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); - - requestBattData(BATTGO_CMD_REQUEST_HISTORY); + gcs().update_serial_battgo_info(&battgo_info_t); + //requestBattData(BATTGO_CMD_REQUEST_HISTORY); return true; } else if (data_buf[0] == 0x45) // { - battgo_info_t.last_charge_cap = (data_buf[7] << 24) | (data_buf[6] << 16) | (data_buf[5] << 8) |data_buf[4]; + 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_info_t.Internal_resistance[i] = (data_buf[20+2*i+1]<<8)|data_buf[20+2*i]; - } - battgo_info_t.charge_state = data_buf[32]; - gcs().update_serial_battgo(&battgo_info_t); + 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); return true; } return false; @@ -447,5 +454,5 @@ void AP_BattMonitor_Serial_BattGo::read() void AP_BattMonitor_Serial_BattGo::notice_to_send_battgo_param() { - gcs().send_message(MSG_BATTGAO_PARAM); + gcs().send_message(MSG_BATTGAO_INFO); } diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Serial_BattGo.h b/libraries/AP_BattMonitor/AP_BattMonitor_Serial_BattGo.h index f70937af96..0aae394466 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Serial_BattGo.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Serial_BattGo.h @@ -32,10 +32,7 @@ public: /// Read the battery voltage and current. Should be called at 10hz void read() override; - bool has_cell_voltages() const - { - return true; - } + bool has_cell_voltages() const override { return true; } // static detection function static bool detect(uint8_t serial_instance); diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 878e6e39e4..3b34c600fe 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -217,7 +217,8 @@ public: void send_sys_status(); void send_set_position_target_global_int(uint8_t target_system, uint8_t target_component, const Location& loc); void send_rpm() const; - bool send_battgo_param(const mavlink_battgo_info_t *mavlink_battgo_info_t); + bool send_battgo_info(const mavlink_battgo_info_t *mavlink_battgo_info_t); + bool send_battgo_history(const mavlink_battgo_history_t *mavlink_battgo_info_t); //void send_serial_battgo(mavlink_battgo_info_t &battgo_info_t ); bool locked() const; @@ -832,8 +833,8 @@ public: void update_send(); void update_receive(); - void update_serial_battgo(mavlink_battgo_info_t *battgo_info_t ); - + void update_serial_battgo_info(mavlink_battgo_info_t *battgo_info_t ); + void update_serial_battgo_history(mavlink_battgo_history_t *battgo_history_t ); // minimum amount of time (in microseconds) that must remain in // the main scheduler loop before we are allowed to send any // mavlink messages. We want to prioritise the main flight diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index f3fb4f54cd..19d8f0ce17 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -253,9 +253,16 @@ bool GCS_MAVLINK::send_battery_status() const } return true; } -bool GCS_MAVLINK::send_battgo_param(const mavlink_battgo_info_t *battgo_info_t){ - mavlink_msg_battgo_info_send_struct(chan,battgo_info_t); - return true; +bool GCS_MAVLINK::send_battgo_info(const mavlink_battgo_info_t *battgo_info_t) +{ + mavlink_msg_battgo_info_send_struct(chan, battgo_info_t); + return true; +} + +bool GCS_MAVLINK::send_battgo_history(const mavlink_battgo_history_t *battgo_history_t) +{ + mavlink_msg_battgo_history_send_struct(chan, battgo_history_t); + return true; } void GCS_MAVLINK::send_distance_sensor(const AP_RangeFinder_Backend *sensor, const uint8_t instance) const { @@ -1900,15 +1907,21 @@ void GCS::update_receive(void) update_passthru(); } -void GCS::update_serial_battgo(mavlink_battgo_info_t *battgo_info_t ) +void GCS::update_serial_battgo_info(mavlink_battgo_info_t *battgo_info_t ) { for (uint8_t i=0; isend_battgo_param(battgo_info_t); - } - + chan(i)->send_battgo_info(battgo_info_t); + } } +void GCS::update_serial_battgo_history(mavlink_battgo_history_t *battgo_history_t ) +{ + for (uint8_t i=0; isend_battgo_history(battgo_history_t); + } +} void GCS::send_mission_item_reached_message(uint16_t mission_index) { for (uint8_t i=0; i