diff --git a/ArduCopter/APM_Config.h b/ArduCopter/APM_Config.h index 3bdf538b12..86a00ee034 100644 --- a/ArduCopter/APM_Config.h +++ b/ArduCopter/APM_Config.h @@ -54,8 +54,6 @@ // other settings //#define THROTTLE_IN_DEADBAND 100 // redefine size of throttle deadband in pwm (0 ~ 1000) -//#define HIL_MODE HIL_MODE_SENSORS // build for hardware-in-the-loop simulation - // User Hooks : For User Developed code that you wish to run // Put your variable definitions into the UserVariables.h file (or another file name and then change the #define below). //#define USERHOOK_VARIABLES "UserVariables.h" diff --git a/ArduCopter/APM_Config_mavlink_hil.h b/ArduCopter/APM_Config_mavlink_hil.h deleted file mode 100644 index 9c5d37c78a..0000000000 --- a/ArduCopter/APM_Config_mavlink_hil.h +++ /dev/null @@ -1,28 +0,0 @@ -// HIL_MODE SELECTION -// -// Mavlink supports -// 1. HIL_MODE_SENSORS: full sensor simulation -#define HIL_MODE HIL_MODE_SENSORS - -// HIL_PORT SELECTION -// -// PORT 1 -// If you would like to run telemetry communications for a groundstation -// while you are running hardware in the loop it is necessary to set -// HIL_PORT to 1. 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 baud rate is set to 115200 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 SERIAL1_BAUD in this mode. - -#define HIL_PORT 3 - diff --git a/ArduCopter/AP_Arming.cpp b/ArduCopter/AP_Arming.cpp index 7e52fcb221..eb7cb5384e 100644 --- a/ArduCopter/AP_Arming.cpp +++ b/ArduCopter/AP_Arming.cpp @@ -801,7 +801,7 @@ bool AP_Arming_Copter::arm(const AP_Arming::Method method, const bool do_arming_ AP::notify().update(); } -#if HIL_MODE != HIL_MODE_DISABLED || CONFIG_HAL_BOARD == HAL_BOARD_SITL +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL gcs().send_text(MAV_SEVERITY_INFO, "Arming motors"); #endif @@ -895,7 +895,7 @@ bool AP_Arming_Copter::disarm(const AP_Arming::Method method, bool do_disarm_che return false; } -#if HIL_MODE != HIL_MODE_DISABLED || CONFIG_HAL_BOARD == HAL_BOARD_SITL +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL gcs().send_text(MAV_SEVERITY_INFO, "Disarming motors"); #endif diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index 41f7931a08..329f565c8a 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -447,12 +447,6 @@ void Copter::ten_hz_logging_loop() // twentyfive_hz_logging - should be run at 25hz void Copter::twentyfive_hz_logging() { -#if HIL_MODE != HIL_MODE_DISABLED - // HIL for a copter needs very fast update of the servo values - gcs().send_message(MSG_SERVO_OUTPUT_RAW); -#endif - -#if HIL_MODE == HIL_MODE_DISABLED if (should_log(MASK_LOG_ATTITUDE_FAST)) { Log_Write_EKF_POS(); } @@ -460,7 +454,6 @@ void Copter::twentyfive_hz_logging() if (should_log(MASK_LOG_IMU)) { AP::ins().Write_IMU(); } -#endif #if MODE_AUTOROTATE_ENABLED == ENABLED if (should_log(MASK_LOG_ATTITUDE_MED) || should_log(MASK_LOG_ATTITUDE_FAST)) { @@ -602,13 +595,6 @@ void Copter::update_super_simple_bearing(bool force_update) void Copter::read_AHRS(void) { - // Perform IMU calculations and get attitude info - //----------------------------------------------- -#if HIL_MODE != HIL_MODE_DISABLED - // update hil before ahrs update - gcs().update(); -#endif - // we tell AHRS to skip INS update as we have already done it in fast_loop() ahrs.update(true); } diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 52eeaa441a..d3fc27db93 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -55,10 +55,6 @@ MAV_MODE GCS_MAVLINK_Copter::base_mode() const // override if stick mixing is enabled _base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; -#if HIL_MODE != HIL_MODE_DISABLED - _base_mode |= MAV_MODE_FLAG_HIL_ENABLED; -#endif - // we are armed if we are not initialising if (copter.motors != nullptr && copter.motors->armed()) { _base_mode |= MAV_MODE_FLAG_SAFETY_ARMED; @@ -372,8 +368,8 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = { AP_GROUPINFO("RC_CHAN", 2, GCS_MAVLINK_Parameters, streamRates[2], 0), // @Param: RAW_CTRL - // @DisplayName: Raw Control stream rate to ground station - // @Description: Stream rate of RC_CHANNELS_SCALED (HIL only) to ground station + // @DisplayName: Unused + // @Description: Unused // @Units: Hz // @Range: 0 50 // @Increment: 1 @@ -1253,53 +1249,6 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg) } #endif -#if HIL_MODE != HIL_MODE_DISABLED - case MAVLINK_MSG_ID_HIL_STATE: // MAV ID: 90 - { - mavlink_hil_state_t packet; - mavlink_msg_hil_state_decode(&msg, &packet); - - // sanity check location - if (!check_latlng(packet.lat, packet.lon)) { - break; - } - - // set gps hil sensor - Location loc; - loc.lat = packet.lat; - loc.lng = packet.lon; - loc.alt = packet.alt/10; - Vector3f vel(packet.vx, packet.vy, packet.vz); - vel *= 0.01f; - - gps.setHIL(0, AP_GPS::GPS_OK_FIX_3D, - packet.time_usec/1000, - loc, vel, 10, 0); - - // rad/sec - Vector3f gyros; - gyros.x = packet.rollspeed; - gyros.y = packet.pitchspeed; - gyros.z = packet.yawspeed; - - // m/s/s - Vector3f accels; - accels.x = packet.xacc * (GRAVITY_MSS/1000.0f); - accels.y = packet.yacc * (GRAVITY_MSS/1000.0f); - accels.z = packet.zacc * (GRAVITY_MSS/1000.0f); - - ins.set_gyro(0, gyros); - - ins.set_accel(0, accels); - - AP::baro().setHIL(packet.alt*0.001f); - copter.compass.setHIL(0, packet.roll, packet.pitch, packet.yaw); - copter.compass.setHIL(1, packet.roll, packet.pitch, packet.yaw); - - break; - } -#endif // HIL_MODE != HIL_MODE_DISABLED - case MAVLINK_MSG_ID_RADIO: case MAVLINK_MSG_ID_RADIO_STATUS: // MAV ID: 109 { diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 00fb4dd330..53d4dde5a1 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -39,13 +39,6 @@ #error CONFIG_HAL_BOARD must be defined to build ArduCopter #endif -////////////////////////////////////////////////////////////////////////////// -// HIL_MODE OPTIONAL - -#ifndef HIL_MODE - #define HIL_MODE HIL_MODE_DISABLED -#endif - #ifndef ARMING_DELAY_SEC # define ARMING_DELAY_SEC 2.0f #endif diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 646cdb2da3..d7f6d43b13 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -27,10 +27,6 @@ enum autopilot_yaw_mode { #define MULTICOPTER_FRAME 1 #define HELI_FRAME 2 -// HIL enumerations -#define HIL_MODE_DISABLED 0 -#define HIL_MODE_SENSORS 1 - // Tuning enumeration enum tuning_func { TUNING_NONE = 0, // diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index 00f05bec7d..1e640e829b 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -142,18 +142,6 @@ void Copter::init_ardupilot() USERHOOK_INIT #endif -#if HIL_MODE != HIL_MODE_DISABLED - while (barometer.get_last_update() == 0) { - // the barometer begins updating when we get the first - // HIL_STATE message - gcs().send_text(MAV_SEVERITY_WARNING, "Waiting for first HIL_STATE message"); - delay(1000); - } - - // set INS to HIL mode - ins.set_hil_mode(); -#endif - // read Baro pressure at ground //----------------------------- barometer.set_log_baro_bit(MASK_LOG_IMU);