Skip to content

Commit

Permalink
SITL: correct and improve Hirth simulator
Browse files Browse the repository at this point in the history
we weren't sending an ACK for the set-values message
  • Loading branch information
peterbarker committed Jun 18, 2024
1 parent 7003647 commit 7d81e2c
Showing 1 changed file with 13 additions and 1 deletion.
14 changes: 13 additions & 1 deletion libraries/SITL/SIM_EFI_Hirth.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -61,13 +61,17 @@ void EFI_Hirth::update_receive()
if (received_packet_code == PacketCode::SetValues) {
// do this synchronously for now
handle_set_values();
} else {
} else if (uint8_t(received_packet_code) == 0x04 ||
uint8_t(received_packet_code) == 0x0B ||
uint8_t(received_packet_code) == 0x0D) {
assert_receive_size(3);
if (requested_data_record.time_ms != 0) {
AP_HAL::panic("Requesting too fast?");
}
requested_data_record.code = received_packet_code;
requested_data_record.time_ms = AP_HAL::millis();
} else {
AP_HAL::panic("Invalid packet code");
}
} else {
AP_HAL::panic("checksum failed");
Expand All @@ -90,6 +94,14 @@ void EFI_Hirth::handle_set_values()
assert_receive_size(23);
static_assert(sizeof(settings) == 20, "correct number of bytes in settings");
memcpy((void*)&settings, &receive_buf[2], sizeof(settings));

// send ACK for set-values
constexpr uint8_t set_values_ack[] {
3, // length
uint8_t(PacketCode::SetValues), // code
3 + uint8_t(PacketCode::SetValues)
};
write_to_autopilot((const char*)set_values_ack, sizeof(set_values_ack));
}

void EFI_Hirth::update_send()
Expand Down

0 comments on commit 7d81e2c

Please sign in to comment.