From 65dda9a02cc6fa385ebacfa1da6655da2f4f3e3d Mon Sep 17 00:00:00 2001 From: yaozb Date: Fri, 17 Apr 2020 09:07:20 +0800 Subject: [PATCH] actual data read --- libraries/AP_BattMonitor/AP_BattMonitor.cpp | 5 +- libraries/AP_BattMonitor/AP_BattMonitor.h | 2 +- .../AP_BattMonitor/AP_BattMonitor_Params.h | 1 + .../AP_BattMonitor_Serial_BattGo.cpp | 366 ++++++++++++++++++ .../AP_BattMonitor_Serial_BattGo.h | 69 ++++ libraries/AP_SerialManager/AP_SerialManager.h | 1 + 6 files changed, 442 insertions(+), 2 deletions(-) create mode 100644 libraries/AP_BattMonitor/AP_BattMonitor_Serial_BattGo.cpp create mode 100644 libraries/AP_BattMonitor/AP_BattMonitor_Serial_BattGo.h diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.cpp b/libraries/AP_BattMonitor/AP_BattMonitor.cpp index 284c56b72f..7390a0db85 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor.cpp @@ -6,7 +6,7 @@ #include "AP_BattMonitor_Sum.h" #include "AP_BattMonitor_FuelFlow.h" #include "AP_BattMonitor_FuelLevel_PWM.h" - +#include "AP_BattMonitor_Serial_BattGo.h" #include #if HAL_WITH_UAVCAN @@ -146,6 +146,9 @@ AP_BattMonitor::init() case AP_BattMonitor_Params::BattMonitor_TYPE_FuelLevel_PWM: drivers[instance] = new AP_BattMonitor_FuelLevel_PWM(*this, state[instance], _params[instance]); break; + case AP_BattMonitor_Params::BattMonitor_Serial_BattGo: + drivers[instance] = new AP_BattMonitor_Serial_BattGo(*this, state[instance], _params[instance],instance);//TODO serial_instance here is 0 + break; case AP_BattMonitor_Params::BattMonitor_TYPE_NONE: default: break; diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.h b/libraries/AP_BattMonitor/AP_BattMonitor.h index af285ac1d2..5679569bad 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor.h @@ -38,7 +38,7 @@ class AP_BattMonitor friend class AP_BattMonitor_Sum; friend class AP_BattMonitor_FuelFlow; friend class AP_BattMonitor_FuelLevel_PWM; - + friend class AP_BattMonitor_Serial_BattGo; public: // battery failsafes must be defined in levels of severity so that vehicles wont fall backwards diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Params.h b/libraries/AP_BattMonitor/AP_BattMonitor_Params.h index 072ecfca7c..9dc078bff4 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Params.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Params.h @@ -25,6 +25,7 @@ public: BattMonitor_TYPE_Sum = 10, BattMonitor_TYPE_FuelFlow = 11, BattMonitor_TYPE_FuelLevel_PWM = 12, + BattMonitor_Serial_BattGo =13, }; // low voltage sources (used for BATT_LOW_TYPE parameter) diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Serial_BattGo.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Serial_BattGo.cpp new file mode 100644 index 0000000000..c2bf458d73 --- /dev/null +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Serial_BattGo.cpp @@ -0,0 +1,366 @@ + +#include +#include "AP_BattMonitor_Serial_BattGo.h" +#include + +#define MAXSONAR_SERIAL_LV_BAUD_RATE 115200 +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 ¶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_Rangefinder, serial_instance)); + } +}; + +/* + detect if a battGo is connected. We'll detect by + 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 +{ + return AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_Rangefinder, serial_instance) != nullptr; +} + +void AP_BattMonitor_Serial_BattGo::init(void ){ + if (uart != nullptr) + { + requestBattData(BATTGO_CMD_REQUEST_ACTUAL_DATA); + requestBattData(BATTGO_CMD_REQUEST_HISTORY); + } +} + +bool AP_BattMonitor_Serial_BattGo::get_reading() //TODO need to Refactor +{ + + if (uart == nullptr) + { + return false; + } + + int16_t numc; + numc = uart->available(); + uint8_t data; + + bool parsed = false; + for (int16_t i = 0; i < numc; i++) + { + data = uart->read(); + switch (step) + { + case 0: + if (Head1 == data) + { + payload_counter = 0; + payload_length = 0; + step++; + } + is_read_header = false; + break; + case 1: //addr + if (!is_read_header) + { + if (Head1 != data) + { + addr = data; + check_sum = data; + step++; + } + else + { + is_read_header = true; + } + } + else //this data is 0x55 + { + is_read_header = false; + if (Head1 == data) // is repeat data + { + addr = data; + step++; + check_sum = data; + } + else + { //last data 0x55 is header, this data is addr,next step =2; + gotoStepAddr(data); + } + } + break; + case 2: //payload_length; + if (!is_read_header) + { + if (Head1 != data) + { + payload_length = data; + check_sum += data; + step++; + } + else + { + is_read_header = true; + } + } + else //this data is 0x55 + { + is_read_header = false; + if (Head1 == data) // is repeat data + { + payload_length = data; + check_sum += data; + step++; + } + else + { //last data 0x55 is header, this data is addr,next step =2; + gotoStepAddr(data); + } + } + break; + case 3: //payloaddata + if (!is_read_header) + { + if (Head1 != data) + { + setPayLoadData(data); + } + else + { + is_read_header = true; + } + } + else //this data is 0x55 + { + is_read_header = false; + if (Head1 == data) + { // is repeat data + setPayLoadData(data); + } + else + { //last data 0x55 is header, this data is addr,next step =2; + gotoStepAddr(data); + } + } + break; + case 4: //crc_a + if (!is_read_header) + { + if (Head1 == data) + { + is_read_header = true; + } + else + { + if ((check_sum & 0x00ff) != data) + { + step = 0; + } + else + { + step++; + } + } + } + else //this data is 0x55 + { + is_read_header = false; + if (Head1 == data) // is repeat data + { + if ((check_sum & 0x00ff) != data) + { + step = 0; + } + else + { + step++; + } + } + else + { //last data 0x55 is header, this data is addr,next step =2; + gotoStepAddr(data); + } + } + break; + case 5: //crc_b + if (!is_read_header) + { + if (Head1 == data) + { + is_read_header = true; + } + else + { + if (((check_sum >> 8) & 0x00ff) == data) + { + if(parseBattGo()) + parsed = true; + } + step = 0; + } + } + else //this data is 0x55 + { + is_read_header = false; + if (Head1 == data) // is repeat data + { + if (((check_sum >> 8) & 0x00ff) == data) + { + if(parseBattGo()) + parsed = true; + } + step = 0; + } + else + { //last data 0x55 is header, this data is addr,next step =2; + gotoStepAddr( data); + } + } + break; + default: + break; + } + } + return parsed; +} + +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处理 + uint8_t send_data[6]; + send_data[0] = 0x55; + send_data[1] = 0x21; + send_data[2] = 0x01; + send_data[3] = 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)); + } +} + +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++; + } +} + +// void AP_BattMonitor_Serial_BattGo::doWithReadHeaderData(uint8_t &usart_data, uint8_t target_data, uint8_t step, uint8_t &payload_length, uint8_t &payload_count, uint8_t *data_buf, bool &is_read_header) +// { +// if (!is_read_header) +// { +// target_data = usart_data; +// switch (step) +// { +// case 1: //addr +// check_sum = 0; +// step++; +// case 2: //payload_length; +// payload_length = usart_data; +// step++; +// break; +// case 3: //data +// if (payload_count < payload_length) +// { +// data_buf[payload_count] = usart_data; +// payload_count++; +// } +// break; +// default: +// break; +// } +// check_sum += usart_data; +// } +// else +// { +// } + +// } + +bool AP_BattMonitor_Serial_BattGo::parseBattGo() +{ + if (data_buf[0] == 0x43) + { //电池实时数据 + 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; + _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); + + // update total current drawn since startup + if (_interim_state.last_time_micros != 0 && dt < 2000000) + { + // .0002778 is 1/3600 (conversion to hours) + float mah = (float)((double)_interim_state.current_amps * (double)dt * (double)0.0000002778f); + _interim_state.consumed_mah += mah; + _interim_state.consumed_wh += 0.001f * mah * _interim_state.voltage; + } + + // record time + _interim_state.last_time_micros = tnow; + _interim_state.healthy = true; + + return true; + } + else if (data_buf[0] == 0x41) + { + //电池参数 + return true; + } + else if (data_buf[0] == 0x45) // + { + 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; + } + + // 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; + } +} diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Serial_BattGo.h b/libraries/AP_BattMonitor/AP_BattMonitor_Serial_BattGo.h new file mode 100644 index 0000000000..08e865dfee --- /dev/null +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Serial_BattGo.h @@ -0,0 +1,69 @@ +#include +#include "AP_BattMonitor.h" +#include "AP_BattMonitor_Backend.h" + +#define AP_BATTMONITOR_SERIAL_TIMEOUT_MICROS 5000000 +#define BATTGO_CMD_REQUEST_PARAM 0x40 +#define BATTGO_CMD_REQUEST_ACTUAL_DATA 0x42 +#define BATTGO_CMD_REQUEST_HISTORY 0x44 +class AP_BattMonitor_Serial_BattGo : public AP_BattMonitor_Backend +{ + +public: + enum BattMonitor_Serial_BattGo_Type + { + SERIAL_BATTGO_INFO = 0 + }; + + AP_BattMonitor_Serial_BattGo(AP_BattMonitor &mon, + AP_BattMonitor::BattMonitor_State &mon_state, + AP_BattMonitor_Params ¶ms, + uint8_t serial_instance); + + // ~AP_BattMonitor_Serial_BattGo(); + + bool has_current() const override + { + return true; + } + + void init() override; + + /// Read the battery voltage and current. Should be called at 10hz + void read() override; + + bool has_cell_voltages() const + { + return true; + } + + // static detection function + static bool detect(uint8_t serial_instance); + +private: + // get a reading + + AP_HAL::UARTDriver *uart = nullptr; + AP_BattMonitor::BattMonitor_State _interim_state; + char linebuf[10]; + uint8_t linebuf_len = 0; + const int Head1 = 0x55; //数据包帧头 + uint8_t data_buf[88]; + + uint8_t step; + uint8_t addr; + uint8_t payload_length; + uint8_t payload_counter; + uint16_t check_sum; + bool is_read_header = false; + + void requestBattData(uint8_t data); + + bool get_reading(); + uint8_t crc_crc8(const uint8_t *p, uint8_t len); + void gotoStepAddr(uint8_t &data); + void setPayLoadData(uint8_t &usart_data); + bool parseBattGo(); + + //void AP_BattMonitor_Serial_BattGo::doWithReadHeaderData(uint8_t &usart_data,uint8_t target_data,uint8_t step,uint8_t &payload_length,uint8_t &payload_count,uint8_t * data_buf,bool &is_read_header); +}; diff --git a/libraries/AP_SerialManager/AP_SerialManager.h b/libraries/AP_SerialManager/AP_SerialManager.h index f0517bf167..d5962efabf 100644 --- a/libraries/AP_SerialManager/AP_SerialManager.h +++ b/libraries/AP_SerialManager/AP_SerialManager.h @@ -123,6 +123,7 @@ public: SerialProtocol_WindVane = 21, SerialProtocol_SLCAN = 22, SerialProtocol_RCIN = 23, + SerialProtocol_Battery =24, }; // get singleton instance