diff --git a/src/modules/commander/HealthAndArmingChecks/Common.cpp b/src/modules/commander/HealthAndArmingChecks/Common.cpp index fb5639087ea1..16e074ba1893 100644 --- a/src/modules/commander/HealthAndArmingChecks/Common.cpp +++ b/src/modules/commander/HealthAndArmingChecks/Common.cpp @@ -73,6 +73,14 @@ void Report::armingCheckFailure(NavModes required_modes, HealthComponentIndex co addEvent(event_id, log_levels, message, (uint32_t)reportedModes(required_modes), component.index); } +void Report::armingCheckFailure(NavModesMessageFail required_modes, HealthComponentIndex component, + uint32_t event_id, const events::LogLevels &log_levels, const char *message) +{ + armingCheckFailure(required_modes.fail_modes, component, log_levels.external); + addEvent(event_id, log_levels, message, + (uint32_t)reportedModes(required_modes.message_modes | required_modes.fail_modes), component.index); +} + Report::EventBufferHeader *Report::addEventToBuffer(uint32_t event_id, const events::LogLevels &log_levels, uint32_t modes, unsigned args_size) { diff --git a/src/modules/commander/HealthAndArmingChecks/Common.hpp b/src/modules/commander/HealthAndArmingChecks/Common.hpp index 826aa7629c0e..e069788ddf93 100644 --- a/src/modules/commander/HealthAndArmingChecks/Common.hpp +++ b/src/modules/commander/HealthAndArmingChecks/Common.hpp @@ -74,6 +74,12 @@ static_assert(sizeof(navigation_mode_group_t) == sizeof(NavModes), "type mismatc static_assert(vehicle_status_s::NAVIGATION_STATE_MAX <= CHAR_BIT *sizeof(navigation_mode_group_t), "type too small, use next larger type"); +// Type to pass two mode groups in one struct to have the same number of function arguments to facilitate events parsing +struct NavModesMessageFail { + NavModes message_modes; ///< modes in which there's user messageing but arming is allowed + NavModes fail_modes; ///< modes in which checks fail which must be a subset of message_modes +}; + static inline NavModes operator|(NavModes a, NavModes b) { return static_cast(static_cast(a) | static_cast(b)); @@ -251,6 +257,14 @@ class Report void armingCheckFailure(NavModes required_modes, HealthComponentIndex component, uint32_t event_id, const events::LogLevels &log_levels, const char *message); + /** + * Overloaded variant of armingCheckFailure() which allows to separately specify modes in which a message should be emitted and a subset in which arming is blocked + * @param required_modes .message_modes modes in which to put out the event and hence user message. + * .failing_modes modes in which to to fail arming. Has to be a subset of message_modes to never disallow arming without a reason. + */ + void armingCheckFailure(NavModesMessageFail required_modes, HealthComponentIndex component, + uint32_t event_id, const events::LogLevels &log_levels, const char *message); + void clearArmingBits(NavModes modes); /** diff --git a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp index 1cd140fe5bae..96b32c947c47 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp @@ -273,7 +273,7 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor } if (!context.isArmed() && ekf_gps_check_fail) { - NavModes required_groups_gps; + NavModesMessageFail required_modes; events::Log log_level; switch (static_cast(_param_com_arm_wo_gps.get())) { @@ -281,17 +281,21 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor /* FALLTHROUGH */ case GnssArmingCheck::DenyArming: - required_groups_gps = required_groups; + required_modes.message_modes = required_modes.fail_modes = NavModes::All; log_level = events::Log::Error; break; case GnssArmingCheck::WarningOnly: - required_groups_gps = NavModes::None; // optional + required_modes.message_modes = (NavModes)(reporter.failsafeFlags().mode_req_local_position + | reporter.failsafeFlags().mode_req_local_position_relaxed + | reporter.failsafeFlags().mode_req_global_position); + // Only warn and don't block arming because there could still be a valid position estimate from another source e.g. optical flow, VIO + required_modes.fail_modes = NavModes::None; log_level = events::Log::Warning; break; case GnssArmingCheck::Disabled: - required_groups_gps = NavModes::None; + required_modes.message_modes = required_modes.fail_modes = NavModes::None; log_level = events::Log::Disabled; break; } @@ -304,10 +308,10 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor /* EVENT * @description * - * This check can be configured via EKF2_GPS_CHECK parameter. + * Can be configured with EKF2_GPS_CHECK and COM_ARM_WO_GPS. * */ - reporter.armingCheckFailure(required_groups_gps, health_component_t::gps, + reporter.armingCheckFailure(required_modes, health_component_t::gps, events::ID("check_estimator_gps_fix_too_low"), log_level, "GPS fix too low"); @@ -316,10 +320,10 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor /* EVENT * @description * - * This check can be configured via EKF2_GPS_CHECK parameter. + * Can be configured with EKF2_GPS_CHECK and COM_ARM_WO_GPS. * */ - reporter.armingCheckFailure(required_groups_gps, health_component_t::gps, + reporter.armingCheckFailure(required_modes, health_component_t::gps, events::ID("check_estimator_gps_num_sats_too_low"), log_level, "Not enough GPS Satellites"); @@ -328,10 +332,10 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor /* EVENT * @description * - * This check can be configured via EKF2_GPS_CHECK parameter. + * Can be configured with EKF2_GPS_CHECK and COM_ARM_WO_GPS. * */ - reporter.armingCheckFailure(required_groups_gps, health_component_t::gps, + reporter.armingCheckFailure(required_modes, health_component_t::gps, events::ID("check_estimator_gps_pdop_too_high"), log_level, "GPS PDOP too high"); @@ -340,10 +344,10 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor /* EVENT * @description * - * This check can be configured via EKF2_GPS_CHECK parameter. + * Can be configured with EKF2_GPS_CHECK and COM_ARM_WO_GPS. * */ - reporter.armingCheckFailure(required_groups_gps, health_component_t::gps, + reporter.armingCheckFailure(required_modes, health_component_t::gps, events::ID("check_estimator_gps_hor_pos_err_too_high"), log_level, "GPS Horizontal Position Error too high"); @@ -352,10 +356,10 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor /* EVENT * @description * - * This check can be configured via EKF2_GPS_CHECK parameter. + * Can be configured with EKF2_GPS_CHECK and COM_ARM_WO_GPS. * */ - reporter.armingCheckFailure(required_groups_gps, health_component_t::gps, + reporter.armingCheckFailure(required_modes, health_component_t::gps, events::ID("check_estimator_gps_vert_pos_err_too_high"), log_level, "GPS Vertical Position Error too high"); @@ -364,10 +368,10 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor /* EVENT * @description * - * This check can be configured via EKF2_GPS_CHECK parameter. + * Can be configured with EKF2_GPS_CHECK and COM_ARM_WO_GPS. * */ - reporter.armingCheckFailure(required_groups_gps, health_component_t::gps, + reporter.armingCheckFailure(required_modes, health_component_t::gps, events::ID("check_estimator_gps_speed_acc_too_low"), log_level, "GPS Speed Accuracy too low"); @@ -376,10 +380,10 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor /* EVENT * @description * - * This check can be configured via EKF2_GPS_CHECK parameter. + * Can be configured with EKF2_GPS_CHECK and COM_ARM_WO_GPS. * */ - reporter.armingCheckFailure(required_groups_gps, health_component_t::gps, + reporter.armingCheckFailure(required_modes, health_component_t::gps, events::ID("check_estimator_gps_hor_pos_drift_too_high"), log_level, "GPS Horizontal Position Drift too high"); @@ -388,10 +392,10 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor /* EVENT * @description * - * This check can be configured via EKF2_GPS_CHECK parameter. + * Can be configured with EKF2_GPS_CHECK and COM_ARM_WO_GPS. * */ - reporter.armingCheckFailure(required_groups_gps, health_component_t::gps, + reporter.armingCheckFailure(required_modes, health_component_t::gps, events::ID("check_estimator_gps_vert_pos_drift_too_high"), log_level, "GPS Vertical Position Drift too high"); @@ -400,10 +404,10 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor /* EVENT * @description * - * This check can be configured via EKF2_GPS_CHECK parameter. + * Can be configured with EKF2_GPS_CHECK and COM_ARM_WO_GPS. * */ - reporter.armingCheckFailure(required_groups_gps, health_component_t::gps, + reporter.armingCheckFailure(required_modes, health_component_t::gps, events::ID("check_estimator_gps_hor_speed_drift_too_high"), log_level, "GPS Horizontal Speed Drift too high"); @@ -412,10 +416,10 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor /* EVENT * @description * - * This check can be configured via EKF2_GPS_CHECK parameter. + * Can be configured with EKF2_GPS_CHECK and COM_ARM_WO_GPS. * */ - reporter.armingCheckFailure(required_groups_gps, health_component_t::gps, + reporter.armingCheckFailure(required_modes, health_component_t::gps, events::ID("check_estimator_gps_vert_speed_drift_too_high"), log_level, "GPS Vertical Speed Drift too high"); @@ -424,10 +428,10 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor /* EVENT * @description * - * This check can be configured via EKF2_GPS_CHECK parameter. + * Can be configured with EKF2_GPS_CHECK and COM_ARM_WO_GPS. * */ - reporter.armingCheckFailure(required_groups_gps, health_component_t::gps, + reporter.armingCheckFailure(required_modes, health_component_t::gps, events::ID("check_estimator_gps_spoofed"), log_level, "GPS signal spoofed"); @@ -437,7 +441,7 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor message = "Preflight%s: Estimator not using GPS"; /* EVENT */ - reporter.armingCheckFailure(required_groups_gps, health_component_t::gps, + reporter.armingCheckFailure(required_modes, health_component_t::gps, events::ID("check_estimator_gps_not_fusing"), log_level, "Estimator not using GPS"); @@ -446,7 +450,7 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor message = "Preflight%s: Poor GPS Quality"; /* EVENT */ - reporter.armingCheckFailure(required_groups_gps, health_component_t::gps, + reporter.armingCheckFailure(required_modes, health_component_t::gps, events::ID("check_estimator_gps_generic"), log_level, "Poor GPS Quality"); } diff --git a/src/modules/gimbal/output_mavlink.cpp b/src/modules/gimbal/output_mavlink.cpp index ca562ec87047..ae5222c674de 100644 --- a/src/modules/gimbal/output_mavlink.cpp +++ b/src/modules/gimbal/output_mavlink.cpp @@ -109,6 +109,9 @@ void OutputMavlinkV1::update(const ControlData &control_data, bool new_setpoints _stream_device_attitude_status(); + // If the output is MAVLink v1, then we signal this by referring to compid 1. + gimbal_device_id = 1; + _last_update = now; } diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 37328503b9d7..aa8b50d01aaf 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -3075,12 +3075,7 @@ MavlinkReceiver::handle_message_gimbal_device_information(mavlink_message_t *msg gimbal_information.yaw_max = gimbal_device_info_msg.yaw_max; gimbal_information.yaw_min = gimbal_device_info_msg.yaw_min; - if (gimbal_device_info_msg.gimbal_device_id == 0) { - gimbal_information.gimbal_device_id = msg->compid; - - } else { - gimbal_information.gimbal_device_id = gimbal_device_info_msg.gimbal_device_id; - } + gimbal_information.gimbal_device_id = msg->compid; _gimbal_device_information_pub.publish(gimbal_information); } diff --git a/src/modules/mavlink/streams/GIMBAL_DEVICE_ATTITUDE_STATUS.hpp b/src/modules/mavlink/streams/GIMBAL_DEVICE_ATTITUDE_STATUS.hpp index c8338996ae67..ae7f3cbad14e 100644 --- a/src/modules/mavlink/streams/GIMBAL_DEVICE_ATTITUDE_STATUS.hpp +++ b/src/modules/mavlink/streams/GIMBAL_DEVICE_ATTITUDE_STATUS.hpp @@ -74,48 +74,31 @@ class MavlinkStreamGimbalDeviceAttitudeStatus : public MavlinkStream return false; } - if (gimbal_device_attitude_status.gimbal_device_id >= 1 && gimbal_device_attitude_status.gimbal_device_id <= 6) { - // A non-MAVLink gimbal is signalled and addressed using 1 to 6 as the gimbal_device_id - mavlink_gimbal_device_attitude_status_t msg{}; - - msg.target_system = gimbal_device_attitude_status.target_system; - msg.target_component = gimbal_device_attitude_status.target_component; - - msg.time_boot_ms = gimbal_device_attitude_status.timestamp / 1000; - - msg.flags = gimbal_device_attitude_status.device_flags; - - msg.q[0] = gimbal_device_attitude_status.q[0]; - msg.q[1] = gimbal_device_attitude_status.q[1]; - msg.q[2] = gimbal_device_attitude_status.q[2]; - msg.q[3] = gimbal_device_attitude_status.q[3]; - - msg.angular_velocity_x = gimbal_device_attitude_status.angular_velocity_x; - msg.angular_velocity_y = gimbal_device_attitude_status.angular_velocity_y; - msg.angular_velocity_z = gimbal_device_attitude_status.angular_velocity_z; - - msg.failure_flags = gimbal_device_attitude_status.failure_flags; - msg.gimbal_device_id = gimbal_device_attitude_status.gimbal_device_id; - - msg.delta_yaw = gimbal_device_attitude_status.delta_yaw; - msg.delta_yaw_velocity = gimbal_device_attitude_status.delta_yaw_velocity; - - mavlink_msg_gimbal_device_attitude_status_send_struct(_mavlink->get_channel(), &msg); - - } else { - // We have a Mavlink gimbal. We simulate its mavlink instance by spoofing the component ID - mavlink_message_t message; - mavlink_msg_gimbal_device_attitude_status_pack_chan(_mavlink->get_system_id(), MAV_COMP_ID_GIMBAL, - _mavlink->get_channel(), &message, - gimbal_device_attitude_status.target_system, gimbal_device_attitude_status.target_component, - gimbal_device_attitude_status.timestamp / 1000, - gimbal_device_attitude_status.device_flags, gimbal_device_attitude_status.q, - gimbal_device_attitude_status.angular_velocity_x, - gimbal_device_attitude_status.angular_velocity_y, gimbal_device_attitude_status.angular_velocity_z, - gimbal_device_attitude_status.failure_flags, - 0, 0, 0); - _mavlink->forward_message(&message, _mavlink); - } + mavlink_gimbal_device_attitude_status_t msg{}; + + msg.target_system = gimbal_device_attitude_status.target_system; + msg.target_component = gimbal_device_attitude_status.target_component; + + msg.time_boot_ms = gimbal_device_attitude_status.timestamp / 1000; + + msg.flags = gimbal_device_attitude_status.device_flags; + + msg.q[0] = gimbal_device_attitude_status.q[0]; + msg.q[1] = gimbal_device_attitude_status.q[1]; + msg.q[2] = gimbal_device_attitude_status.q[2]; + msg.q[3] = gimbal_device_attitude_status.q[3]; + + msg.angular_velocity_x = gimbal_device_attitude_status.angular_velocity_x; + msg.angular_velocity_y = gimbal_device_attitude_status.angular_velocity_y; + msg.angular_velocity_z = gimbal_device_attitude_status.angular_velocity_z; + + msg.failure_flags = gimbal_device_attitude_status.failure_flags; + msg.gimbal_device_id = gimbal_device_attitude_status.gimbal_device_id; + + msg.delta_yaw = gimbal_device_attitude_status.delta_yaw; + msg.delta_yaw_velocity = gimbal_device_attitude_status.delta_yaw_velocity; + + mavlink_msg_gimbal_device_attitude_status_send_struct(_mavlink->get_channel(), &msg); return true; } diff --git a/src/modules/simulation/gz_bridge/GZGimbal.hpp b/src/modules/simulation/gz_bridge/GZGimbal.hpp index 00b46becc48f..662268abd2c3 100644 --- a/src/modules/simulation/gz_bridge/GZGimbal.hpp +++ b/src/modules/simulation/gz_bridge/GZGimbal.hpp @@ -115,7 +115,7 @@ class GZGimbal : public px4::ScheduledWorkItem, public ModuleParams const float _yaw_min = NAN; // infinite yaw const float _yaw_max = NAN; // infinite yaw - const uint8_t _gimbal_device_id = 154; // TODO the implementation differs from the protocol + const uint8_t _gimbal_device_id = 1; // Gimbal is implemented by the same component: options are 1..6 uint16_t _gimbal_device_flags = 0; // GIMBAL_DEVICE_FLAGS bool init(const std::string &world_name, const std::string &model_name);