Skip to content

Commit

Permalink
Merge branch 'main' into feature/23316-duplicate-emptiness-check
Browse files Browse the repository at this point in the history
  • Loading branch information
@PavloZMN
PavloZMN authored Jan 31, 2025
2 parents a5407df + a9214b3 commit 0775359
Show file tree
Hide file tree
Showing 7 changed files with 84 additions and 77 deletions.
8 changes: 8 additions & 0 deletions src/modules/commander/HealthAndArmingChecks/Common.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
{
Expand Down
14 changes: 14 additions & 0 deletions src/modules/commander/HealthAndArmingChecks/Common.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<NavModes>(static_cast<uint32_t>(a) | static_cast<uint32_t>(b));
Expand Down Expand Up @@ -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);

/**
Expand Down
60 changes: 32 additions & 28 deletions src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -273,25 +273,29 @@ 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<GnssArmingCheck>(_param_com_arm_wo_gps.get())) {
default:

/* 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;
}
Expand All @@ -304,10 +308,10 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
/* EVENT
* @description
* <profile name="dev">
* This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
* Can be configured with <param>EKF2_GPS_CHECK</param> and <param>COM_ARM_WO_GPS</param>.
* </profile>
*/
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");

Expand All @@ -316,10 +320,10 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
/* EVENT
* @description
* <profile name="dev">
* This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
* Can be configured with <param>EKF2_GPS_CHECK</param> and <param>COM_ARM_WO_GPS</param>.
* </profile>
*/
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");

Expand All @@ -328,10 +332,10 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
/* EVENT
* @description
* <profile name="dev">
* This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
* Can be configured with <param>EKF2_GPS_CHECK</param> and <param>COM_ARM_WO_GPS</param>.
* </profile>
*/
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");

Expand All @@ -340,10 +344,10 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
/* EVENT
* @description
* <profile name="dev">
* This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
* Can be configured with <param>EKF2_GPS_CHECK</param> and <param>COM_ARM_WO_GPS</param>.
* </profile>
*/
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");

Expand All @@ -352,10 +356,10 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
/* EVENT
* @description
* <profile name="dev">
* This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
* Can be configured with <param>EKF2_GPS_CHECK</param> and <param>COM_ARM_WO_GPS</param>.
* </profile>
*/
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");

Expand All @@ -364,10 +368,10 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
/* EVENT
* @description
* <profile name="dev">
* This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
* Can be configured with <param>EKF2_GPS_CHECK</param> and <param>COM_ARM_WO_GPS</param>.
* </profile>
*/
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");

Expand All @@ -376,10 +380,10 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
/* EVENT
* @description
* <profile name="dev">
* This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
* Can be configured with <param>EKF2_GPS_CHECK</param> and <param>COM_ARM_WO_GPS</param>.
* </profile>
*/
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");

Expand All @@ -388,10 +392,10 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
/* EVENT
* @description
* <profile name="dev">
* This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
* Can be configured with <param>EKF2_GPS_CHECK</param> and <param>COM_ARM_WO_GPS</param>.
* </profile>
*/
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");

Expand All @@ -400,10 +404,10 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
/* EVENT
* @description
* <profile name="dev">
* This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
* Can be configured with <param>EKF2_GPS_CHECK</param> and <param>COM_ARM_WO_GPS</param>.
* </profile>
*/
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");

Expand All @@ -412,10 +416,10 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
/* EVENT
* @description
* <profile name="dev">
* This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
* Can be configured with <param>EKF2_GPS_CHECK</param> and <param>COM_ARM_WO_GPS</param>.
* </profile>
*/
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");

Expand All @@ -424,10 +428,10 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
/* EVENT
* @description
* <profile name="dev">
* This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
* Can be configured with <param>EKF2_GPS_CHECK</param> and <param>COM_ARM_WO_GPS</param>.
* </profile>
*/
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");

Expand All @@ -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");

Expand All @@ -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");
}
Expand Down
3 changes: 3 additions & 0 deletions src/modules/gimbal/output_mavlink.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}

Expand Down
7 changes: 1 addition & 6 deletions src/modules/mavlink/mavlink_receiver.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}
Expand Down
67 changes: 25 additions & 42 deletions src/modules/mavlink/streams/GIMBAL_DEVICE_ATTITUDE_STATUS.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
Expand Down
2 changes: 1 addition & 1 deletion src/modules/simulation/gz_bridge/GZGimbal.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down

0 comments on commit 0775359

Please sign in to comment.