diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.cpp b/libraries/AP_BattMonitor/AP_BattMonitor.cpp index 7390a0db85..26b954add6 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor.cpp @@ -507,6 +507,12 @@ bool AP_BattMonitor::reset_remaining(uint16_t battery_mask, float percentage) return ret; } +void AP_BattMonitor::send_serial_batteryGo_param(mavlink_channel_t chan){ + + +} + + namespace AP { AP_BattMonitor &battery() diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.h b/libraries/AP_BattMonitor/AP_BattMonitor.h index 5679569bad..300a4a9438 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor.h @@ -161,6 +161,10 @@ public: bool reset_remaining(uint16_t battery_mask, float percentage); static const struct AP_Param::GroupInfo var_info[]; + + void send_serial_batteryGo_param(mavlink_channel_t chan); + + protected: @@ -186,7 +190,7 @@ private: int8_t _highest_failsafe_priority; // highest selected failsafe action level (used to restrict what actions we move into) bool _has_triggered_failsafe; // true after a battery failsafe has been triggered for the first time - + }; namespace AP { diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Serial_BattGo.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Serial_BattGo.cpp index c2bf458d73..b61e3ef280 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Serial_BattGo.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Serial_BattGo.cpp @@ -2,7 +2,8 @@ #include #include "AP_BattMonitor_Serial_BattGo.h" #include - +#include +#include #define MAXSONAR_SERIAL_LV_BAUD_RATE 115200 extern const AP_HAL::HAL &hal; @@ -25,16 +26,17 @@ AP_BattMonitor_Serial_BattGo::AP_BattMonitor_Serial_BattGo(AP_BattMonitor &mon, trying to take a reading on Serial. If we get a result the sensor is there. */ -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; } -void AP_BattMonitor_Serial_BattGo::init(void ){ - if (uart != nullptr) +void AP_BattMonitor_Serial_BattGo::init(void) +{ + if (uart != nullptr) { - requestBattData(BATTGO_CMD_REQUEST_ACTUAL_DATA); - requestBattData(BATTGO_CMD_REQUEST_HISTORY); + requestBattData(BATTGO_CMD_REQUEST_ACTUAL_DATA); + requestBattData(BATTGO_CMD_REQUEST_HISTORY); } } @@ -198,8 +200,8 @@ bool AP_BattMonitor_Serial_BattGo::get_reading() //TODO need to Refactor { if (((check_sum >> 8) & 0x00ff) == data) { - if(parseBattGo()) - parsed = true; + if (parseBattGo()) + parsed = true; } step = 0; } @@ -211,14 +213,14 @@ bool AP_BattMonitor_Serial_BattGo::get_reading() //TODO need to Refactor { if (((check_sum >> 8) & 0x00ff) == data) { - if(parseBattGo()) - parsed = true; + if (parseBattGo()) + parsed = true; } step = 0; } else { //last data 0x55 is header, this data is addr,next step =2; - gotoStepAddr( data); + gotoStepAddr(data); } } break; @@ -229,7 +231,7 @@ bool AP_BattMonitor_Serial_BattGo::get_reading() //TODO need to Refactor return parsed; } -void AP_BattMonitor_Serial_BattGo::gotoStepAddr( uint8_t &data) +void AP_BattMonitor_Serial_BattGo::gotoStepAddr(uint8_t &data) { step = 2; //next step addr = data; @@ -238,7 +240,6 @@ void AP_BattMonitor_Serial_BattGo::gotoStepAddr( uint8_t &data) void AP_BattMonitor_Serial_BattGo::requestBattData(uint8_t data) { - //发送data主要 0x40 0x42 0x44 地址0x21 及计算crc 均不会出现0x55，故不做0x55处理 uint8_t send_data[6]; send_data[0] = 0x55; @@ -248,9 +249,10 @@ void AP_BattMonitor_Serial_BattGo::requestBattData(uint8_t data) uint16_t crc = send_data[1] + send_data[2] + send_data[3]; send_data[4] = crc & 0xff; send_data[5] = crc >> 8 & 0xff; - if (uart->txspace() > sizeof(send_data)) { - uart->write(&send_data[0], sizeof(send_data)); - } + if (uart->txspace() > sizeof(send_data)) + { + uart->write(&send_data[0], sizeof(send_data)); + } } void AP_BattMonitor_Serial_BattGo::setPayLoadData(uint8_t &usart_data) @@ -297,23 +299,24 @@ void AP_BattMonitor_Serial_BattGo::setPayLoadData(uint8_t &usart_data) // } // } - + mavlink_battgo_info_t battgo_info_t; bool AP_BattMonitor_Serial_BattGo::parseBattGo() { + if (data_buf[0] == 0x43) { //电池实时数据 - uint32_t tnow = AP_HAL::micros(); + uint32_t tnow = AP_HAL::micros(); uint32_t dt = tnow - _interim_state.last_time_micros; 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; + uint16_t valtemp = ((data_buf[4 + 2 * i] << 8) & 0xff00) + data_buf[3 + 2 * i]; + _interim_state.cell_voltages.cells[i] = valtemp; _interim_state.voltage += valtemp; } _interim_state.temperature = data_buf[15] - 128; - _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_time = AP_HAL::millis(); + _interim_state.current_amps = data_buf[16] + (data_buf[17] << 8) + (data_buf[18] << 16) + (data_buf[19] << 24); // update total current drawn since startup if (_interim_state.last_time_micros != 0 && dt < 2000000) @@ -327,40 +330,122 @@ bool AP_BattMonitor_Serial_BattGo::parseBattGo() // record time _interim_state.last_time_micros = tnow; _interim_state.healthy = true; - + return true; } else if (data_buf[0] == 0x41) { //电池参数 - return true; + + 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]; + for (int i = 0; i < 8; i++) + { + battgo_info_t.equipment_id[i] = data_buf[7 + i]; + } + for (int i = 0; i < 10; i++) + { + 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]; + + 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; + + battgo_info_t.production_time_year = year; + battgo_info_t.production_time_mon = month; + battgo_info_t.production_time_mday = day; + battgo_info_t.production_time_hour = hour; + battgo_info_t.production_time_min = min; + 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.type = data_buf[52]; + battgo_info_t.core_amount = data_buf[53]; + battgo_info_t.over_discharge_vol = (data_buf[55] << 8) | data_buf[54]; + battgo_info_t.warning_vol = (data_buf[57] << 8) | data_buf[56]; + battgo_info_t.storage_vol = (data_buf[59] << 8) | data_buf[58]; + battgo_info_t.full_charge_vol = (data_buf[61] << 8) | data_buf[60]; + battgo_info_t.battery_capacity = (data_buf[65] << 24) | (data_buf[64] << 16) | (data_buf[63] << 8) | data_buf[62]; + battgo_info_t.charge_c_value = (data_buf[67] << 8) | data_buf[66]; + battgo_info_t.discharge_c_value = (data_buf[69] << 8) | data_buf[68]; + battgo_info_t.temp_using_min = data_buf[70]; + battgo_info_t.temp_using_max = data_buf[71]; + 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); + + requestBattData(BATTGO_CMD_REQUEST_HISTORY); + return true; } - else if (data_buf[0] == 0x45) // + else if (data_buf[0] == 0x45) // { - return true; + battgo_info_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_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); + return true; } return false; } void AP_BattMonitor_Serial_BattGo::read() { - requestBattData(0x42); - if( get_reading()) - { - 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; + requestBattData(0x42); + if (get_reading()) + { + 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.temperature = _interim_state.temperature; + _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.healthy = _interim_state.healthy; } +} - // Copy over relevant states over to main state - // WITH_SEMAPHORE(_sem_battmon); - _state.temperature = _interim_state.temperature; - _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.healthy = _interim_state.healthy; - } +void AP_BattMonitor_Serial_BattGo::notice_to_send_battgo_param() +{ + gcs().send_message(MSG_BATTGAO_PARAM); } diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Serial_BattGo.h b/libraries/AP_BattMonitor/AP_BattMonitor_Serial_BattGo.h index 08e865dfee..f70937af96 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Serial_BattGo.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Serial_BattGo.h @@ -39,7 +39,8 @@ public: // static detection function 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(); private: // get a reading @@ -48,7 +49,7 @@ private: char linebuf[10]; uint8_t linebuf_len = 0; const int Head1 = 0x55; //数据包帧头 - uint8_t data_buf[88]; + uint8_t data_buf[116]; uint8_t step; uint8_t addr; diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index cf9c68d77f..878e6e39e4 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); + //void send_serial_battgo(mavlink_battgo_info_t &battgo_info_t ); bool locked() const; // return a bitmap of active channels. Used by libraries to loop @@ -831,6 +832,8 @@ public: void update_send(); void update_receive(); + void update_serial_battgo(mavlink_battgo_info_t *battgo_info_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 24ce4499fb..f5a5a936dc 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -210,7 +210,7 @@ void GCS_MAVLINK::send_battery_status(const uint8_t instance) const } } } - + float current, consumed_mah, consumed_wh; if (battery.current_amps(current, instance)) { current *= 100; @@ -239,6 +239,7 @@ void GCS_MAVLINK::send_battery_status(const uint8_t instance) const MAV_BATTERY_CHARGE_STATE_UNDEFINED); } + // returns true if all battery instances were reported bool GCS_MAVLINK::send_battery_status() const { @@ -252,7 +253,10 @@ 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; +} void GCS_MAVLINK::send_distance_sensor(const AP_RangeFinder_Backend *sensor, const uint8_t instance) const { if (!sensor->has_data()) { @@ -1896,6 +1900,15 @@ void GCS::update_receive(void) update_passthru(); } +void GCS::update_serial_battgo(mavlink_battgo_info_t *battgo_info_t ) +{ + for (uint8_t i=0; isend_battgo_param(battgo_info_t); + } + +} + void GCS::send_mission_item_reached_message(uint16_t mission_index) { for (uint8_t i=0; i