You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
78 lines
2.2 KiB
78 lines
2.2 KiB
// -*- 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)dcm.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 |
|
|
|
|