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.

Sign up for GitHub

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

Merged
merged 7 commits into from
Jul 17, 2024
Merged

Add validation to initialization configuration commands #27459

Show file tree
Hide file tree
Changes from 2 commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
3207d2f
AP_ExternalAHRS: VectorNav: Add response validation to all sent commands
lashVN Jul 5, 2024
d4722b1
AP_ExternalAHRS: VectorNav: Disable ASCII messages on both ports, rat…
lashVN Jul 5, 2024
884a889
AP_ExternalAHRS_VectorNav: Handle VNERR responses
lashVN Jul 8, 2024
51db6eb
AP_ExternalAHRS_VectorNav: Minor code clarity improvements
lashVN Jul 9, 2024
9611503
AP_ExternalAHRS_VectorNav: Bugfixes and review comment address
lashVN Jul 10, 2024
a8370f3
AP_ExternalAHRS_VectorNav: Update VectorNav sim to respond to receive…
lashVN Jul 16, 2024
93673c7
AP_ExternalAHRS_VectorNav: Move allocation to local scope
lashVN Jul 16, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
79 changes: 48 additions & 31 deletions libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -286,21 +286,18 @@ 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;
}

Expand Down Expand Up @@ -371,57 +368,66 @@ 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
AP_HAL::panic("VectorNav received VN error response");
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Panicing is not a great idea, it effectively just gives up. Would you expect to see this in normal operation? In which case it should trigger a pre-arm. Or is it only if there is some edge case and very unlikely? I which case you could throw a internal error.

Would it be worth trying a reset of the vector nav to see if we could recover?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@IamPete1

The only cases I'd expect to see an error in response is

  1. If something is catastrophically wrong with the VN sensor (hardware failure, etc)
  2. If the code was incorrectly edited and sent a command that isn't acceptable to the sensor
  3. If the AP-configured IMU Rate (via get_rate()) is set too high, we will reply with insufficient baudrate VNERR

I don't think any of those cases would be resolved by resetting the sensor, but I agree especially since (3) is very viable with normal use panicing may not be the not the correct response. Given the failure cases, what would you suggest instead?

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

OK, in that case, I would print the error specific (GCS_SEND_TEXT) so the user will have some feedback. Then you need to make sure that they can't arm. Hopefully the existing health checks are sufficient, if not then update those so they give a "init failed" error.

Really there are three things that you need to acheave:

  • Not prevent the rest of the code running.
  • Give the user a clue so they can track down how to resolve
  • Prevent the vehicle flying in a unsafe state

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Just pushed a change to resolve this issue.

In this change, the system will continue to send the message to the VectorNav every READ_REQUEST_RETRY_MS (500ms). The pre-arm check should continue to show as a failure, because the system will never pass initialization (assuming the VNERR is never resolved) THis is consistent with previous behavior: block initialization and retry until a valid response is received. It will result in the received error message being sent to GCS every 500ms.

}
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 (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 +438,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
11 changes: 6 additions & 5 deletions libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.h
View file
Open in desktop
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,21 @@ 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
} nmea;

};

#endif // AP_EXTERNAL_AHRS_VECTORNAV_ENABLED
Loading