Skip to content
Open
Show file tree
Hide file tree
Changes from 11 commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
28 changes: 14 additions & 14 deletions msg/FailsafeFlags.msg
Original file line number Diff line number Diff line change
Expand Up @@ -6,20 +6,20 @@
uint64 timestamp # time since system start (microseconds)

# Per-mode requirements
uint32 mode_req_angular_velocity
uint32 mode_req_attitude
uint32 mode_req_local_alt
uint32 mode_req_local_position
uint32 mode_req_local_position_relaxed
uint32 mode_req_global_position
uint32 mode_req_global_position_relaxed
uint32 mode_req_mission
uint32 mode_req_offboard_signal
uint32 mode_req_home_position
uint32 mode_req_wind_and_flight_time_compliance # if set, mode cannot be entered if wind or flight time limit exceeded
uint32 mode_req_prevent_arming # if set, cannot arm while in this mode
uint32 mode_req_manual_control
uint32 mode_req_other # other requirements, not covered above (for external modes)
uint64 mode_req_angular_velocity
uint64 mode_req_attitude
uint64 mode_req_local_alt
uint64 mode_req_local_position
uint64 mode_req_local_position_relaxed
uint64 mode_req_global_position
uint64 mode_req_global_position_relaxed
uint64 mode_req_mission
uint64 mode_req_offboard_signal
uint64 mode_req_home_position
uint64 mode_req_wind_and_flight_time_compliance # if set, mode cannot be entered if wind or flight time limit exceeded
uint64 mode_req_prevent_arming # if set, cannot arm while in this mode
uint64 mode_req_manual_control
uint64 mode_req_other # other requirements, not covered above (for external modes)


# Mode requirements
Expand Down
2 changes: 1 addition & 1 deletion msg/versioned/ArmingCheckReply.msg
Original file line number Diff line number Diff line change
Expand Up @@ -40,4 +40,4 @@ bool mode_req_home_position # Requires a home position (such as RTL/Return mode)
bool mode_req_prevent_arming # Prevent arming (such as in Land mode).
bool mode_req_manual_control # Requires a manual controller

uint8 ORB_QUEUE_LENGTH = 4 #
uint8 ORB_QUEUE_LENGTH = 16 #
10 changes: 9 additions & 1 deletion msg/versioned/VehicleStatus.msg
Original file line number Diff line number Diff line change
Expand Up @@ -64,7 +64,15 @@ uint8 NAVIGATION_STATE_EXTERNAL5 = 27
uint8 NAVIGATION_STATE_EXTERNAL6 = 28
uint8 NAVIGATION_STATE_EXTERNAL7 = 29
uint8 NAVIGATION_STATE_EXTERNAL8 = 30
uint8 NAVIGATION_STATE_MAX = 31
uint8 NAVIGATION_STATE_EXTERNAL9 = 31
uint8 NAVIGATION_STATE_EXTERNAL10 = 32
uint8 NAVIGATION_STATE_EXTERNAL11 = 33
uint8 NAVIGATION_STATE_EXTERNAL12 = 34
uint8 NAVIGATION_STATE_EXTERNAL13 = 35
uint8 NAVIGATION_STATE_EXTERNAL14 = 36
uint8 NAVIGATION_STATE_EXTERNAL15 = 37
uint8 NAVIGATION_STATE_EXTERNAL16 = 38
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

So 16 external modes. Does this need to be documented in the user guide? Perhaps pointing here so that people can find latest version if it changes.

uint8 NAVIGATION_STATE_MAX = 39

uint8 executor_in_charge # Current mode executor in charge (0=Autopilot)

Expand Down
78 changes: 75 additions & 3 deletions src/lib/events/enums.json
Original file line number Diff line number Diff line change
Expand Up @@ -47,7 +47,7 @@
"namespace": "px4",
"enums": {
"navigation_mode_group_t": {
"type": "uint32_t",
"type": "uint64_t",
"description": "Navigation/flight mode group bits. All modes within a group share the same arming checks. For now we directly map group == mode.",
"is_bitfield": true,
"entries": {
Expand Down Expand Up @@ -142,6 +142,38 @@
"1073741824": {
"name": "external8",
"description": "External 8"
},
"2147483648": {
"name": "external9",
"description": "External 9"
},
"4294967296": {
"name": "external10",
"description": "External 10"
},
"8589934592": {
"name": "external11",
"description": "External 11"
},
"17179869184": {
"name": "external12",
"description": "External 12"
},
"34359738368": {
"name": "external13",
"description": "External 13"
},
"68719476736": {
"name": "external14",
"description": "External 14"
},
"137438953472": {
"name": "external15",
"description": "External 15"
},
"274877906944": {
"name": "external16",
"description": "External 16"
}
}
},
Expand Down Expand Up @@ -604,6 +636,38 @@
"name": "external8",
"description": "External 8"
},
"24": {
"name": "external9",
"description": "External 9"
},
"25": {
"name": "external10",
"description": "External 10"
},
"26": {
"name": "external11",
"description": "External 11"
},
"27": {
"name": "external12",
"description": "External 12"
},
"28": {
"name": "external13",
"description": "External 13"
},
"29": {
"name": "external14",
"description": "External 14"
},
"30": {
"name": "external15",
"description": "External 15"
},
"31": {
"name": "external16",
"description": "External 16"
},
"255": {
"name": "unknown",
"description": "[Unknown]"
Expand Down Expand Up @@ -727,7 +791,6 @@
},
"navigation_mode_groups": {
"groups": {
"0": [65536],
"1": [131072],
"2": [196608],
"3": [67371008],
Expand All @@ -749,7 +812,16 @@
"27": [251920384],
"28": [268697600],
"29": [285474816],
"30": [302252032]
"30": [302252032],
"31": [536870912],
"32": [1073741824],
"33": [2147483648],
"34": [4294967296],
"35": [8589934592],
"36": [17179869184],
"37": [34359738368],
"38": [68719476736],
"39": [137438953472]
}
},
"supported_protocols": [ "health_and_arming_check" ]
Expand Down
26 changes: 25 additions & 1 deletion src/lib/modes/ui.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -63,7 +63,7 @@ static inline uint32_t getValidNavStates()
(1u << vehicle_status_s::NAVIGATION_STATE_ORBIT) |
(1u << vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF);

