fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-09 02:18:53 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'main' into pr-dual-atmos

Browse Source
This commit is contained in:
Pedro Roque
2025-12-15 14:52:32 -08:00
committed by GitHub
parent cd5650c823 1345b3500a
commit 8cfc8d77ed
6 changed files with 69 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
msg/versioned/VehicleCommand.msg
+5
View File
@@ -100,6 +100,7 @@ uint16 VEHICLE_CMD_LOGGING_START = 2510 # Start streaming ULog data.
uint16 VEHICLE_CMD_LOGGING_STOP = 2511 # Stop streaming ULog data.
uint16 VEHICLE_CMD_CONTROL_HIGH_LATENCY = 2600 # Control starting/stopping transmitting data over the high latency link.
uint16 VEHICLE_CMD_DO_VTOL_TRANSITION = 3000 # Command VTOL transition.
uint16 VEHICLE_CMD_DO_SET_SAFETY_SWITCH_STATE = 5300 # Command safety on/off. |1 to activate safety, 0 to deactivate safety and allow control surface movements|Unused|Unused|Unused|Unused|Unused|Unused|
uint16 VEHICLE_CMD_ARM_AUTHORIZATION_REQUEST = 3001 # Request arm authorization.
uint16 VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY = 30001 # Prepare a payload deployment in the flight plan.
uint16 VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY = 30002 # Control a pre-programmed payload deployment.
@@ -179,6 +180,10 @@ int8 ARMING_ACTION_ARM = 1
uint8 GRIPPER_ACTION_RELEASE = 0
uint8 GRIPPER_ACTION_GRAB = 1
# Used as param1 in DO_SET_SAFETY_SWITCH_STATE command.
uint8 SAFETY_OFF = 0
uint8 SAFETY_ON = 1
uint8 ORB_QUEUE_LENGTH = 8
float32 param1 # Parameter 1, as defined by MAVLink uint16 VEHICLE_CMD enum.
src/modules/commander/Commander.cpp
+46
View File
@@ -302,6 +302,26 @@ int Commander::custom_command(int argc, char *argv[])
return 0;
}
if (!strcmp(argv[0], "safety")) {
if (argc < 2) {
PX4_ERR("missing argument");
return 1;
}
if (!strcmp(argv[1], "on")) {
send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_SAFETY_SWITCH_STATE, vehicle_command_s::SAFETY_ON);
} else if (!strcmp(argv[1], "off")) {
send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_SAFETY_SWITCH_STATE, vehicle_command_s::SAFETY_OFF);
} else {
PX4_ERR("invlaid argument, use [on|off]");
return 1;
}
return 0;
}
if (!strcmp(argv[0], "arm")) {
float param2 = 0.f;
@@ -495,6 +515,7 @@ int Commander::custom_command(int argc, char *argv[])
int Commander::print_status()
{
PX4_INFO("%s", isArmed() ? "Armed" : "Disarmed");
PX4_INFO("prearm safety: %s", _safety.isSafetyOff() ? "Off" : "On");
PX4_INFO("navigation mode: %s", mode_util::nav_state_names[_vehicle_status.nav_state]);
PX4_INFO("user intended navigation mode: %s", mode_util::nav_state_names[_vehicle_status.nav_state_user_intention]);
PX4_INFO("in failsafe: %s", _failsafe.inFailsafe() ? "yes" : "no");
@@ -1508,6 +1529,29 @@ Commander::handle_command(const vehicle_command_s &cmd)
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
break;
case vehicle_command_s::VEHICLE_CMD_DO_SET_SAFETY_SWITCH_STATE: {
// reject if armed, only allow pre or post flight for safety
if (isArmed()) {
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
} else {
int commanded_state = (int)cmd.param1;
if (commanded_state == vehicle_command_s::SAFETY_OFF) {
_safety.deactivateSafety();
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
} else if (commanded_state == vehicle_command_s::SAFETY_ON) {
_safety.activateSafety();
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
} else {
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_FAILED);
}
}
}
break;
case vehicle_command_s::VEHICLE_CMD_START_RX_PAIR:
case vehicle_command_s::VEHICLE_CMD_CUSTOM_0:
case vehicle_command_s::VEHICLE_CMD_CUSTOM_1:
@@ -3026,6 +3070,8 @@ The commander module contains the state machine for mode switching and failsafe
PRINT_MODULE_USAGE_ARG("mag|baro|accel|gyro|level|esc|airspeed", "Calibration type", false);
PRINT_MODULE_USAGE_ARG("quick", "Quick calibration [mag, accel (not recommended)]", false);
PRINT_MODULE_USAGE_COMMAND_DESCR("check", "Run preflight checks");
PRINT_MODULE_USAGE_COMMAND_DESCR("safety", "Change prearm safety state");
PRINT_MODULE_USAGE_ARG("on|off", "[on] to activate safety, [off] to deactivate safety and allow control surface movements", false);
PRINT_MODULE_USAGE_COMMAND("arm");
PRINT_MODULE_USAGE_PARAM_FLAG('f', "Force arming (do not run preflight checks)", true);
PRINT_MODULE_USAGE_COMMAND("disarm");
src/modules/commander/Safety.cpp
+7
View File
@@ -76,3 +76,10 @@ void Safety::activateSafety()
_safety_off = false;
}
}
void Safety::deactivateSafety()
{
if (!_safety_disabled) {
_safety_off = true;
}
}
src/modules/commander/Safety.hpp
+1
View File
@@ -49,6 +49,7 @@ public:
bool safetyButtonHandler();
void activateSafety();
void deactivateSafety();
bool isButtonAvailable() const { return _button_available; }
bool isSafetyOff() const { return _safety_off; }
bool isSafetyDisabled() const { return _safety_disabled; }
src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp
+10 -5
View File
@@ -104,12 +104,17 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
bool do_vel_pos_reset = false;
if (!_control_status.flags.gnss_fault && (_control_status.flags.gnss_vel || _control_status.flags.gnss_pos)) {
if (!_control_status.flags.gnss_fault && _control_status.flags.in_air && isYawFailure()) {
const bool velocity_fusion_failure = _aid_src_gnss_vel.innovation_rejected
&& isTimedOut(_time_last_hor_vel_fuse, _params.EKFGSF_reset_delay)
&& (_time_last_hor_vel_fuse > _time_last_on_ground_us);
if (_control_status.flags.in_air
&& isYawFailure()
&& isTimedOut(_time_last_hor_vel_fuse, _params.EKFGSF_reset_delay)
&& (_time_last_hor_vel_fuse > _time_last_on_ground_us)) {
const bool position_fusion_failure = _aid_src_gnss_pos.innovation_rejected
&& isTimedOut(_time_last_hor_pos_fuse, _params.EKFGSF_reset_delay)
&& (_time_last_hor_pos_fuse > _time_last_on_ground_us);
if ((_control_status.flags.gnss_vel && velocity_fusion_failure)
|| (_control_status.flags.gnss_pos && position_fusion_failure)) {
do_vel_pos_reset = tryYawEmergencyReset();
}
}
src/modules/logger/logged_topics.cpp
-2
View File
@@ -63,7 +63,6 @@ void LoggedTopics::add_default_topics()
add_optional_topic("external_ins_attitude");
add_optional_topic("external_ins_global_position");
add_optional_topic("external_ins_local_position");
// add_optional_topic("esc_status", 250);
add_topic("esc_status");
add_topic("failure_detector_status", 100);
add_topic("failsafe_flags");
@@ -209,7 +208,6 @@ void LoggedTopics::add_default_topics()
add_optional_topic_multi("vehicle_magnetometer", 500, 4);
add_topic("vehicle_optical_flow", 500);
add_topic("aux_global_position", 500);
//add_optional_topic("vehicle_optical_flow_vel", 100);
add_optional_topic("pps_capture");
// additional control allocation logging