diff --git a/ArduPlane/APM_Config.h b/ArduPlane/APM_Config.h index 40fcc180a9..7282760e37 100644 --- a/ArduPlane/APM_Config.h +++ b/ArduPlane/APM_Config.h @@ -16,9 +16,5 @@ // The following are the recommended settings for Xplane simulation. Remove the leading "/* and trailing "*/" to enable: /* -#define HIL_PROTOCOL HIL_PROTOCOL_MAVLINK #define HIL_MODE HIL_MODE_ATTITUDE -#define HIL_PORT 0 -#define GCS_PROTOCOL GCS_PROTOCOL_MAVLINK -#define GCS_PORT 3 */ diff --git a/ArduPlane/APM_Config_mavlink_hil.h b/ArduPlane/APM_Config_mavlink_hil.h index 189950e9b7..23968a9773 100644 --- a/ArduPlane/APM_Config_mavlink_hil.h +++ b/ArduPlane/APM_Config_mavlink_hil.h @@ -23,41 +23,11 @@ // 2. HIL_MODE_SENSORS: full sensor simulation #define HIL_MODE HIL_MODE_ATTITUDE -// HIL_PORT SELCTION -// -// PORT 0 -// Port 0 is the port usually used for HIL. You attatch your HIL computer -// to APM using a USB cable. You can run telemetry on Port 3 -// -// PORT 1 -// Available for special situations. This uses the port that would have -// been used for the gps -// as the hardware in the loop port. You will have to solder -// headers onto the gps port connection on the apm -// and connect via an ftdi cable. -// The buad rate is controlled by SERIAL1_BAUD in this mode. -// -// PORT 3 -// If you don't require telemetry communication with a gcs while running -// hardware in the loop you may use the telemetry port as the hardware in -// the loop port. Alternatively, use a telemetry/HIL shim like FGShim -// https://ardupilot-mega.googlecode.com/svn/Tools/trunk/FlightGear -// -// The buad rate is controlled by SERIAL3_BAUD in this mode. - -#define HIL_PORT 0 - // You can set your gps protocol here for your actual // hardware and leave it without affecting the hardware // in the loop simulation #define GPS_PROTOCOL GPS_PROTOCOL_MTK -// Ground control station comms -#if HIL_PORT != 3 -# define GCS_PROTOCOL GCS_PROTOCOL_MAVLINK -# define GCS_PORT 3 -#endif - // Sensors // All sensors are supported in all modes. // The magnetometer is not used in diff --git a/ArduPlane/APM_Config_xplane.h b/ArduPlane/APM_Config_xplane.h deleted file mode 100644 index 38af972bad..0000000000 --- a/ArduPlane/APM_Config_xplane.h +++ /dev/null @@ -1,22 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -// THIS IS A SAMPLE CONFIGURATION FILE FOR DOING HARDWARE IN THE LOOP TESTING USING THE ORIGINAL X-PLANE INTERFACE -// IF YOU WANTED TO USE THIS YOU WOULD COPY THE CONTENTS INTO YOUR APM_Config.h FILE! - - -#define FLIGHT_MODE_CHANNEL 8 -#define FLIGHT_MODE_1 AUTO -#define FLIGHT_MODE_2 RTL -#define FLIGHT_MODE_3 FLY_BY_WIRE_A -#define FLIGHT_MODE_4 FLY_BY_WIRE_A -#define FLIGHT_MODE_5 MANUAL -#define FLIGHT_MODE_6 MANUAL - -#define HIL_PROTOCOL HIL_PROTOCOL_XPLANE - -#define HIL_MODE HIL_MODE_ATTITUDE -#define HIL_PORT 0 - -#define AIRSPEED_CRUISE 25 -#define THROTTLE_FAILSAFE ENABLED -#define AIRSPEED_SENSOR ENABLED diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 569708c354..c063bb0c5f 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -51,7 +51,6 @@ version 2.1 of the License, or (at your option) any later version. #include "defines.h" #include "Parameters.h" #include "GCS.h" -#include "HIL.h" //////////////////////////////////////////////////////////////////////////////// // Serial ports @@ -149,19 +148,6 @@ AP_IMU_Shim imu; // never used #error Unrecognised HIL_MODE setting. #endif // HIL MODE -#if HIL_MODE != HIL_MODE_DISABLED - #if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK - GCS_MAVLINK hil(Parameters::k_param_streamrates_port0); - #elif HIL_PROTOCOL == HIL_PROTOCOL_XPLANE - HIL_XPLANE hil; - #endif // HIL PROTOCOL -#endif // HIL_MODE - -// We may have a hil object instantiated just for mission planning -#if HIL_MODE == HIL_MODE_DISABLED && HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && HIL_PORT == 0 - GCS_MAVLINK hil(Parameters::k_param_streamrates_port0); -#endif - #if HIL_MODE != HIL_MODE_ATTITUDE #if HIL_MODE != HIL_MODE_SENSORS // Normal @@ -178,12 +164,8 @@ AP_IMU_Shim imu; // never used // GCS selection //////////////////////////////////////////////////////////////////////////////// // -#if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK - GCS_MAVLINK gcs(Parameters::k_param_streamrates_port3); -#else -// If we are not using a GCS, we need a stub that does nothing. - GCS_Class gcs; -#endif +GCS_MAVLINK gcs(Parameters::k_param_streamrates_port3); +GCS_MAVLINK hil(Parameters::k_param_streamrates_port0); //////////////////////////////////////////////////////////////////////////////// // SONAR selection @@ -481,9 +463,7 @@ static void fast_loop() // try to send any deferred messages if the serial port now has // some space available gcs.send_message(MSG_RETRY_DEFERRED); -#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0) hil.send_message(MSG_RETRY_DEFERRED); -#endif // check for loss of control signal failsafe condition // ------------------------------------ @@ -538,28 +518,10 @@ static void fast_loop() // XXX is it appropriate to be doing the comms below on the fast loop? - #if HIL_MODE != HIL_MODE_DISABLED && HIL_PORT != GCS_PORT - // kick the HIL to process incoming sensor packets - hil.update(); - - #if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK - hil.data_stream_send(45,1000); - #else - hil.send_message(MSG_SERVO_OUT); - #endif - #elif HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && HIL_MODE == HIL_MODE_DISABLED && HIL_PORT == 0 - // Case for hil object on port 0 just for mission planning - hil.update(); - hil.data_stream_send(45,1000); - #endif - - // kick the GCS to process uplink data gcs.update(); - #if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK - gcs.data_stream_send(45,1000); - #endif - // XXX this should be absorbed into the above, - // or be a "GCS fast loop" interface + hil.update(); + gcs.data_stream_send(45,1000); + hil.data_stream_send(45,1000); } static void medium_loop() @@ -650,18 +612,10 @@ Serial.println(tempaccel.z, DEC); if (g.log_bitmask & MASK_LOG_GPS) Log_Write_GPS(g_gps->time, current_loc.lat, current_loc.lng, g_gps->altitude, current_loc.alt, (long) g_gps->ground_speed, g_gps->ground_course, g_gps->fix, g_gps->num_sats); - // XXX this should be a "GCS medium loop" interface - #if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK - gcs.data_stream_send(5,45); - // send all requested output streams with rates requested - // between 5 and 45 Hz - #else - gcs.send_message(MSG_ATTITUDE); // Sends attitude data - #endif - - #if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0) - hil.data_stream_send(5,45); - #endif + // send all requested output streams with rates requested + // between 5 and 45 Hz + gcs.data_stream_send(5,45); + hil.data_stream_send(5,45); break; // This case controls the slow loop @@ -719,21 +673,9 @@ static void slow_loop() slow_loopCounter = 0; update_events(); - #if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK - mavlink_system.sysid = g.sysid_this_mav; // This is just an ugly hack to keep mavlink_system.sysid sync'd with our parameter - gcs.data_stream_send(3,5); - // send all requested output streams with rates requested - // between 3 and 5 Hz - #else - gcs.send_message(MSG_LOCATION); - gcs.send_message(MSG_CPU_LOAD, load*100); - #endif - - #if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0) - hil.data_stream_send(3,5); - #endif - - + mavlink_system.sysid = g.sysid_this_mav; // This is just an ugly hack to keep mavlink_system.sysid sync'd with our parameter + gcs.data_stream_send(3,5); + hil.data_stream_send(3,5); break; } } @@ -745,16 +687,9 @@ static void one_second_loop() // send a heartbeat gcs.send_message(MSG_HEARTBEAT); - #if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK - gcs.data_stream_send(1,3); - // send all requested output streams with rates requested - // between 1 and 3 Hz - #endif - - #if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0) - hil.send_message(MSG_HEARTBEAT); - hil.data_stream_send(1,3); - #endif + hil.send_message(MSG_HEARTBEAT); + gcs.data_stream_send(1,3); + hil.data_stream_send(1,3); } static void update_GPS(void) diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index bc0268ab66..a475e4198d 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -1111,18 +1111,14 @@ static void mavlink_delay(unsigned long t) if (tnow - last_1hz > 1000) { last_1hz = tnow; gcs.send_message(MSG_HEARTBEAT); - gcs.send_message(MSG_EXTENDED_STATUS1); -#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0) hil.send_message(MSG_HEARTBEAT); + gcs.send_message(MSG_EXTENDED_STATUS1); hil.send_message(MSG_EXTENDED_STATUS1); -#endif } if (tnow - last_50hz > 20) { last_50hz = tnow; gcs.update(); -#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0) hil.update(); -#endif } delay(1); } while (millis() - tstart < t); diff --git a/ArduPlane/HIL.h b/ArduPlane/HIL.h deleted file mode 100644 index 70ad73d6de..0000000000 --- a/ArduPlane/HIL.h +++ /dev/null @@ -1,135 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -/// @file HIL.h -/// @brief Interface definition for the various Ground Control System protocols. - -#ifndef __HIL_H -#define __HIL_H - -# if HIL_MODE != HIL_MODE_DISABLED - -#include -#include -#include -#include -#include - -/// -/// @class HIL -/// @brief Class describing the interface between the APM code -/// proper and the HIL implementation. -/// -/// HIL' are currently implemented inside the sketch and as such have -/// access to all global state. The sketch should not, however, call HIL -/// internal functions - all calls to the HIL should be routed through -/// this interface (or functions explicitly exposed by a subclass). -/// -class HIL_Class -{ -public: - - /// Startup initialisation. - /// - /// This routine performs any one-off initialisation required before - /// HIL messages are exchanged. - /// - /// @note The stream is expected to be set up and configured for the - /// correct bitrate before ::init is called. - /// - /// @note The stream is currently BetterStream so that we can use the _P - /// methods; this may change if Arduino adds them to Print. - /// - /// @param port The stream over which messages are exchanged. - /// - void init(FastSerial *port) { _port = port; } - - /// Update HIL state. - /// - /// This may involve checking for received bytes on the stream, - /// or sending additional periodic messages. - void update(void) {} - - /// Send a message with a single numeric parameter. - /// - /// This may be a standalone message, or the HIL driver may - /// have its own way of locating additional parameters to send. - /// - /// @param id ID of the message to send. - /// @param param Explicit message parameter. - /// - void send_message(uint8_t id, int32_t param = 0) {} - - /// Send a text message. - /// - /// @param severity A value describing the importance of the message. - /// @param str The text to be sent. - /// - void send_text(uint8_t severity, const char *str) {} - - /// Send a text message with a PSTR() - /// - /// @param severity A value describing the importance of the message. - /// @param str The text to be sent. - /// - void send_text(uint8_t severity, const prog_char_t *str) {} - - /// Send acknowledgement for a message. - /// - /// @param id The message ID being acknowledged. - /// @param sum1 Checksum byte 1 from the message being acked. - /// @param sum2 Checksum byte 2 from the message being acked. - /// - void acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2) {} - -protected: - /// The stream we are communicating over - FastSerial *_port; -}; - -// -// HIL class definitions. -// -// These are here so that we can declare the HIL object early in the sketch -// and then reference it statically rather than via a pointer. -// - -/// -/// @class HIL_MAVLINK -/// @brief The mavlink protocol for hil -/// -#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK -// uses gcs -#endif // HIL_PROTOCOL_MAVLINK - -/// -/// @class HIL_XPLANE -/// @brief The xplane protocol for hil -/// -#if HIL_PROTOCOL == HIL_PROTOCOL_XPLANE -class HIL_XPLANE : public HIL_Class -{ -public: - HIL_XPLANE(); - void update(void); - void init(FastSerial *port); - void send_message(uint8_t id, uint32_t param = 0); - void send_text(uint8_t severity, const char *str); - void send_text(uint8_t severity, const prog_char_t *str); - void acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2); -private: - void output_HIL(); - void output_HIL_(); - void output_int(int value); - void output_byte(byte value); - void print_buffer(); - - AP_GPS_IMU * hilPacketDecoder; - byte buf_len; - byte out_buffer[32]; -}; -#endif // HIL_PROTOCOL_XPLANE - -#endif // HIL not disabled - -#endif // __HIL_H - diff --git a/ArduPlane/HIL_Xplane.pde b/ArduPlane/HIL_Xplane.pde deleted file mode 100644 index d87b01c344..0000000000 --- a/ArduPlane/HIL_Xplane.pde +++ /dev/null @@ -1,129 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#if HIL_MODE != HIL_MODE_DISABLED && HIL_PROTOCOL == HIL_PROTOCOL_XPLANE - - -void HIL_XPLANE::output_HIL(void) -{ - // output real-time sensor data - Serial.printf_P(PSTR("AAA")); // Message preamble - output_int((int)(g.channel_roll.servo_out)); // 0 bytes 0, 1 - output_int((int)(g.channel_pitch.servo_out)); // 1 bytes 2, 3 - output_int((int)(g.channel_throttle.servo_out)); // 2 bytes 4, 5 - output_int((int)(g.channel_rudder.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)altitude_error); // 6 bytes 12, 13 - output_int((int)energy_error); // 7 bytes 14,15 - output_byte((int)g.waypoint_index); // 8 bytes 16 - output_byte(control_mode); // 9 bytes 17 - - // print out the buffer and checksum - // --------------------------------- - print_buffer(); -} - -void HIL_XPLANE::output_int(int value) -{ - //buf_len += 2; - out_buffer[buf_len++] = value & 0xff; - out_buffer[buf_len++] = (value >> 8) & 0xff; -} - -void HIL_XPLANE::output_byte(byte value) -{ - out_buffer[buf_len++] = value; -} - -void HIL_XPLANE::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; -} - - - -HIL_XPLANE::HIL_XPLANE() : - buf_len(0) -{ -} - -void -HIL_XPLANE::init(FastSerial * port) -{ - HIL_Class::init(port); - hilPacketDecoder = new AP_GPS_IMU(port); - hilPacketDecoder->init(); -} - -void -HIL_XPLANE::update(void) -{ - hilPacketDecoder->update(); - airspeed = (float)hilPacketDecoder->airspeed; //airspeed is * 100 coming in from Xplane for accuracy - calc_airspeed_errors(); - dcm.setHil(hilPacketDecoder->roll_sensor*M_PI/18000, - hilPacketDecoder->pitch_sensor*M_PI/18000, - hilPacketDecoder->ground_course*M_PI/18000, - 0,0,0); - - // set gps hil sensor - g_gps->setHIL(hilPacketDecoder->time/1000.0,(float)hilPacketDecoder->latitude/1.0e7,(float)hilPacketDecoder->longitude/1.0e7,(float)hilPacketDecoder->altitude/1.0e2, - (float)hilPacketDecoder->speed_3d/1.0e2,(float)hilPacketDecoder->ground_course/1.0e2,0,0); -} - -void -HIL_XPLANE::send_message(uint8_t id, uint32_t param) -{ - // TODO: split output by actual request - uint64_t timeStamp = micros(); - switch(id) { - - case MSG_HEARTBEAT: - break; - case MSG_EXTENDED_STATUS: - break; - case MSG_ATTITUDE: - break; - case MSG_LOCATION: - break; - case MSG_GPS_RAW: - break; - case MSG_SERVO_OUT: - output_HIL(); - break; - case MSG_RADIO_OUT: - break; - case MSG_RAW_IMU: - break; - case MSG_GPS_STATUS: - break; - case MSG_CURRENT_WAYPOINT: - break; - defualt: - break; - } -} - -void -HIL_XPLANE::send_text(uint8_t severity, const char *str) -{ -} - -void -HIL_XPLANE::send_text(uint8_t severity, const prog_char_t *str) -{ -} - -void -HIL_XPLANE::acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2) -{ -} - -#endif diff --git a/ArduPlane/config.h b/ArduPlane/config.h index d48b4cb73d..69cc9df28f 100644 --- a/ArduPlane/config.h +++ b/ArduPlane/config.h @@ -72,27 +72,15 @@ ////////////////////////////////////////////////////////////////////////////// -// HIL_PROTOCOL OPTIONAL // HIL_MODE OPTIONAL -// HIL_PORT OPTIONAL #ifndef HIL_MODE #define HIL_MODE HIL_MODE_DISABLED #endif -#ifndef HIL_PROTOCOL -#define HIL_PROTOCOL HIL_PROTOCOL_MAVLINK -#endif - -#ifndef HIL_PORT -#define HIL_PORT 0 -#endif - #if HIL_MODE != HIL_MODE_DISABLED // we are in HIL mode - # undef GPS_PROTOCOL # define GPS_PROTOCOL GPS_PROTOCOL_NONE - #endif diff --git a/ArduPlane/system.pde b/ArduPlane/system.pde index 4c994cdb3e..b7893a821c 100644 --- a/ArduPlane/system.pde +++ b/ArduPlane/system.pde @@ -60,8 +60,6 @@ static void init_ardupilot() // GPS serial port. // - // Not used if the IMU/X-Plane GPS is in use. - // // XXX currently the EM406 (SiRF receiver) is nominally configured // at 57600, however it's not been supported to date. We should // probably standardise on 38400. @@ -69,13 +67,8 @@ static void init_ardupilot() // XXX the 128 byte receive buffer may be too small for NMEA, depending // on the message set configured. // - #if HIL_MODE != HIL_MODE_DISABLED && HIL_PORT == 1 // TODO: figure out a better way to do this - // Steal gps port for hardware in the loop - Serial1.begin(115200, 128, 128); - #else - // standard gps running - Serial1.begin(38400, 128, 16); - #endif + // standard gps running + Serial1.begin(38400, 128, 16); Serial.printf_P(PSTR("\n\nInit " THISFIRMWARE "\n\nFree RAM: %lu\n"), @@ -176,22 +169,8 @@ static void init_ardupilot() mavlink_system.compid = 1; //MAV_COMP_ID_IMU; // We do not check for comp id mavlink_system.type = MAV_FIXED_WING; -// init the HIL -#if HIL_MODE != HIL_MODE_DISABLED - - #if HIL_PORT == 3 - hil.init(&Serial3); - #elif HIL_PORT == 1 - hil.init(&Serial1); - #else + // init the HIL hil.init(&Serial); - #endif -#endif - -// We may have a hil object instantiated just for mission planning -#if HIL_MODE == HIL_MODE_DISABLED && HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && HIL_PORT == 0 - hil.init(&Serial); -#endif rc_override_active = APM_RC.setHIL(rc_override); // Set initial values for no override init_rc_in(); // sets up rc channels from radio