Skip to content

Commit

Permalink
Merge branch 'master' into master
Browse files Browse the repository at this point in the history
  • Loading branch information
sunny-1987 authored Oct 29, 2024
2 parents 36663e5 + a6f00a3 commit e404681
Show file tree
Hide file tree
Showing 16 changed files with 179 additions and 97 deletions.
4 changes: 4 additions & 0 deletions .github/workflows/qurt_build.yml
Original file line number Diff line number Diff line change
Expand Up @@ -157,6 +157,9 @@ jobs:
cp -a build/QURT/bin/arducopter build/QURT/ArduPilot_Copter.so
cp -a build/QURT/bin/arduplane build/QURT/ArduPilot_Plane.so
cp -a build/QURT/bin/ardurover build/QURT/ArduPilot_Rover.so
libraries/AP_HAL_QURT/packaging/make_package.sh ArduCopter arducopter
libraries/AP_HAL_QURT/packaging/make_package.sh ArduPlane arduplane
libraries/AP_HAL_QURT/packaging/make_package.sh Rover ardurover
- name: Archive build
uses: actions/upload-artifact@v4
Expand All @@ -168,4 +171,5 @@ jobs:
build/QURT/ArduPilot_Plane.so
build/QURT/ArduPilot_Rover.so
build/QURT/ArduPilot.so
libraries/AP_HAL_QURT/packaging/*.deb
retention-days: 7
17 changes: 14 additions & 3 deletions libraries/AP_DDS/AP_DDS_Client.cpp
Original file line number Diff line number Diff line change
@@ -1,5 +1,6 @@
#include <AP_HAL/AP_HAL_Boards.h>

#include "AP_DDS_config.h"
#if AP_DDS_ENABLED
#include <uxr/client/util/ping.h>

Expand All @@ -12,16 +13,22 @@
#include <GCS_MAVLink/GCS.h>
#include <AP_BattMonitor/AP_BattMonitor.h>
#include <AP_AHRS/AP_AHRS.h>
#if AP_DDS_ARM_SERVER_ENABLED
#include <AP_Arming/AP_Arming.h>
# endif // AP_DDS_ARM_SERVER_ENABLED
#include <AP_Vehicle/AP_Vehicle.h>
#include <AP_ExternalControl/AP_ExternalControl_config.h>

#if AP_DDS_ARM_SERVER_ENABLED
#include "ardupilot_msgs/srv/ArmMotors.h"
#endif // AP_DDS_ARM_SERVER_ENABLED
#if AP_DDS_MODE_SWITCH_SERVER_ENABLED
#include "ardupilot_msgs/srv/ModeSwitch.h"
#endif // AP_DDS_MODE_SWITCH_SERVER_ENABLED

#if AP_EXTERNAL_CONTROL_ENABLED
#include "AP_DDS_ExternalControl.h"
#endif
#endif // AP_EXTERNAL_CONTROL_ENABLED
#include "AP_DDS_Frames.h"

#include "AP_DDS_Client.h"
Expand Down Expand Up @@ -667,7 +674,7 @@ void AP_DDS_Client::on_topic(uxrSession* uxr_session, uxrObjectId object_id, uin
break;
}
#endif // AP_DDS_JOY_SUB_ENABLED
#if AP_DDS_DYNAMIC_TF_SUB
#if AP_DDS_DYNAMIC_TF_SUB_ENABLED
case topics[to_underlying(TopicIndex::DYNAMIC_TRANSFORMS_SUB)].dr_id.id: {
const bool success = tf2_msgs_msg_TFMessage_deserialize_topic(ub, &rx_dynamic_transforms_topic);
if (success == false) {
Expand All @@ -684,7 +691,7 @@ void AP_DDS_Client::on_topic(uxrSession* uxr_session, uxrObjectId object_id, uin
}
break;
}
#endif // AP_DDS_DYNAMIC_TF_SUB
#endif // AP_DDS_DYNAMIC_TF_SUB_ENABLED
#if AP_DDS_VEL_CTRL_ENABLED
case topics[to_underlying(TopicIndex::VELOCITY_CONTROL_SUB)].dr_id.id: {
const bool success = geometry_msgs_msg_TwistStamped_deserialize_topic(ub, &rx_velocity_control_topic);
Expand Down Expand Up @@ -732,6 +739,7 @@ void AP_DDS_Client::on_request(uxrSession* uxr_session, uxrObjectId object_id, u
(void) request_id;
(void) length;
switch (object_id.id) {
#if AP_DDS_ARM_SERVER_ENABLED
case services[to_underlying(ServiceIndex::ARMING_MOTORS)].rep_id: {
ardupilot_msgs_srv_ArmMotors_Request arm_motors_request;
ardupilot_msgs_srv_ArmMotors_Response arm_motors_response;
Expand Down Expand Up @@ -761,6 +769,8 @@ void AP_DDS_Client::on_request(uxrSession* uxr_session, uxrObjectId object_id, u
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s Request for Arming/Disarming : %s", msg_prefix, arm_motors_response.result ? "SUCCESS" : "FAIL");
break;
}
#endif // AP_DDS_ARM_SERVER_ENABLED
#if AP_DDS_MODE_SWITCH_SERVER_ENABLED
case services[to_underlying(ServiceIndex::MODE_SWITCH)].rep_id: {
ardupilot_msgs_srv_ModeSwitch_Request mode_switch_request;
ardupilot_msgs_srv_ModeSwitch_Response mode_switch_response;
Expand Down Expand Up @@ -789,6 +799,7 @@ void AP_DDS_Client::on_request(uxrSession* uxr_session, uxrObjectId object_id, u
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s Request for Mode Switch : %s", msg_prefix, mode_switch_response.status ? "SUCCESS" : "FAIL");
break;
}
#endif // AP_DDS_MODE_SWITCH_SERVER_ENABLED
}
}

Expand Down
4 changes: 2 additions & 2 deletions libraries/AP_DDS/AP_DDS_Client.h
Original file line number Diff line number Diff line change
Expand Up @@ -195,10 +195,10 @@ class AP_DDS_Client
// incoming REP147 goal interface global position
static ardupilot_msgs_msg_GlobalPosition rx_global_position_control_topic;
#endif // AP_DDS_GLOBAL_POS_CTRL_ENABLED
#if AP_DDS_DYNAMIC_TF_SUB
#if AP_DDS_DYNAMIC_TF_SUB_ENABLED
// incoming transforms
static tf2_msgs_msg_TFMessage rx_dynamic_transforms_topic;
#endif // AP_DDS_DYNAMIC_TF_SUB
#endif // AP_DDS_DYNAMIC_TF_SUB_ENABLED
HAL_Semaphore csem;

// connection parametrics
Expand Down
9 changes: 9 additions & 0 deletions libraries/AP_DDS/AP_DDS_Service_Table.h
Original file line number Diff line number Diff line change
@@ -1,8 +1,13 @@
#include "uxr/client/client.h"
#include <AP_DDS/AP_DDS_config.h>

enum class ServiceIndex: uint8_t {
#if AP_DDS_ARM_SERVER_ENABLED
ARMING_MOTORS,
#endif // #if AP_DDS_ARM_SERVER_ENABLED
#if AP_DDS_MODE_SWITCH_SERVER_ENABLED
MODE_SWITCH
#endif // AP_DDS_MODE_SWITCH_SERVER_ENABLED
};

static inline constexpr uint8_t to_underlying(const ServiceIndex index)
Expand All @@ -12,6 +17,7 @@ static inline constexpr uint8_t to_underlying(const ServiceIndex index)
}

constexpr struct AP_DDS_Client::Service_table AP_DDS_Client::services[] = {
#if AP_DDS_ARM_SERVER_ENABLED
{
.req_id = to_underlying(ServiceIndex::ARMING_MOTORS),
.rep_id = to_underlying(ServiceIndex::ARMING_MOTORS),
Expand All @@ -28,6 +34,8 @@ constexpr struct AP_DDS_Client::Service_table AP_DDS_Client::services[] = {
.depth = 5,
},
},
#endif // AP_DDS_ARM_SERVER_ENABLED
#if AP_DDS_MODE_SWITCH_SERVER_ENABLED
{
.req_id = to_underlying(ServiceIndex::MODE_SWITCH),
.rep_id = to_underlying(ServiceIndex::MODE_SWITCH),
Expand All @@ -38,4 +46,5 @@ constexpr struct AP_DDS_Client::Service_table AP_DDS_Client::services[] = {
.request_topic_name = "rq/ap/mode_switchRequest",
.reply_topic_name = "rr/ap/mode_switchReply",
},
#endif // AP_DDS_MODE_SWITCH_SERVER_ENABLED
};
8 changes: 4 additions & 4 deletions libraries/AP_DDS/AP_DDS_Topic_Table.h
Original file line number Diff line number Diff line change
Expand Up @@ -51,9 +51,9 @@ enum class TopicIndex: uint8_t {
#if AP_DDS_JOY_SUB_ENABLED
JOY_SUB,
#endif // AP_DDS_JOY_SUB_ENABLED
#if AP_DDS_DYNAMIC_TF_SUB
#if AP_DDS_DYNAMIC_TF_SUB_ENABLED
DYNAMIC_TRANSFORMS_SUB,
#endif // AP_DDS_DYNAMIC_TF_SUB
#endif // AP_DDS_DYNAMIC_TF_SUB_ENABLED
#if AP_DDS_VEL_CTRL_ENABLED
VELOCITY_CONTROL_SUB,
#endif // AP_DDS_VEL_CTRL_ENABLED
Expand Down Expand Up @@ -286,7 +286,7 @@ constexpr struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = {
},
},
#endif // AP_DDS_JOY_SUB_ENABLED
#if AP_DDS_DYNAMIC_TF_SUB
#if AP_DDS_DYNAMIC_TF_SUB_ENABLED
{
.topic_id = to_underlying(TopicIndex::DYNAMIC_TRANSFORMS_SUB),
.pub_id = to_underlying(TopicIndex::DYNAMIC_TRANSFORMS_SUB),
Expand All @@ -303,7 +303,7 @@ constexpr struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = {
.depth = 5,
},
},
#endif // AP_DDS_DYNAMIC_TF_SUB
#endif // AP_DDS_DYNAMIC_TF_SUB_ENABLED
#if AP_DDS_VEL_CTRL_ENABLED
{
.topic_id = to_underlying(TopicIndex::VELOCITY_CONTROL_SUB),
Expand Down
14 changes: 11 additions & 3 deletions libraries/AP_DDS/AP_DDS_config.h
Original file line number Diff line number Diff line change
Expand Up @@ -114,15 +114,23 @@
#define AP_DDS_GLOBAL_POS_CTRL_ENABLED 1
#endif

#ifndef AP_DDS_DYNAMIC_TF_SUB
#define AP_DDS_DYNAMIC_TF_SUB 1
#ifndef AP_DDS_DYNAMIC_TF_SUB_ENABLED
#define AP_DDS_DYNAMIC_TF_SUB_ENABLED 1
#endif

#ifndef AP_DDS_ARM_SERVER_ENABLED
#define AP_DDS_ARM_SERVER_ENABLED 1
#endif

#ifndef AP_DDS_MODE_SWITCH_SERVER_ENABLED
#define AP_DDS_MODE_SWITCH_SERVER_ENABLED 1
#endif

// Whether to include Twist support
#define AP_DDS_NEEDS_TWIST AP_DDS_VEL_CTRL_ENABLED || AP_DDS_LOCAL_VEL_PUB_ENABLED

// Whether to include Transform support
#define AP_DDS_NEEDS_TRANSFORMS AP_DDS_DYNAMIC_TF_SUB || AP_DDS_STATIC_TF_PUB_ENABLED
#define AP_DDS_NEEDS_TRANSFORMS AP_DDS_DYNAMIC_TF_SUB_ENABLED || AP_DDS_STATIC_TF_PUB_ENABLED

#ifndef AP_DDS_DEFAULT_UDP_IP_ADDR
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
Expand Down
3 changes: 2 additions & 1 deletion libraries/AP_HAL_ChibiOS/hwdef/ZeroOneX6/README.md
Original file line number Diff line number Diff line change
@@ -1,6 +1,7 @@
## ZeroOneX6 Flight Controller
The ZeroOne X6 is a flight controller manufactured by ZeroOne, which is based on the open-source FMU v6X architecture and Pixhawk Autopilot Bus open source specifications.
![ZeroOneX6](ZeroOneX6.png "ZeroOneX6")
![Uploading ZeroOneX6.jpg…]()


## Features:
- Separate flight control core design.
Expand Down
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Loading

0 comments on commit e404681

Please sign in to comment.