diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 4068a7b023..2dd6998e40 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -133,13 +133,6 @@ void Plane::ahrs_update() { arming.update_soft_armed(); -#if HIL_SUPPORT - if (g.hil_mode == 1) { - // update hil before AHRS update - gcs().update_receive(); - } -#endif - ahrs.update(); if (should_log(MASK_LOG_IMU)) { diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 16a2c3d6f3..655d8e5d23 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -74,12 +74,6 @@ MAV_MODE GCS_MAVLINK_Plane::base_mode() const } } -#if HIL_SUPPORT - if (plane.g.hil_mode == 1) { - _base_mode |= MAV_MODE_FLAG_HIL_ENABLED; - } -#endif - // we are armed if we are not initialising if (plane.control_mode != &plane.mode_initializing && plane.arming.is_armed()) { _base_mode |= MAV_MODE_FLAG_SAFETY_ARMED; @@ -216,26 +210,6 @@ void GCS_MAVLINK_Plane::send_position_target_global_int() } -void Plane::send_servo_out(mavlink_channel_t chan) -{ - // normalized values scaled to -10000 to 10000 - // This is used for HIL. Do not change without discussing with - // HIL maintainers - mavlink_msg_rc_channels_scaled_send( - chan, - millis(), - 0, // port 0 - 10000 * (SRV_Channels::get_output_scaled(SRV_Channel::k_aileron) / 4500.0f), - 10000 * (SRV_Channels::get_output_scaled(SRV_Channel::k_elevator) / 4500.0f), - 10000 * (SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) / 100.0f), - 10000 * (SRV_Channels::get_output_scaled(SRV_Channel::k_rudder) / 4500.0f), - 0, - 0, - 0, - 0, - rssi.read_receiver_rssi_uint8()); -} - float GCS_MAVLINK_Plane::vfr_hud_airspeed() const { // airspeed sensors are best. While the AHRS airspeed_estimate @@ -271,33 +245,11 @@ float GCS_MAVLINK_Plane::vfr_hud_climbrate() const return AP::baro().get_climb_rate(); } -/* - keep last HIL_STATE message to allow sending SIM_STATE - */ -#if HIL_SUPPORT -static mavlink_hil_state_t last_hil_state; -#endif - // report simulator state void GCS_MAVLINK_Plane::send_simstate() const { #if CONFIG_HAL_BOARD == HAL_BOARD_SITL GCS_MAVLINK::send_simstate(); -#elif HIL_SUPPORT - if (plane.g.hil_mode == 1) { - mavlink_msg_simstate_send(chan, - last_hil_state.roll, - last_hil_state.pitch, - last_hil_state.yaw, - last_hil_state.xacc*0.001f*GRAVITY_MSS, - last_hil_state.yacc*0.001f*GRAVITY_MSS, - last_hil_state.zacc*0.001f*GRAVITY_MSS, - last_hil_state.rollspeed, - last_hil_state.pitchspeed, - last_hil_state.yawspeed, - last_hil_state.lat, - last_hil_state.lon); - } #endif } @@ -403,12 +355,7 @@ bool GCS_MAVLINK_Plane::try_send_message(enum ap_message id) switch (id) { case MSG_SERVO_OUT: -#if HIL_SUPPORT - if (plane.g.hil_mode == 1) { - CHECK_PAYLOAD_SIZE(RC_CHANNELS_SCALED); - plane.send_servo_out(chan); - } -#endif + // unused break; case MSG_TERRAIN: @@ -629,14 +576,6 @@ const struct GCS_MAVLINK::stream_entries GCS_MAVLINK::all_stream_entries[] = { MAV_STREAM_TERMINATOR // must have this at end of stream_entries }; -bool GCS_MAVLINK_Plane::in_hil_mode() const -{ -#if HIL_SUPPORT - return plane.g.hil_mode == 1; -#endif - return false; -} - /* handle a request to switch to guided mode. This happens via a callback from handle_mission_item() @@ -1162,65 +1101,6 @@ void GCS_MAVLINK_Plane::handleMessage(const mavlink_message_t &msg) break; } - case MAVLINK_MSG_ID_HIL_STATE: - { -#if HIL_SUPPORT - if (plane.g.hil_mode != 1) { - break; - } - - mavlink_hil_state_t packet; - mavlink_msg_hil_state_decode(&msg, &packet); - - // sanity check location - if (!check_latlng(packet.lat, packet.lon)) { - break; - } - - last_hil_state = packet; - - // set gps hil sensor - const Location loc{packet.lat, packet.lon, packet.alt/10, Location::AltFrame::ABSOLUTE}; - Vector3f vel(packet.vx, packet.vy, packet.vz); - vel *= 0.01f; - - // setup airspeed pressure based on 3D speed, no wind - plane.airspeed.setHIL(sq(vel.length()) / 2.0f + 2013); - - plane.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*0.001f; - accels.y = packet.yacc * GRAVITY_MSS*0.001f; - accels.z = packet.zacc * GRAVITY_MSS*0.001f; - - plane.ins.set_gyro(0, gyros); - plane.ins.set_accel(0, accels); - - plane.barometer.setHIL(packet.alt*0.001f); - plane.compass.setHIL(0, packet.roll, packet.pitch, packet.yaw); - plane.compass.setHIL(1, packet.roll, packet.pitch, packet.yaw); - - // cope with DCM getting badly off due to HIL lag - if (plane.g.hil_err_limit > 0 && - (fabsf(packet.roll - plane.ahrs.roll) > ToRad(plane.g.hil_err_limit) || - fabsf(packet.pitch - plane.ahrs.pitch) > ToRad(plane.g.hil_err_limit) || - wrap_PI(fabsf(packet.yaw - plane.ahrs.yaw)) > ToRad(plane.g.hil_err_limit))) { - plane.ahrs.reset_attitude(packet.roll, packet.pitch, packet.yaw); - } -#endif - break; - } - case MAVLINK_MSG_ID_RADIO: case MAVLINK_MSG_ID_RADIO_STATUS: { diff --git a/ArduPlane/GCS_Mavlink.h b/ArduPlane/GCS_Mavlink.h index 7b3e93d084..671f3d4a09 100644 --- a/ArduPlane/GCS_Mavlink.h +++ b/ArduPlane/GCS_Mavlink.h @@ -27,8 +27,6 @@ protected: void send_position_target_global_int() override; - virtual bool in_hil_mode() const override; - void send_aoa_ssa(); void send_attitude() const override; void send_simstate() const override; diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index 3ad1c9d363..0a66ff17c4 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -740,32 +740,6 @@ const AP_Param::Info Plane::var_info[] = { GSCALAR(override_safety, "OVERRIDE_SAFETY", 1), #endif -#if HIL_SUPPORT - // @Param: HIL_MODE - // @DisplayName: HIL mode enable - // @Description: This enables and disables hardware in the loop mode. If HIL_MODE is 1 then on the next reboot all sensors are replaced with HIL sensors which come from the GCS. - // @Values: 0:Disabled,1:Enabled - // @User: Advanced - // @RebootRequired: True - GSCALAR(hil_mode, "HIL_MODE", 0), -#endif - - // @Param: HIL_SERVOS - // @DisplayName: HIL Servos enable - // @Description: This controls whether real servo controls are used in HIL mode. If you enable this then the APM will control the real servos in HIL mode. If disabled it will report servo values, but will not output to the real servos. Be careful that your motor and propeller are not connected if you enable this option. - // @Values: 0:Disabled,1:Enabled - // @User: Advanced - GSCALAR(hil_servos, "HIL_SERVOS", 0), - - // @Param: HIL_ERR_LIMIT - // @DisplayName: Limit of error in HIL attitude before reset - // @Description: This controls the maximum error in degrees on any axis before HIL will reset the DCM attitude to match the HIL_STATE attitude. This limit will prevent poor timing on HIL from causing a major attitude error. If the value is zero then no limit applies. - // @Units: deg - // @Range: 0 90 - // @Increment: 0.1 - // @User: Advanced - GSCALAR(hil_err_limit, "HIL_ERR_LIMIT", 5), - // @Param: RTL_AUTOLAND // @DisplayName: RTL auto land // @Description: Automatically begin landing sequence after arriving at RTL location. This requires the addition of a DO_LAND_START mission item, which acts as a marker for the start of a landing sequence. The closest landing sequence will be chosen to the current location. diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index a41c66cfa7..c5684734a5 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -79,7 +79,7 @@ public: k_param_takeoff_throttle_min_accel, k_param_takeoff_heading_hold, // unused k_param_level_roll_limit, - k_param_hil_servos, + k_param_hil_servos_unused, // unused k_param_vtail_output, // unused k_param_nav_controller, k_param_elevon_output, // unused @@ -94,7 +94,7 @@ public: k_param_ground_steer_alt, k_param_ground_steer_dps, k_param_rally_limit_km_old, //unused anymore -- just holding this index - k_param_hil_err_limit, + k_param_hil_err_limit_unused, // unused k_param_sonar_old, // unused k_param_log_bitmask, k_param_BoardConfig, @@ -130,7 +130,7 @@ public: k_param_optflow, k_param_cli_enabled_old, // unused - CLI removed k_param_trim_rc_at_start, // unused - k_param_hil_mode, + k_param_hil_mode_unused, // unused k_param_land_disarm_delay, // unused - moved to AP_Landing k_param_glide_slope_threshold, k_param_rudder_only, @@ -361,8 +361,6 @@ public: AP_Int16 sysid_my_gcs; AP_Int8 telem_delay; - AP_Float hil_err_limit; - AP_Int8 rtl_autoland; AP_Int8 crash_accel_threshold; @@ -449,10 +447,6 @@ public: AP_Int32 RTL_altitude_cm; AP_Int16 pitch_trim_cd; AP_Int16 FBWB_min_altitude_cm; - AP_Int8 hil_servos; -#if HIL_SUPPORT - AP_Int8 hil_mode; -#endif AP_Int8 flap_1_percent; AP_Int8 flap_1_speed; diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index abb8fba3a5..c652f38033 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -854,9 +854,6 @@ private: void calc_nav_yaw_course(void); void calc_nav_yaw_ground(void); - // GCS_Mavlink.cpp - void send_servo_out(mavlink_channel_t chan); - // Log.cpp void Log_Write_Fast(void); void Log_Write_Attitude(void); diff --git a/ArduPlane/config.h b/ArduPlane/config.h index 5e43e36276..089f287f20 100644 --- a/ArduPlane/config.h +++ b/ArduPlane/config.h @@ -257,10 +257,6 @@ # define FENCE_TRIGGERED_PIN -1 #endif -#ifndef HIL_SUPPORT -# define HIL_SUPPORT !HAL_MINIMIZE_FEATURES -#endif - ////////////////////////////////////////////////////////////////////////////// // Parachute release #ifndef PARACHUTE diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index 5e2b808998..0c1de29ef0 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -869,19 +869,6 @@ void Plane::set_servos(void) } } -#if HIL_SUPPORT - if (g.hil_mode == 1) { - // get the servos to the GCS immediately for HIL - if (HAVE_PAYLOAD_SPACE(MAVLINK_COMM_0, RC_CHANNELS_SCALED)) { - send_servo_out(MAVLINK_COMM_0); - } - if (!g.hil_servos) { - // we don't run the output mixer - return; - } - } -#endif - if (landing.get_then_servos_neutral() > 0 && control_mode == &mode_auto && landing.get_disarm_delay() > 0 && diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index a585912763..1585004193 100644 --- a/ArduPlane/system.cpp +++ b/ArduPlane/system.cpp @@ -20,15 +20,6 @@ void Plane::init_ardupilot() g2.stats.init(); #endif -#if HIL_SUPPORT - if (g.hil_mode == 1) { - // set sensors to HIL mode - ins.set_hil_mode(); - compass.set_hil_mode(); - barometer.set_hil_mode(); - } -#endif - ins.set_log_raw_bit(MASK_LOG_IMU_RAW); // setup any board specific drivers @@ -366,17 +357,6 @@ void Plane::check_short_failsafe() void Plane::startup_INS_ground(void) { -#if HIL_SUPPORT - if (g.hil_mode == 1) { - 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"); - hal.scheduler->delay(1000); - } - } -#endif - if (ins.gyro_calibration_timing() != AP_InertialSensor::GYRO_CAL_NEVER) { gcs().send_text(MAV_SEVERITY_ALERT, "Beginning INS calibration. Do not move plane"); } else {