-
Notifications
You must be signed in to change notification settings - Fork 17.3k
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
AP_DDS: Status Message #28337
base: master
Are you sure you want to change the base?
AP_DDS: Status Message #28337
Changes from all commits
File filter
Filter by extension
Conversations
Jump to
Diff view
Diff view
There are no files selected for viewing
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,26 @@ | ||
std_msgs/Header header | ||
|
||
uint8 APM_ROVER = 1 | ||
uint8 APM_ARDUCOPTER = 2 | ||
uint8 APM_ARDUPLANE = 3 | ||
uint8 APM_ANTENNATRACKER = 4 | ||
uint8 APM_UNKNOWN = 5 | ||
uint8 APM_REPLAY = 6 | ||
uint8 APM_ARDUSUB = 7 | ||
uint8 APM_IOFIRMWARE = 8 | ||
uint8 APM_AP_PERIPH = 9 | ||
uint8 APM_AP_DAL_STANDALONE = 10 | ||
uint8 APM_AP_BOOTLOADER = 11 | ||
uint8 APM_BLIMP = 12 | ||
uint8 APM_HELI = 13 | ||
uint8 vehicle_type # From AP_Vehicle_Type.h | ||
|
||
bool armed # true if vehicle is armed. | ||
uint8 mode # Vehicle mode, enum depending on vehicle type. | ||
bool flying # True if flying/driving/diving/tracking. | ||
|
||
uint8 FS_RADIO = 1 | ||
uint8 FS_BATTERY = 2 | ||
uint8 FS_GCS = 3 | ||
uint8 FS_EKF = 4 | ||
uint8[] failsafe # Array containing all active failsafe. |
Original file line number | Diff line number | Diff line change |
---|---|---|
|
@@ -14,6 +14,7 @@ | |
#include <AP_AHRS/AP_AHRS.h> | ||
#include <AP_Arming/AP_Arming.h> | ||
#include <AP_Vehicle/AP_Vehicle.h> | ||
#include <AP_Common/AP_FWVersion.h> | ||
#include <AP_ExternalControl/AP_ExternalControl_config.h> | ||
|
||
#include "ardupilot_msgs/srv/ArmMotors.h" | ||
|
@@ -42,6 +43,7 @@ static constexpr uint16_t DELAY_GEO_POSE_TOPIC_MS = 33; | |
static constexpr uint16_t DELAY_CLOCK_TOPIC_MS = 10; | ||
static constexpr uint16_t DELAY_GPS_GLOBAL_ORIGIN_TOPIC_MS = 1000; | ||
static constexpr uint16_t DELAY_PING_MS = 500; | ||
static constexpr uint16_t DELAY_STATUS_TOPIC_MS = 500; | ||
|
||
// Define the subscriber data members, which are static class scope. | ||
// If these are created on the stack in the subscriber, | ||
|
@@ -525,6 +527,50 @@ void AP_DDS_Client::update_topic(geographic_msgs_msg_GeoPointStamped& msg) | |
} | ||
} | ||
|
||
bool AP_DDS_Client::update_topic(ardupilot_msgs_msg_Status& msg) | ||
{ | ||
// Fill the new message. | ||
msg.vehicle_type = static_cast<uint8_t>(AP::fwversion().vehicle_type); | ||
msg.armed = AP_Notify::flags.armed; | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. should be hal.util->get_soft_armed() |
||
msg.mode = AP::vehicle()->get_mode(); | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. should do const auto &vehicle = AP::vehicle(); |
||
msg.flying = AP_Notify::flags.flying; | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. should be vehicle->get_likely_flying(); |
||
uint8_t fs_iter = 0; | ||
msg.failsafe_size = 0; | ||
if ( AP_Notify::flags.failsafe_radio ) { | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. nitpick: little whitespace within the brackets |
||
msg.failsafe[fs_iter++] = FS_RADIO; | ||
} | ||
if ( AP_Notify::flags.failsafe_battery ) { | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Use this from AP_BattMon There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. I wonder if we could get the battery failsafe state from the battery library. For RC, EKF and GCS failsafe we probably need to do what you've done here |
||
msg.failsafe[fs_iter++] = FS_BATTERY; | ||
} | ||
if ( AP_Notify::flags.failsafe_gcs ) { | ||
msg.failsafe[fs_iter++] = FS_GCS; | ||
} | ||
if ( AP_Notify::flags.failsafe_ekf ) { | ||
msg.failsafe[fs_iter++] = FS_EKF; | ||
} | ||
msg.failsafe_size = fs_iter; | ||
|
||
// Compare with the previous one. | ||
bool is_message_changed {false}; | ||
is_message_changed |= (last_status_msg_.flying != msg.flying); | ||
is_message_changed |= (last_status_msg_.armed != msg.armed); | ||
is_message_changed |= (last_status_msg_.mode != msg.mode); | ||
is_message_changed |= (last_status_msg_.vehicle_type != msg.vehicle_type); | ||
is_message_changed |= (last_status_msg_.failsafe_size != msg.failsafe_size); | ||
|
||
if ( is_message_changed ) { | ||
last_status_msg_.flying = msg.flying; | ||
last_status_msg_.armed = msg.armed; | ||
last_status_msg_.mode = msg.mode; | ||
last_status_msg_.vehicle_type = msg.vehicle_type; | ||
last_status_msg_.failsafe_size = msg.failsafe_size; | ||
update_topic(msg.header.stamp); | ||
return true; | ||
} else { | ||
return false; | ||
} | ||
} | ||
|
||
/* | ||
start the DDS thread | ||
*/ | ||
|
@@ -1134,6 +1180,21 @@ void AP_DDS_Client::write_gps_global_origin_topic() | |
} | ||
} | ||
|
||
void AP_DDS_Client::write_status_topic() | ||
{ | ||
WITH_SEMAPHORE(csem); | ||
if (connected) { | ||
ucdrBuffer ub {}; | ||
const uint32_t topic_size = ardupilot_msgs_msg_Status_size_of_topic(&status_topic, 0); | ||
uxr_prepare_output_stream(&session, reliable_out, topics[to_underlying(TopicIndex::STATUS_PUB)].dw_id, &ub, topic_size); | ||
const bool success = ardupilot_msgs_msg_Status_serialize_topic(&ub, &status_topic); | ||
if (!success) { | ||
// TODO sometimes serialization fails on bootup. Determine why. | ||
// AP_HAL::panic("FATAL: DDS_Client failed to serialize\n"); | ||
} | ||
} | ||
} | ||
|
||
void AP_DDS_Client::update() | ||
{ | ||
WITH_SEMAPHORE(csem); | ||
|
@@ -1194,6 +1255,13 @@ void AP_DDS_Client::update() | |
write_gps_global_origin_topic(); | ||
} | ||
|
||
if (cur_time_ms - last_status_check_time_ms > DELAY_STATUS_TOPIC_MS) { | ||
if (update_topic(status_topic)) { | ||
write_status_topic(); | ||
} | ||
last_status_check_time_ms = cur_time_ms; | ||
} | ||
|
||
status_ok = uxr_run_session_time(&session, 1); | ||
} | ||
|
||
|
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,52 @@ | ||
// generated from rosidl_adapter/resource/msg.idl.em | ||
// with input from ardupilot_msgs/msg/Status.msg | ||
// generated code does not contain a copyright notice | ||
|
||
#include "std_msgs/msg/Header.idl" | ||
|
||
module ardupilot_msgs { | ||
module msg { | ||
module Status_Constants { | ||
const uint8 APM_ROVER = 1; | ||
const uint8 APM_ARDUCOPTER = 2; | ||
const uint8 APM_ARDUPLANE = 3; | ||
const uint8 APM_ANTENNATRACKER = 4; | ||
const uint8 APM_UNKNOWN = 5; | ||
const uint8 APM_REPLAY = 6; | ||
const uint8 APM_ARDUSUB = 7; | ||
const uint8 APM_IOFIRMWARE = 8; | ||
const uint8 APM_AP_PERIPH = 9; | ||
const uint8 APM_AP_DAL_STANDALONE = 10; | ||
const uint8 APM_AP_BOOTLOADER = 11; | ||
const uint8 APM_BLIMP = 12; | ||
const uint8 APM_HELI = 13; | ||
const uint8 FS_RADIO = 1; | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. should this be a separate enum? seems odd to fix FS and APM_xxx |
||
const uint8 FS_BATTERY = 2; | ||
const uint8 FS_GCS = 3; | ||
const uint8 FS_EKF = 4; | ||
}; | ||
struct Status { | ||
std_msgs::msg::Header header; | ||
|
||
@verbatim (language="comment", text= | ||
"From AP_Vehicle_Type.h") | ||
uint8 vehicle_type; | ||
|
||
@verbatim (language="comment", text= | ||
"true if vehicle is armed.") | ||
boolean armed; | ||
|
||
@verbatim (language="comment", text= | ||
"Vehicle mode, enum depending on vehicle type.") | ||
uint8 mode; | ||
|
||
@verbatim (language="comment", text= | ||
"True if flying/driving/diving/tracking.") | ||
boolean flying; | ||
|
||
@verbatim (language="comment", text= | ||
"Array containing all active failsafe.") | ||
sequence<uint8> failsafe; | ||
}; | ||
}; | ||
}; |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Can we make this event driven instead rather than poll?
This introduces as most 500mS delay on known the vehicle has failsafed which may be a long time.