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.
113 lines
2.3 KiB
113 lines
2.3 KiB
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- |
|
|
|
/// |
|
/// @file GCS_Xplane.pde |
|
/// @brief GCS driver for the X-plane HIL interface. |
|
/// |
|
|
|
#if GCS_PROTOCOL == GCS_PROTOCOL_XPLANE |
|
|
|
void |
|
GCS_XPLANE::send_text(uint8_t severity, const char *str) |
|
{ |
|
if(severity >= DEBUG_LEVEL){ |
|
Serial.print("MSG: "); |
|
Serial.println(str); |
|
} |
|
} |
|
|
|
void |
|
GCS_XPLANE::send_message(uint8_t id, uint32_t param) |
|
{ |
|
switch(id) { |
|
case MSG_HEARTBEAT: |
|
print_control_mode(); |
|
break; |
|
|
|
case MSG_ATTITUDE: |
|
//print_attitude(); |
|
break; |
|
|
|
case MSG_LOCATION: |
|
//print_position(); |
|
break; |
|
|
|
case MSG_CPU_LOAD: |
|
//TODO: implement appropriate routine here if applicable |
|
break; |
|
|
|
case MSG_COMMAND_LIST: |
|
struct Location cmd = get_wp_with_index(param); |
|
print_waypoint(&cmd, param); |
|
break; |
|
|
|
} |
|
} |
|
|
|
void |
|
GCS_XPLANE::print_control_mode(void) |
|
{ |
|
Serial.print("MSG: "); |
|
Serial.print(flight_mode_strings[control_mode]); |
|
} |
|
|
|
void |
|
GCS_XPLANE::print_current_waypoints() |
|
{ |
|
Serial.print("MSG: "); |
|
Serial.print("CUR:"); |
|
Serial.print("\t"); |
|
Serial.print(current_loc.lat,DEC); |
|
Serial.print(",\t"); |
|
Serial.print(current_loc.lng,DEC); |
|
Serial.print(",\t"); |
|
Serial.println(current_loc.alt,DEC); |
|
|
|
Serial.print("MSG: "); |
|
Serial.print("NWP:"); |
|
Serial.print(wp_index,DEC); |
|
Serial.print(",\t"); |
|
Serial.print(next_WP.lat,DEC); |
|
Serial.print(",\t"); |
|
Serial.print(next_WP.lng,DEC); |
|
Serial.print(",\t"); |
|
Serial.println(next_WP.alt,DEC); |
|
} |
|
|
|
void |
|
GCS_XPLANE::print_waypoint(struct Location *cmd,byte index) |
|
{ |
|
Serial.print("MSG: command #: "); |
|
Serial.print(index, DEC); |
|
Serial.print(" id: "); |
|
Serial.print(cmd->id,DEC); |
|
Serial.print(" p1: "); |
|
Serial.print(cmd->p1,DEC); |
|
Serial.print(" p2: "); |
|
Serial.print(cmd->alt,DEC); |
|
Serial.print(" p3: "); |
|
Serial.print(cmd->lat,DEC); |
|
Serial.print(" p4: "); |
|
Serial.println(cmd->lng,DEC); |
|
} |
|
|
|
void |
|
GCS_XPLANE::print_waypoints() |
|
{ |
|
Serial.println("Commands in memory"); |
|
Serial.print("commands total: "); |
|
Serial.println(wp_total, DEC); |
|
// create a location struct to hold the temp Waypoints for printing |
|
//Location tmp; |
|
Serial.println("Home: "); |
|
struct Location cmd = get_wp_with_index(0); |
|
print_waypoint(&cmd, 0); |
|
Serial.println("Commands: "); |
|
|
|
for (int i=1; i<wp_total; i++){ |
|
cmd = get_wp_with_index(i); |
|
print_waypoint(&cmd, i); |
|
} |
|
} |
|
|
|
#endif // GCS_PROTOCOL_XPLANE
|
|
|