Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add validation to initialization configuration commands #27459

88 changes: 57 additions & 31 deletions libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -286,28 +286,29 @@ bool AP_ExternalAHRS_VectorNav::check_uart()
return true;
}

// Send command to read given register number and wait for response
// Send command and wait for response
// Only run from thread! This blocks until a response is received
peterbarker marked this conversation as resolved.
Show resolved Hide resolved
#define READ_REQUEST_RETRY_MS 500
void AP_ExternalAHRS_VectorNav::wait_register_responce(const uint8_t register_num)
void AP_ExternalAHRS_VectorNav::send_command_blocking()
peterbarker marked this conversation as resolved.
Show resolved Hide resolved
{
nmea.register_number = register_num;

uint32_t request_sent = 0;
while (true) {
hal.scheduler->delay(1);

const uint32_t now = AP_HAL::millis();
if (now - request_sent > READ_REQUEST_RETRY_MS) {
// Send request to read
nmea_printf(uart, "$%s%u", "VNRRG,", nmea.register_number);
nmea_printf(uart, "$%s", message_to_send);
request_sent = now;
}

int16_t nbytes = uart->available();
while (nbytes-- > 0) {
char c = uart->read();
if (decode(c)) {
if (nmea.error_response) {
// Received a valid VNERR. Try to resend after the timeout length
break;
}
return;
}
}
Expand Down Expand Up @@ -356,6 +357,7 @@ bool AP_ExternalAHRS_VectorNav::decode(char c)
nmea.checksum = 0;
nmea.term_is_checksum = false;
nmea.sentence_done = false;
nmea.error_response = false;
return false;
}

Expand All @@ -371,57 +373,70 @@ bool AP_ExternalAHRS_VectorNav::decode(char c)
}

