mirror of https://github.com/ArduPilot/ardupilot
79 lines
2.2 KiB
Plaintext
79 lines
2.2 KiB
Plaintext
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
|
|
|
|
#if ENABLE_HIL
|
|
// An Xplane/Flightgear output
|
|
// Use with the GPS_IMU to do Hardware in teh loop simulations
|
|
|
|
byte buf_len = 0;
|
|
byte out_buffer[32];
|
|
|
|
void output_HIL(void)
|
|
{
|
|
// output real-time sensor data
|
|
Serial.print("AAA"); // Message preamble
|
|
output_int((int)(rc_1.servo_out)); // 0 bytes 0, 1
|
|
output_int((int)(rc_2.servo_out)); // 1 bytes 2, 3
|
|
output_int((int)(rc_3.servo_out)); // 2 bytes 4, 5
|
|
output_int((int)(rc_4.servo_out)); // 3 bytes 6, 7
|
|
output_int((int)wp_distance); // 4 bytes 8,9
|
|
output_int((int)bearing_error); // 5 bytes 10,11
|
|
output_int((int)next_WP.alt / 100); // 6 bytes 12, 13
|
|
output_int((int)energy_error); // 7 bytes 14,15
|
|
output_byte(wp_index); // 8 bytes 16
|
|
output_byte(control_mode); // 9 bytes 17
|
|
|
|
// print out the buffer and checksum
|
|
// ---------------------------------
|
|
print_buffer();
|
|
}
|
|
|
|
// This is for debugging only!,
|
|
// I just move the underscore to keep the above version pristene.
|
|
void output_HIL_(void)
|
|
{
|
|
// output real-time sensor data
|
|
Serial.print("AAA"); // Message preamble
|
|
output_int((int)(rc_1.servo_out)); // 0 bytes 0, 1
|
|
output_int((int)(rc_2.servo_out)); // 1 bytes 2, 3
|
|
output_int((int)(rc_3.servo_out)); // 2 bytes 4, 5
|
|
output_int((int)(rc_4.servo_out)); // 3 bytes 6, 7
|
|
output_int((int)wp_distance); // 4 bytes 8, 9
|
|
output_int((int)bearing_error); // 5 bytes 10,11
|
|
output_int((int)roll_sensor); // 6 bytes 12,13
|
|
output_int((int)loiter_total); // 7 bytes 14,15
|
|
output_byte(wp_index); // 8 bytes 16
|
|
output_byte(control_mode); // 9 bytes 17
|
|
|
|
// print out the buffer and checksum
|
|
// ---------------------------------
|
|
print_buffer();
|
|
}
|
|
|
|
void output_int(int value)
|
|
{
|
|
//buf_len += 2;
|
|
out_buffer[buf_len++] = value & 0xff;
|
|
out_buffer[buf_len++] = (value >> 8) & 0xff;
|
|
}
|
|
|
|
void output_byte(byte value)
|
|
{
|
|
out_buffer[buf_len++] = value;
|
|
}
|
|
|
|
void print_buffer()
|
|
{
|
|
byte ck_a = 0;
|
|
byte ck_b = 0;
|
|
for (byte i = 0;i < buf_len; i++){
|
|
Serial.print (out_buffer[i]);
|
|
}
|
|
Serial.print('\r');
|
|
Serial.print('\n');
|
|
buf_len = 0;
|
|
}
|
|
|
|
|
|
#endif
|
|
|