diff --git a/Rover/GCS_Mavlink.cpp b/Rover/GCS_Mavlink.cpp index f6b8b14d44..9684197853 100644 --- a/Rover/GCS_Mavlink.cpp +++ b/Rover/GCS_Mavlink.cpp @@ -38,10 +38,6 @@ MAV_MODE GCS_MAVLINK_Rover::base_mode() const _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 (rover.control_mode != &rover.mode_initializing && rover.arming.is_armed()) { _base_mode |= MAV_MODE_FLAG_SAFETY_ARMED; @@ -538,14 +534,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_Rover::in_hil_mode() const -{ -#if HIL_MODE != HIL_MODE_DISABLED - return rover.g.hil_mode == 1; -#endif - return false; -} - bool GCS_MAVLINK_Rover::handle_guided_request(AP_Mission::Mission_Command &cmd) { if (!rover.control_mode->in_guided_mode()) { @@ -755,12 +743,6 @@ void GCS_MAVLINK_Rover::handleMessage(const mavlink_message_t &msg) handle_set_position_target_global_int(msg); break; -#if HIL_MODE != HIL_MODE_DISABLED - case MAVLINK_MSG_ID_HIL_STATE: - handle_hil_state(msg); - break; -#endif - case MAVLINK_MSG_ID_RADIO: case MAVLINK_MSG_ID_RADIO_STATUS: handle_radio(msg); @@ -1056,49 +1038,6 @@ void GCS_MAVLINK_Rover::handle_set_position_target_global_int(const mavlink_mess } } -#if HIL_MODE != HIL_MODE_DISABLED -void GCS_MAVLINK_Rover::handle_hil_state(const mavlink_message_t &msg) -{ - mavlink_hil_state_t packet; - mavlink_msg_hil_state_decode(&msg, &packet); - - // sanity check location - if (!check_latlng(packet.lat, packet.lon)) { - return; - } - - // 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); - compass.setHIL(0, packet.roll, packet.pitch, packet.yaw); - compass.setHIL(1, packet.roll, packet.pitch, packet.yaw); -} -#endif // HIL_MODE - void GCS_MAVLINK_Rover::handle_radio(const mavlink_message_t &msg) { handle_radio_status(msg, rover.should_log(MASK_LOG_PM)); diff --git a/Rover/GCS_Mavlink.h b/Rover/GCS_Mavlink.h index 3a8da8be48..3ede3bf492 100644 --- a/Rover/GCS_Mavlink.h +++ b/Rover/GCS_Mavlink.h @@ -22,8 +22,6 @@ protected: void send_position_target_global_int() override; - virtual bool in_hil_mode() const override; - bool persist_streamrates() const override { return true; } bool set_home_to_current_location(bool lock) override; @@ -44,7 +42,6 @@ private: void handle_set_attitude_target(const mavlink_message_t &msg); void handle_set_position_target_local_ned(const mavlink_message_t &msg); void handle_set_position_target_global_int(const mavlink_message_t &msg); - void handle_hil_state(const mavlink_message_t &msg); void handle_radio(const mavlink_message_t &msg); void packetReceived(const mavlink_status_t &status, const mavlink_message_t &msg) override; diff --git a/Rover/Rover.cpp b/Rover/Rover.cpp index 80809d5fce..708464447a 100644 --- a/Rover/Rover.cpp +++ b/Rover/Rover.cpp @@ -231,11 +231,6 @@ void Rover::ahrs_update() { arming.update_soft_armed(); -#if HIL_MODE != HIL_MODE_DISABLED - // update hil before AHRS update - gcs().update(); -#endif - // AHRS may use movement to calculate heading update_ahrs_flyforward(); diff --git a/Rover/config.h b/Rover/config.h index 3bf53ce917..1326006d70 100644 --- a/Rover/config.h +++ b/Rover/config.h @@ -2,16 +2,6 @@ #include "defines.h" -////////////////////////////////////////////////////////////////////////////// -// sensor types - -////////////////////////////////////////////////////////////////////////////// -// HIL_MODE OPTIONAL - -#ifndef HIL_MODE - #define HIL_MODE HIL_MODE_DISABLED -#endif - #ifndef MAV_SYSTEM_ID #define MAV_SYSTEM_ID 1 #endif diff --git a/Rover/defines.h b/Rover/defines.h index 2c53628321..49f5a5034a 100644 --- a/Rover/defines.h +++ b/Rover/defines.h @@ -13,10 +13,6 @@ #define SERVO_MAX 4500 // This value represents 45 degrees and is just an arbitrary representation of servo max travel. -// HIL enumerations -#define HIL_MODE_DISABLED 0 -#define HIL_MODE_SENSORS 1 - // types of failsafe events #define FAILSAFE_EVENT_THROTTLE (1<<0) #define FAILSAFE_EVENT_GCS (1<<1)