yaozb
5 years ago
6 changed files with 442 additions and 2 deletions
@ -0,0 +1,366 @@
@@ -0,0 +1,366 @@
|
||||
|
||||
#include <AP_HAL/AP_HAL.h> |
||||
#include "AP_BattMonitor_Serial_BattGo.h" |
||||
#include <AP_SerialManager/AP_SerialManager.h> |
||||
|
||||
#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; |
||||
} |
||||
} |
||||
@ -0,0 +1,69 @@
@@ -0,0 +1,69 @@
|
||||
#include <AP_HAL/AP_HAL.h> |
||||
#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);
|
||||
}; |
||||
Loading…
Reference in new issue