Added hw status to ublox test scripts and ubloxGnss packet (#1054)
* Added hw status to ublox test scripts and ubloxGnss packet * Added extra config to boardd * Implemented new ublox message handler in ubloxd * Fixed debug text * Added some explanation and cereal bumppull/1065/head
parent
920572442b
commit
d0c1f9a864
2
cereal
2
cereal
|
@ -1 +1 @@
|
|||
Subproject commit 23abef4fdb18bc2b06281f9937ed3e7fc31e5c37
|
||||
Subproject commit ab32956aaf927ee6318bcf639e3a25eea72027a9
|
|
@ -789,6 +789,7 @@ void pigeon_init() {
|
|||
usleep(100*1000);
|
||||
|
||||
// init from ubloxd
|
||||
// To generate this data, run test/ubloxd.py with the print statements enabled in the write function in panda/python/serial.py
|
||||
pigeon_send("\xB5\x62\x06\x00\x14\x00\x03\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x01\x00\x01\x00\x00\x00\x00\x00\x1E\x7F");
|
||||
pigeon_send("\xB5\x62\x06\x3E\x00\x00\x44\xD2");
|
||||
pigeon_send("\xB5\x62\x06\x00\x14\x00\x00\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x19\x35");
|
||||
|
@ -807,6 +808,7 @@ void pigeon_init() {
|
|||
pigeon_send("\xB5\x62\x06\x01\x03\x00\x01\x07\x01\x13\x51");
|
||||
pigeon_send("\xB5\x62\x06\x01\x03\x00\x02\x15\x01\x22\x70");
|
||||
pigeon_send("\xB5\x62\x06\x01\x03\x00\x02\x13\x01\x20\x6C");
|
||||
pigeon_send("\xB5\x62\x06\x01\x03\x00\x0A\x09\x01\x1E\x70");
|
||||
|
||||
LOGW("panda GPS on");
|
||||
}
|
||||
|
|
|
@ -475,6 +475,8 @@ msg_types = {
|
|||
UBloxDescriptor('AID_ALM', '<II', '_remaining', 'I', ['dwrd']),
|
||||
(CLASS_RXM, MSG_RXM_ALM):
|
||||
UBloxDescriptor('RXM_ALM', '<II , 8I', ['svid', 'week', 'dwrd[8]']),
|
||||
(CLASS_CFG, MSG_CFG_ANT):
|
||||
UBloxDescriptor('CFG_ANT', '<HH', ['flags', 'pins']),
|
||||
(CLASS_CFG, MSG_CFG_ODO):
|
||||
UBloxDescriptor('CFG_ODO', '<B3BBB6BBB2BBB2B', [
|
||||
'version', 'reserved1[3]', 'flags', 'odoCfg', 'reserverd2[6]', 'cogMaxSpeed',
|
||||
|
@ -494,9 +496,9 @@ msg_types = {
|
|||
'reserved12', 'reserved13', 'aopOrbMaxErr', 'reserved3', 'reserved4'
|
||||
]),
|
||||
(CLASS_MON, MSG_MON_HW):
|
||||
UBloxDescriptor('MON_HW', '<IIIIHHBBBBIB25BHIII', [
|
||||
UBloxDescriptor('MON_HW', '<IIIIHHBBBBIB17BHIII', [
|
||||
'pinSel', 'pinBank', 'pinDir', 'pinVal', 'noisePerMS', 'agcCnt', 'aStatus', 'aPower',
|
||||
'flags', 'reserved1', 'usedMask', 'VP[25]', 'jamInd', 'reserved3', 'pinInq', 'pullH',
|
||||
'flags', 'reserved1', 'usedMask', 'VP[17]', 'jamInd', 'reserved3', 'pinInq', 'pullH',
|
||||
'pullL'
|
||||
]),
|
||||
(CLASS_MON, MSG_MON_HW2):
|
||||
|
@ -827,7 +829,10 @@ class UBlox:
|
|||
if not self.read_only:
|
||||
if self.use_sendrecv:
|
||||
return self.dev.send(buf)
|
||||
return self.dev.write(buf)
|
||||
if type(buf) == str:
|
||||
return self.dev.write(str.encode(buf))
|
||||
else:
|
||||
return self.dev.write(buf)
|
||||
|
||||
def read(self, n):
|
||||
'''read some bytes'''
|
||||
|
@ -973,7 +978,7 @@ class UBlox:
|
|||
payload = struct.pack('<IIIB', clearMask, saveMask, loadMask, deviceMask)
|
||||
self.send_message(CLASS_CFG, MSG_CFG_CFG, payload)
|
||||
|
||||
def configure_poll(self, msg_class, msg_id, payload=''):
|
||||
def configure_poll(self, msg_class, msg_id, payload=b''):
|
||||
'''poll a configuration message'''
|
||||
self.send_message(msg_class, msg_id, payload)
|
||||
|
||||
|
|
|
@ -72,10 +72,11 @@ def configure_ublox(dev):
|
|||
dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_NAVX5)
|
||||
dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_ODO)
|
||||
|
||||
# Configure RAW and PVT messages to be sent every solution cycle
|
||||
# Configure RAW, PVT and HW messages to be sent every solution cycle
|
||||
dev.configure_message_rate(ublox.CLASS_NAV, ublox.MSG_NAV_PVT, 1)
|
||||
dev.configure_message_rate(ublox.CLASS_RXM, ublox.MSG_RXM_RAW, 1)
|
||||
dev.configure_message_rate(ublox.CLASS_RXM, ublox.MSG_RXM_SFRBX, 1)
|
||||
dev.configure_message_rate(ublox.CLASS_MON, ublox.MSG_MON_HW, 1)
|
||||
|
||||
|
||||
|
||||
|
@ -222,6 +223,17 @@ def gen_raw(msg):
|
|||
'measurements': measurements_parsed}}
|
||||
return log.Event.new_message(ubloxGnss=raw_meas)
|
||||
|
||||
def gen_hw_status(msg):
|
||||
msg_data = msg.unpack()[0]
|
||||
ublox_hw_status = {'hwStatus': {
|
||||
'noisePerMS': msg_data['noisePerMS'],
|
||||
'agcCnt': msg_data['agcCnt'],
|
||||
'aStatus': msg_data['aStatus'],
|
||||
'aPower': msg_data['aPower'],
|
||||
'jamInd': msg_data['jamInd']
|
||||
}}
|
||||
return log.Event.new_message(ubloxGnss=ublox_hw_status)
|
||||
|
||||
def init_reader():
|
||||
port_counter = 0
|
||||
while True:
|
||||
|
@ -252,9 +264,12 @@ def handle_msg(dev, msg, nav_frame_buffer):
|
|||
if nav is not None:
|
||||
nav.logMonoTime = int(realtime.sec_since_boot() * 1e9)
|
||||
ubloxGnss.send(nav.to_bytes())
|
||||
|
||||
elif msg.name() == 'MON_HW':
|
||||
hw = gen_hw_status(msg)
|
||||
hw.logMonoTime = int(realtime.sec_since_boot() * 1e9)
|
||||
ubloxGnss.send(hw.to_bytes())
|
||||
else:
|
||||
print("UNKNNOWN MESSAGE:", msg.name())
|
||||
print("UNKNOWN MESSAGE:", msg.name())
|
||||
except ublox.UBloxError as e:
|
||||
print(e)
|
||||
|
||||
|
|
|
@ -347,6 +347,22 @@ kj::Array<capnp::word> UbloxMsgParser::gen_nav_data() {
|
|||
return kj::Array<capnp::word>();
|
||||
}
|
||||
|
||||
kj::Array<capnp::word> UbloxMsgParser::gen_mon_hw() {
|
||||
mon_hw_msg *msg = (mon_hw_msg *)&msg_parse_buf[UBLOX_HEADER_SIZE];
|
||||
|
||||
capnp::MallocMessageBuilder msg_builder;
|
||||
cereal::Event::Builder event = msg_builder.initRoot<cereal::Event>();
|
||||
event.setLogMonoTime(nanos_since_boot());
|
||||
auto gnss = event.initUbloxGnss();
|
||||
auto hwStatus = gnss.initHwStatus();
|
||||
hwStatus.setNoisePerMS(msg->noisePerMS);
|
||||
hwStatus.setAgcCnt(msg->agcCnt);
|
||||
hwStatus.setAStatus((cereal::UbloxGnss::HwStatus::AntennaSupervisorState) msg->aStatus);
|
||||
hwStatus.setAPower((cereal::UbloxGnss::HwStatus::AntennaPowerStatus) msg->aPower);
|
||||
hwStatus.setJamInd(msg->jamInd);
|
||||
return capnp::messageToFlatArray(msg_builder);
|
||||
}
|
||||
|
||||
bool UbloxMsgParser::add_data(const uint8_t *incoming_data, uint32_t incoming_data_len, size_t &bytes_consumed) {
|
||||
int needed = needed_bytes();
|
||||
if(needed > 0) {
|
||||
|
|
|
@ -85,6 +85,27 @@ typedef struct __attribute__((packed)) {
|
|||
uint32_t dwrd;
|
||||
} rxm_sfrbx_msg_extra;
|
||||
|
||||
// MON_HW
|
||||
typedef struct __attribute__((packed)) {
|
||||
uint32_t pinSel;
|
||||
uint32_t pinBank;
|
||||
uint32_t pinDir;
|
||||
uint32_t pinVal;
|
||||
uint16_t noisePerMS;
|
||||
uint16_t agcCnt;
|
||||
uint8_t aStatus;
|
||||
uint8_t aPower;
|
||||
uint8_t flags;
|
||||
uint8_t reserved1;
|
||||
uint32_t usedMask;
|
||||
uint8_t VP[17];
|
||||
uint8_t jamInd;
|
||||
uint8_t reserved2[2];
|
||||
uint32_t pinIrq;
|
||||
uint32_t pullH;
|
||||
uint32_t pullL;
|
||||
} mon_hw_msg;
|
||||
|
||||
namespace ublox {
|
||||
// protocol constants
|
||||
const uint8_t PREAMBLE1 = 0xb5;
|
||||
|
@ -93,6 +114,7 @@ namespace ublox {
|
|||
// message classes
|
||||
const uint8_t CLASS_NAV = 0x01;
|
||||
const uint8_t CLASS_RXM = 0x02;
|
||||
const uint8_t CLASS_MON = 0x0A;
|
||||
|
||||
// NAV messages
|
||||
const uint8_t MSG_NAV_PVT = 0x7;
|
||||
|
@ -101,6 +123,9 @@ namespace ublox {
|
|||
const uint8_t MSG_RXM_RAW = 0x15;
|
||||
const uint8_t MSG_RXM_SFRBX = 0x13;
|
||||
|
||||
// MON messages
|
||||
const uint8_t MSG_MON_HW = 0x09;
|
||||
|
||||
const int UBLOX_HEADER_SIZE = 6;
|
||||
const int UBLOX_CHECKSUM_SIZE = 2;
|
||||
const int UBLOX_MAX_MSG_SIZE = 65536;
|
||||
|
@ -113,6 +138,7 @@ namespace ublox {
|
|||
UbloxMsgParser();
|
||||
kj::Array<capnp::word> gen_solution();
|
||||
kj::Array<capnp::word> gen_raw();
|
||||
kj::Array<capnp::word> gen_mon_hw();
|
||||
|
||||
kj::Array<capnp::word> gen_nav_data();
|
||||
bool add_data(const uint8_t *incoming_data, uint32_t incoming_data_len, size_t &bytes_consumed);
|
||||
|
|
|
@ -97,6 +97,17 @@ int ubloxd_main(poll_ubloxraw_msg_func poll_func, send_gps_event_func send_func)
|
|||
}
|
||||
} else
|
||||
LOGW("Unknown rxm msg id: 0x%02X", parser.msg_id());
|
||||
} else if(parser.msg_class() == CLASS_MON) {
|
||||
if(parser.msg_id() == MSG_MON_HW) {
|
||||
//LOGD("MSG_MON_HW");
|
||||
auto words = parser.gen_mon_hw();
|
||||
if(words.size() > 0) {
|
||||
auto bytes = words.asBytes();
|
||||
send_func(ubloxGnss, bytes.begin(), bytes.size());
|
||||
}
|
||||
} else {
|
||||
LOGW("Unknown mon msg id: 0x%02X", parser.msg_id());
|
||||
}
|
||||
} else
|
||||
LOGW("Unknown msg class: 0x%02X", parser.msg_class());
|
||||
parser.reset();
|
||||
|
|
Loading…
Reference in New Issue