Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Create identified messages field in store, use it in pwmsetup #2245

Merged
Merged
Show file tree
Hide file tree
Changes from all 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
27 changes: 20 additions & 7 deletions core/frontend/src/components/vehiclesetup/PwmSetup.vue
Original file line number Diff line number Diff line change
Expand Up @@ -248,25 +248,38 @@ export default Vue.extend({
}
})
},
vehicle_id(): number {
return autopilot_data.system_id
},
is_armed(): boolean {
const heartbeat = mavlink_store_get(mavlink, 'HEARTBEAT.messageData.message') as Message.Heartbeat
return Boolean(heartbeat.base_mode.bits & MavModeFlag.MAV_MODE_FLAG_SAFETY_ARMED)
const heartbeat = mavlink_store_get(
mavlink,
'HEARTBEAT.messageData.message',
this.vehicle_id,
1,
) as Message.Heartbeat
return Boolean(heartbeat?.base_mode.bits & MavModeFlag.MAV_MODE_FLAG_SAFETY_ARMED)
},
is_manual(): boolean {
const heartbeat = mavlink_store_get(mavlink, 'HEARTBEAT.messageData.message') as Message.Heartbeat
const heartbeat = mavlink_store_get(
mavlink,
'HEARTBEAT.messageData.message',
this.vehicle_id,
1,
) as Message.Heartbeat

// Legacy manual mode
if (!(heartbeat.base_mode.bits & MavModeFlag.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED)) {
return Boolean(heartbeat.base_mode.bits & MavModeFlag.MAV_MODE_FLAG_MANUAL_INPUT_ENABLED)
if (!(heartbeat?.base_mode.bits & MavModeFlag.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED)) {
return Boolean(heartbeat?.base_mode.bits & MavModeFlag.MAV_MODE_FLAG_MANUAL_INPUT_ENABLED)
}

if (this.is_rover) {
const rover_custom_mode_manual = 0
return Boolean(heartbeat.custom_mode === rover_custom_mode_manual)
return Boolean(heartbeat?.custom_mode === rover_custom_mode_manual)
}
if (this.is_sub) {
const sub_custom_mode_manual = 19
return Boolean(heartbeat.custom_mode === sub_custom_mode_manual)
return Boolean(heartbeat?.custom_mode === sub_custom_mode_manual)
}

return false
Expand Down
8 changes: 8 additions & 0 deletions core/frontend/src/store/mavlink.ts
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,7 @@ interface messsageRefreshRate {

class MavlinkStore extends VuexModule {
available_messages: Dictionary<MavlinkMessage> = {}
available_identified_messages: Dictionary<Dictionary<MavlinkMessage>> = {}

message_listeners: Dictionary<Listener> = {}

Expand Down Expand Up @@ -64,6 +65,13 @@ class MavlinkStore extends VuexModule {
// TODO: Check if this is the best possible way to update `available_messages`
// Reference: https://github.com/bluerobotics/blueos-docker/pull/508#discussion_r718729077
Vue.set(this.available_messages, message.messageName, message)
const header = message.messageData.header
const identifier = `${header.system_id}_${header.component_id}`
// make sure identifier exists
if (!(identifier in this.available_identified_messages)) {
Vue.set(this.available_identified_messages, identifier, {})
}
Vue.set(this.available_identified_messages[identifier], message.messageName, message)
}
}
}
Expand Down
5 changes: 4 additions & 1 deletion core/frontend/src/utils/mavlink.ts
Original file line number Diff line number Diff line change
@@ -1,5 +1,8 @@
import { get } from 'lodash'

export default function mavlink_store_get<T>(storage: T, path: string): unknown {
export default function mavlink_store_get<T>(storage: T, path: string, system_id? :number, component_id?: number): unknown {
if (system_id !== undefined && component_id !== undefined) {
return get(storage, `available_identified_messages.${system_id}_${component_id}.${path}`)
}
return get(storage, `available_messages.${path}`)
}