Skip to content

Commit

Permalink
added translation for ArmingCheckReply.msg
Browse files Browse the repository at this point in the history
Signed-off-by: RomanBapst <[email protected]>
  • Loading branch information
RomanBapst committed Feb 21, 2025
1 parent febe9d7 commit eea9764
Show file tree
Hide file tree
Showing 4 changed files with 117 additions and 1 deletion.
35 changes: 35 additions & 0 deletions msg/px4_msgs_old/msg/ArmingCheckReplyV0.msg
Original file line number Diff line number Diff line change
@@ -0,0 +1,35 @@
uint32 MESSAGE_VERSION = 0

uint64 timestamp # time since system start (microseconds)

uint8 request_id
uint8 registration_id

uint8 HEALTH_COMPONENT_INDEX_NONE = 0

uint8 health_component_index # HEALTH_COMPONENT_INDEX_*
bool health_component_is_present
bool health_component_warning
bool health_component_error

bool can_arm_and_run # whether arming is possible, and if it's a navigation mode, if it can run

uint8 num_events

Event[5] events

# Mode requirements
bool mode_req_angular_velocity
bool mode_req_attitude
bool mode_req_local_alt
bool mode_req_local_position
bool mode_req_local_position_relaxed
bool mode_req_global_position
bool mode_req_global_position_relaxed
bool mode_req_mission
bool mode_req_home_position
bool mode_req_prevent_arming
bool mode_req_manual_control


uint8 ORB_QUEUE_LENGTH = 4
1 change: 1 addition & 0 deletions msg/translation_node/translations/all_translations.h
Original file line number Diff line number Diff line change
Expand Up @@ -11,3 +11,4 @@
//#include "example_translation_service_v1.h"

#include "translation_vehicle_status_v1.h"
#include "translation_arming_check_reply_v1.h"
Original file line number Diff line number Diff line change
@@ -0,0 +1,80 @@
/****************************************************************************
* Copyright (c) 2025 PX4 Development Team.
* SPDX-License-Identifier: BSD-3-Clause
****************************************************************************/
#pragma once

// Translate ArmingCheckReply v0 <--> v1
#include <px4_msgs_old/msg/arming_check_reply_v0.hpp>
#include <px4_msgs/msg/arming_check_reply.hpp>

class ArmingCheckReplyV1Translation {
public:
using MessageOlder = px4_msgs_old::msg::ArmingCheckReplyV0;
static_assert(MessageOlder::Request::MESSAGE_VERSION == 0);

using MessageNewer = px4_msgs::msg::ArmingCheckReplyV1;
static_assert(MessageNewer::Request::MESSAGE_VERSION == 1);

static constexpr const char* kTopic = "/fmu/in/arming_check_reply";

static void fromOlder(const MessageOlder::Request &msg_older, MessageNewer::Request &msg_newer) {
// Request: set msg_newer from msg_older
msg_newer.timestamp = msg_older.timestamp;

msg_newer.request_id = msg_older.request_id;
msg_newer.registration_id = msg_older.registration_id;

msg_newer.health_component_index = msg_older.health_component_index;
msg_newer.health_component_is_present = msg_older.health_component_is_present;
msg_newer.health_component_warning = msg_older.health_component_warning;
msg_newer.health_component_error = msg_older.health_component_error;

msg_newer.can_arm_and_run = msg_older.can_arm_and_run;

memcpy(msg_newer.events, msg_older.events, sizeof(Event));

msg_newer.mode_req_angular_velocity = msg_older.mode_req_angular_velocity;
msg_newer.mode_req_attitude = msg_older.mode_req_attitude;
msg_newer.mode_req_local_alt = msg_older.mode_req_local_alt;
msg_newer.mode_req_local_position = msg_older.ode_req_local_position;
msg_newer.mode_req_local_position_relaxed = msg_older.ode_req_local_position_relaxed;
msg_newer.ode_req_global_position = msg_older.ode_req_global_position;
msg_newer.mode_req_global_position_relaxed = false;
msg_newer.mode_req_mission = msg_older.mode_req_mission;
msg_newer.mode_req_home_position = msg_older.mode_req_home_position;
msg_newer.mode_req_prevent_arming = msg_older.mode_req_prevent_arming;
msg_newer.mode_req_manual_control = msg_older.mode_req_manual_control;

}

static void toOlder(const MessageNewer::Request &msg_newer, MessageOlder::Request &msg_older) {
// Request: set msg_older from msg_newer
msg_older.timestamp = msg_newer.timestamp;

msg_older.request_id = msg_newer.request_id;
msg_older.registration_id = msg_newer.registration_id;

msg_older.health_component_index = msg_newer.health_component_index;
msg_older.health_component_is_present = msg_newer.health_component_is_present;
msg_older.health_component_warning = msg_newer.health_component_warning;
msg_older.health_component_error = msg_newer.health_component_error;

msg_older.can_arm_and_run = msg_newer.can_arm_and_run;

memcpy(msg_older.events, msg_newer.events, sizeof(Event));

msg_older.mode_req_angular_velocity = msg_newer.mode_req_angular_velocity;
msg_older.mode_req_attitude = msg_newer.mode_req_attitude;
msg_older.mode_req_local_alt = msg_newer.mode_req_local_alt;
msg_older.mode_req_local_position = msg_newer.ode_req_local_position;
msg_older.ode_req_global_position = msg_newer.ode_req_global_position;
msg_older.mode_req_global_position_relaxed = false;
msg_older.mode_req_mission = msg_newer.mode_req_mission;
msg_older.mode_req_home_position = msg_newer.mode_req_home_position;
msg_older.mode_req_prevent_arming = msg_newer.mode_req_prevent_arming;
msg_older.mode_req_manual_control = msg_newer.mode_req_manual_control;
}
};

REGISTER_SERVICE_TRANSLATION_DIRECT(ArmingCheckReplyV1Translation);
2 changes: 1 addition & 1 deletion msg/versioned/ArmingCheckReply.msg
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
uint32 MESSAGE_VERSION = 0
uint32 MESSAGE_VERSION = 1

uint64 timestamp # time since system start (microseconds)

Expand Down

0 comments on commit eea9764

Please sign in to comment.