static_assert(vehicle_status_s::NAVIGATION_STATE_MAX == 31, "update valid nav states");
static_assert(vehicle_status_s::NAVIGATION_STATE_MAX == 39, "update valid nav states");
}

const char *const nav_state_names[vehicle_status_s::NAVIGATION_STATE_MAX] = {
Expand Down Expand Up @@ -98,6 +98,14 @@ const char *const nav_state_names[vehicle_status_s::NAVIGATION_STATE_MAX] = {
"External 6",
"External 7",
"External 8",
"External 9",
"External 10",
"External 11",
"External 12",
"External 13",
"External 14",
"External 15",
"External 16",
};

/**
Expand Down Expand Up @@ -126,6 +134,22 @@ static inline bool isAdvanced(uint8_t nav_state)

case vehicle_status_s::NAVIGATION_STATE_EXTERNAL8: return false;

case vehicle_status_s::NAVIGATION_STATE_EXTERNAL9: return false;

case vehicle_status_s::NAVIGATION_STATE_EXTERNAL10: return false;

case vehicle_status_s::NAVIGATION_STATE_EXTERNAL11: return false;

case vehicle_status_s::NAVIGATION_STATE_EXTERNAL12: return false;

case vehicle_status_s::NAVIGATION_STATE_EXTERNAL13: return false;

case vehicle_status_s::NAVIGATION_STATE_EXTERNAL14: return false;

case vehicle_status_s::NAVIGATION_STATE_EXTERNAL15: return false;

case vehicle_status_s::NAVIGATION_STATE_EXTERNAL16: return false;

}

return true;
Expand Down
2 changes: 1 addition & 1 deletion src/modules/commander/Commander.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -838,7 +838,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
desired_nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND;
break;

case PX4_CUSTOM_SUB_MODE_EXTERNAL1...PX4_CUSTOM_SUB_MODE_EXTERNAL8:
case PX4_CUSTOM_SUB_MODE_EXTERNAL1...PX4_CUSTOM_SUB_MODE_EXTERNAL16:
desired_nav_state = vehicle_status_s::NAVIGATION_STATE_EXTERNAL1 + (custom_sub_mode - PX4_CUSTOM_SUB_MODE_EXTERNAL1);
break;

Expand Down
18 changes: 10 additions & 8 deletions src/modules/commander/HealthAndArmingChecks/Common.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -43,13 +43,15 @@ void Report::getHealthReport(health_report_s &report) const
for (int i = 0; i < vehicle_status_s::NAVIGATION_STATE_MAX; ++i) {
NavModes group = getModeGroup(i);

if ((uint32_t)(current_results.arming_checks.can_arm & group)) {
if ((uint64_t)(current_results.arming_checks.can_arm & group)) {
report.can_arm_mode_flags |= 1ull << i;
}

if ((uint32_t)(current_results.arming_checks.can_run & group)) {
if ((uint64_t)(current_results.arming_checks.can_run & group)) {
report.can_run_mode_flags |= 1ull << i;
}


}

report.arming_check_error_flags = (uint64_t)current_results.arming_checks.error;
Expand All @@ -63,26 +65,26 @@ void Report::healthFailure(NavModes required_modes, HealthComponentIndex compone
const events::LogLevels &log_levels, const char *message)
{
healthFailure(required_modes, component, log_levels.external);
addEvent(event_id, log_levels, message, (uint32_t)reportedModes(required_modes), component.index);
addEvent(event_id, log_levels, message, (uint64_t)reportedModes(required_modes), component.index);
}

void Report::armingCheckFailure(NavModes required_modes, HealthComponentIndex component, uint32_t event_id,
const events::LogLevels &log_levels, const char *message)
{
armingCheckFailure(required_modes, component, log_levels.external);
addEvent(event_id, log_levels, message, (uint32_t)reportedModes(required_modes), component.index);
addEvent(event_id, log_levels, message, (uint64_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);
(uint64_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)
uint64_t modes, unsigned args_size)
{
unsigned total_size = sizeof(EventBufferHeader) + args_size;
EventBufferHeader *header = (EventBufferHeader *)(_event_buffer + _next_buffer_idx);
Expand Down Expand Up @@ -113,7 +115,7 @@ bool Report::addExternalEvent(const event_s &event, NavModes modes)

events::LogLevels log_levels{events::externalLogLevel(event.log_levels), events::internalLogLevel((event.log_levels))};
memcpy(_event_buffer + _next_buffer_idx + sizeof(EventBufferHeader), &event.arguments, args_size);
addEventToBuffer(event.id, log_levels, (uint32_t)modes, args_size);
addEventToBuffer(event.id, log_levels, (uint64_t)modes, args_size);
return true;
}

Expand Down Expand Up @@ -216,7 +218,7 @@ void Report::prepare(uint8_t vehicle_type)
NavModes Report::getModeGroup(uint8_t nav_state) const
{
// Note: this needs to match with the json metadata definition "navigation_mode_groups"
return (NavModes)(1u << nav_state);
return static_cast<NavModes>(1ull << nav_state);
}

bool Report::finalize()
Expand Down
Loading
Loading