// decode the most recently consumed term
// returns true if new sentence has just passed checksum test and is validated
// returns true if new term is valid
bool AP_ExternalAHRS_VectorNav::decode_latest_term()
{
// Check the first two terms (In most cases header + reg number) that they match the sent
// message. If not, the response is invalid.
switch (nmea.term_number) {
case 0:
if (strcmp(nmea.term, "VNRRG") != 0) {
if (strncmp(nmea.term, message_to_send, nmea.term_offset) != 0) { // ignore sync byte in comparison
return false;
} else if (strncmp(nmea.term, "VNERR", nmea.term_offset) == 0) {
lashVN marked this conversation as resolved.
Show resolved Hide resolved
nmea.error_response = true; // Message will be printed on next term
}
break;
lashVN marked this conversation as resolved.
Show resolved Hide resolved

case 1:
if (nmea.register_number != strtoul(nmea.term, nullptr, 10)) {
case 1:
if (nmea.error_response) {
GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "VectorNav received VNERR code: %s", nmea.term);
break;
}
if (strncmp(nmea.term, message_to_send + 6,
nmea.term_offset) != 0) { // Start after "VNXXX,"
return false;
}
break;

case 2:
strncpy(model_name, nmea.term, sizeof(model_name));
case 2:
if (strncmp(nmea.term, "VN-", 3) == 0) {
// This term is the model number
strncpy(model_name, nmea.term, sizeof(model_name));
break;
}
break;

default:
return false;
break;
}
return true;
}

void AP_ExternalAHRS_VectorNav::update_thread()
{
void AP_ExternalAHRS_VectorNav::initialize() {
// Open port in the thread
uart->begin(baudrate, 1024, 512);

// Reset and wait for module to reboot
// VN_100 takes 1.25 seconds
//nmea_printf(uart, "$VNRST");
//hal.scheduler->delay(3000);
// Pause asynchronous communications to simplify packet finding
hal.util->snprintf(message_to_send, sizeof(message_to_send), "VNASY,0");
lashVN marked this conversation as resolved.
Show resolved Hide resolved
send_command_blocking();

// Stop NMEA Async Outputs (this UART only)
nmea_printf(uart, "$VNWRG,6,0");
// Stop ASCII async outputs for both UARTs. If only active UART is disabled, we get a baudrate
// overflow on the other UART when configuring binary outputs (reg 75 and 76) to both UARTs
hal.util->snprintf(message_to_send, sizeof(message_to_send), "VNWRG,06,1");
send_command_blocking();
hal.util->snprintf(message_to_send, sizeof(message_to_send), "VNWRG,06,2");
send_command_blocking();

// Detect version
// Read Model Number Register, ID 1
wait_register_responce(1);
hal.util->snprintf(message_to_send, sizeof(message_to_send), "VNRRG,01");
send_command_blocking();

// Setup for messages respective model types (on both UARTs)
if (strncmp(model_name, "VN-1", 4) == 0) {
// VN-100
// VN-1X0
type = TYPE::VN_AHRS;

// This assumes unit is still configured at its default rate of 800hz
nmea_printf(uart, "$VNWRG,75,3,%u,14,073E,0004", unsigned(800/get_rate()));

hal.util->snprintf(message_to_send, sizeof(message_to_send),
"VNWRG,75,3,%u,14,073E,0004", unsigned(800 / get_rate()));
send_command_blocking();
} else {
// Default to setup for sensors other than VN-100 or VN-110
// This assumes unit is still configured at its default IMU rate of 400hz for VN-300, 800hz for others
Expand All @@ -432,11 +447,22 @@ void AP_ExternalAHRS_VectorNav::update_thread()
if (strncmp(model_name, "VN-3", 4) == 0) {
has_dual_gnss = true;
}
nmea_printf(uart, "$VNWRG,75,3,%u,34,072E,0106,0612", unsigned(imu_rate/get_rate()));
nmea_printf(uart, "$VNWRG,76,3,%u,4E,0002,0010,20B8,0018", unsigned(imu_rate/5));
hal.util->snprintf(message_to_send, sizeof(message_to_send),
"VNWRG,75,3,%u,34,072E,0106,0612", unsigned(imu_rate / get_rate()));
send_command_blocking();
hal.util->snprintf(message_to_send, sizeof(message_to_send),
"VNWRG,76,3,%u,4E,0002,0010,20B8,0018", unsigned(imu_rate / 5));
send_command_blocking();
}

// Resume asynchronous communications
hal.util->snprintf(message_to_send, sizeof(message_to_send), "VNASY,1");
send_command_blocking();
setup_complete = true;
}

void AP_ExternalAHRS_VectorNav::update_thread() {
initialize();
while (true) {
if (!check_uart()) {
hal.scheduler->delay(1);
Expand Down
12 changes: 7 additions & 5 deletions libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.h
Original file line number Diff line number Diff line change
Expand Up @@ -61,10 +61,12 @@ class AP_ExternalAHRS_VectorNav : public AP_ExternalAHRS_backend {
void update_thread();
bool check_uart();

void initialize();

void process_ins_packet1(const uint8_t *b);
void process_ins_packet2(const uint8_t *b);
void process_ahrs_packet(const uint8_t *b);
void wait_register_responce(const uint8_t register_num);
void send_command_blocking();

uint8_t *pktbuf;
uint16_t pktoffset;
Expand All @@ -83,22 +85,22 @@ class AP_ExternalAHRS_VectorNav : public AP_ExternalAHRS_backend {

bool has_dual_gnss = false;

char model_name[25];
char model_name[20];

char message_to_send[50];
// NMEA parsing for setup
bool decode(char c);
bool decode_latest_term();
struct NMEA_parser {
char term[25]; // buffer for the current term within the current sentence
char term[20]; // buffer for the current term within the current sentence
uint8_t term_offset; // offset within the _term buffer where the next character should be placed
uint8_t term_number; // term index within the current sentence
uint8_t checksum; // checksum accumulator
bool term_is_checksum; // current term is the checksum
bool sentence_valid; // is current sentence valid so far
bool sentence_done; // true if this sentence has already been decoded
uint8_t register_number; // VectorNAV register number were reading
bool error_response; // true if received a VNERR response
} nmea;

};

#endif // AP_EXTERNAL_AHRS_VECTORNAV_ENABLED
Loading