diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 43b3798e5a..488bee5c6b 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -2,21 +2,14 @@ #include "Plane.h" -void Plane::send_heartbeat(mavlink_channel_t chan) +MAV_TYPE GCS_MAVLINK_Plane::frame_type() const +{ + return plane.quadplane.get_mav_type(); +} + +MAV_MODE GCS_MAVLINK_Plane::base_mode() const { uint8_t base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; - uint8_t system_status; - uint32_t custom_mode = control_mode; - - if (failsafe.state != FAILSAFE_NONE || battery.has_failsafed() || failsafe.adsb) { - system_status = MAV_STATE_CRITICAL; - } else if (plane.crash_state.is_crashed) { - system_status = MAV_STATE_EMERGENCY; - } else if (is_flying()) { - system_status = MAV_STATE_ACTIVE; - } else { - system_status = MAV_STATE_STANDBY; - } // work out the base_mode. This value is not very useful // for APM, but we calculate it as best we can so a generic @@ -26,7 +19,7 @@ void Plane::send_heartbeat(mavlink_channel_t chan) // only get useful information from the custom_mode, which maps to // the APM flight mode and has a well defined meaning in the // ArduPlane documentation - switch (control_mode) { + switch (plane.control_mode) { case MANUAL: case TRAINING: case ACRO: @@ -57,45 +50,65 @@ void Plane::send_heartbeat(mavlink_channel_t chan) // positions", which APM does not currently do break; case INITIALISING: - system_status = MAV_STATE_CALIBRATING; break; } - if (!training_manual_pitch || !training_manual_roll) { + if (!plane.training_manual_pitch || !plane.training_manual_roll) { base_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED; } - if (control_mode != MANUAL && control_mode != INITIALISING) { + if (plane.control_mode != MANUAL && plane.control_mode != INITIALISING) { // stabiliser of some form is enabled base_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED; } - if (g.stick_mixing != STICK_MIXING_DISABLED && control_mode != INITIALISING) { + if (plane.g.stick_mixing != STICK_MIXING_DISABLED && plane.control_mode != INITIALISING) { // all modes except INITIALISING have some form of manual // override if stick mixing is enabled base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; } #if HIL_SUPPORT - if (g.hil_mode == 1) { + if (plane.g.hil_mode == 1) { base_mode |= MAV_MODE_FLAG_HIL_ENABLED; } #endif // we are armed if we are not initialising - if (control_mode != INITIALISING && arming.is_armed()) { + if (plane.control_mode != INITIALISING && plane.arming.is_armed()) { base_mode |= MAV_MODE_FLAG_SAFETY_ARMED; } // indicate we have set a custom mode base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; - gcs().chan(chan-MAVLINK_COMM_0).send_heartbeat(quadplane.get_mav_type(), - base_mode, - custom_mode, - system_status); + return (MAV_MODE)base_mode; } +uint32_t GCS_MAVLINK_Plane::custom_mode() const +{ + return plane.control_mode; +} + +MAV_STATE GCS_MAVLINK_Plane::system_status() const +{ + if (plane.control_mode == INITIALISING) { + return MAV_STATE_CALIBRATING; + } + if (plane.any_failsafe_triggered()) { + return MAV_STATE_CRITICAL; + } + if (plane.crash_state.is_crashed) { + return MAV_STATE_EMERGENCY; + } + if (plane.is_flying()) { + return MAV_STATE_ACTIVE; + } + + return MAV_STATE_STANDBY; +} + + void Plane::send_attitude(mavlink_channel_t chan) { float r = ahrs.roll; @@ -404,7 +417,7 @@ bool GCS_MAVLINK_Plane::try_send_message(enum ap_message id) case MSG_HEARTBEAT: CHECK_PAYLOAD_SIZE(HEARTBEAT); last_heartbeat_time = AP_HAL::millis(); - plane.send_heartbeat(chan); + send_heartbeat(); return true; case MSG_EXTENDED_STATUS1: diff --git a/ArduPlane/GCS_Mavlink.h b/ArduPlane/GCS_Mavlink.h index 2f05769bf5..8504897656 100644 --- a/ArduPlane/GCS_Mavlink.h +++ b/ArduPlane/GCS_Mavlink.h @@ -44,4 +44,8 @@ private: bool try_send_message(enum ap_message id) override; void packetReceived(const mavlink_status_t &status, mavlink_message_t &msg) override; + MAV_TYPE frame_type() const override; + MAV_MODE base_mode() const override; + uint32_t custom_mode() const override; + MAV_STATE system_status() const override; }; diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 5e2c51447d..ad5ce37a24 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -355,6 +355,10 @@ private: uint32_t AFS_last_valid_rc_ms; } failsafe; + bool any_failsafe_triggered() { + return failsafe.state != FAILSAFE_NONE || battery.has_failsafed() || failsafe.adsb; + } + // A counter used to count down valid gps fixes to allow the gps estimate to settle // before recording our home position (and executing a ground start if we booted with an air start) uint8_t ground_start_count = 5; @@ -776,7 +780,6 @@ private: void adjust_nav_pitch_throttle(void); void update_load_factor(void); - void send_heartbeat(mavlink_channel_t chan); void send_attitude(mavlink_channel_t chan); void send_fence_status(mavlink_channel_t chan); void update_sensor_status_flags(void); diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 702b52f036..50519a1cca 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -2483,12 +2483,12 @@ bool QuadPlane::do_user_takeoff(float takeoff_altitude) /* return mav_type for heartbeat */ -uint8_t QuadPlane::get_mav_type(void) const +MAV_TYPE QuadPlane::get_mav_type(void) const { if (mav_type.get() == 0) { return MAV_TYPE_FIXED_WING; } - return uint8_t(mav_type.get()); + return MAV_TYPE(mav_type.get()); } /* diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 626786dca8..f2920a61aa 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -268,7 +268,7 @@ private: // HEARTBEAT mav_type override AP_Int8 mav_type; - uint8_t get_mav_type(void) const; + MAV_TYPE get_mav_type(void) const; // time we last got an EKF yaw reset uint32_t ekfYawReset_ms;