gg/fts-can
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

ground FTS pairing WIP

Browse Source
This commit is contained in:
neo 2024-06-13 19:24:20 +02:00
parent 3e0db221ac
commit 2cef6c462b
13 changed files with 209 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
boards/px4/fts-can/init/rc.board_defaults
View File

@ -10,7 +10,7 @@ param set-default SYS_AUTOSTART 1
param set-default MAV_PROTO_VER 2
# Log from boot until shutdown
param set-default SDLOG_MODE 2
#param set-default SDLOG_MODE 2
param set-default SYS_USE_IO 0
param set-default SENS_IMU_MODE 0

1
boards/px4/fts-can/init/rc.board_sensors
View File

@ -3,4 +3,3 @@
# PX4 FTS-CAN specific board sensors init
#------------------------------------------------------------------------------
board_adc start
fts_control start

3
msg/CMakeLists.txt
View File

@ -68,6 +68,7 @@ set(msg_files
DifferentialPressure.msg
DistanceSensor.msg
Ekf2Timestamps.msg
EmergencyCommand.msg
EscReport.msg
EscStatus.msg
EstimatorAidSource1d.msg
@ -103,6 +104,8 @@ set(msg_files
GpsInjectData.msg
GimbalControls.msg
Gripper.msg
GroundFtsMessage.msg
GroundFtsResponse.msg
HealthReport.msg
HeaterStatus.msg
HomePosition.msg

6
msg/EmergencyCommand.msg Normal file
View File

@ -0,0 +1,6 @@
uint64 timestamp # time since system start (microseconds)
uint8 target_system
uint8 target_component
uint8 emergency_command
uint32 param

4
msg/GroundFtsMessage.msg Normal file
View File

@ -0,0 +1,4 @@
uint64 timestamp # time since system start (microseconds)
uint8 message
uint32 fts_uid

4
msg/GroundFtsResponse.msg Normal file
View File

@ -0,0 +1,4 @@
uint64 timestamp # time since system start (microseconds)
uint8 message
uint32 fts_uid

48
src/modules/fts_control/fts_control.cpp
View File

@ -1,11 +1,9 @@
/**
*
* Controller of Nimbus Charging FTS
*
* @author Flavio Tonelli <f.tonelli@nimbus.aero>
*/
#include "fts_control.hpp"
#include "px4_platform_common/board_common.h"
#include "../mavlink/streams/GROUND_FTS_RESPONSE.hpp"
FTSController::FTSController() :
ModuleParams(nullptr),
@ -30,6 +28,12 @@ FTSController::init()
PX4_WARN("fts_control failed to get necessary parameters.");
return false;
}
uint8_t guid[PX4_GUID_BYTE_LENGTH];
board_get_px4_guid(guid);
fts_id.uidBytes[0]=guid[0];
fts_id.uidBytes[1]=guid[1];
fts_id.uidBytes[2]=guid[2];
fts_id.uidBytes[3]=guid[3];
// run every 1s
ScheduleOnInterval(1000_ms);
@ -73,6 +77,26 @@ FTSController::Run()
}
}
// Update GROUND-FTS status
if(_ground_fts_message_sub.updated()) {
_ground_fts_message_sub.copy(&_ground_fts_message);
if (_ground_fts_message.message == PING) {
//PING COMMAND
if(_ground_fts_message.fts_uid == fts_id.uid){
_ground_fts_response_pub.publish(_ground_fts_response);
}
}else if(_ground_fts_message.message == PAIR){
//PAIR COMMAND
_ground_fts_response.message = PAIR;
_ground_fts_response.fts_uid = fts_id.uid;
_ground_fts_response_pub.publish(_ground_fts_response);
MavlinkStreamGroundftsResponse.send();
}
}
// Publish FTS controller state
_fts_state.timestamp = hrt_absolute_time();
@ -170,6 +194,16 @@ FTSController::custom_command(int argc, char *argv[])
return 0;
}
if (!strcmp(argv[0], "arm")) {
get_instance()->arm();
return 0;
}
if (!strcmp(argv[0], "disarm")) {
get_instance()->disarm();
return 0;
}
return print_usage("unknown command");
}
@ -201,6 +235,8 @@ FTSController::print_usage(const char *reason)
PRINT_MODULE_USAGE_NAME("fts_controller", "system");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_COMMAND("status");
PRINT_MODULE_USAGE_COMMAND("arm");
PRINT_MODULE_USAGE_COMMAND("disarm");
PRINT_MODULE_USAGE_COMMAND("fire");
PRINT_MODULE_USAGE_COMMAND("reset");
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();

34
src/modules/fts_control/fts_control.hpp
View File

@ -15,12 +15,33 @@
#include <uORB/Subscription.hpp>
#include <uORB/topics/fts_state.h>
#include <uORB/topics/auto_fts_message.h>
#include <uORB/topics/ground_fts_message.h>
#include <uORB/topics/ground_fts_response.h>
#include <uORB/topics/emergency_command.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/vehicle_local_position.h>
#include <drivers/fts/fts.h>
using namespace time_literals;
// Helper union to construct the UID field of the Mavlink message.
union FTSIDUnion{
uint32_t uid;
uint8_t uidBytes[4];
};
typedef enum FTS_MESSAGE_TYPE
{
PING=1, /* Send from the host to the FTS. | */
PONG=2, /* Send from the FTS to the host. | */
PAIR=3, /* A Pairing operation. Send from host to FTS means the host want to start a pairing procedure.
The FTS will ignore the FTS_UID field check when it receives this message and send back this
same message as a response. Then the host shall register the FTS_UID field in the response.
| */
FTS_MESSAGE_TYPE_ENUM_END=4, /* | */
} FTS_MESSAGE_TYPE;
class FTSController : public ModuleBase<FTSController>, public ModuleParams, public px4::ScheduledWorkItem
{
public:
@ -56,18 +77,25 @@ private:
perf_counter_t _cycle_perf;
perf_counter_t _interval_perf;
uORB::SubscriptionData<vehicle_status_s> _vehicle_status_sub{ORB_ID(vehicle_status)};
uORB::SubscriptionData<auto_fts_message_s> _auto_fts_message_sub{ORB_ID(auto_fts_message)};
uORB::SubscriptionData<home_position_s> _home_position_sub{ORB_ID(home_position)};
uORB::SubscriptionData<vehicle_local_position_s> _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
uORB::SubscriptionData<ground_fts_message_s> _ground_fts_message_sub{ORB_ID(ground_fts_message)};
uORB::SubscriptionData<emergency_command_s> _emergency_command_sub{ORB_ID(emergency_command)};
uORB::Publication<fts_state_s> _fts_state_pub{ORB_ID(fts_state)};
uORB::Publication<ground_fts_response_s> _ground_fts_response_pub{ORB_ID(ground_fts_response)};
FTS fts_drv;
hrt_abstime _last_fts_state_update_time{0};
fts_state_s _fts_state{};
auto_fts_message_s _auto_fts_message{};
ground_fts_message_s _ground_fts_message{};
ground_fts_response_s _ground_fts_response{};
emergency_command_s _emergency_command{};
uint64_t _fts_comm_timeout_threshold_us{10_s}; // Currently the box emits state at 3 Hz
// Global variable storing the UID of the chip.
union FTSIDUnion fts_id;
};

1
src/modules/mavlink/mavlink_main.cpp
View File

@ -1844,6 +1844,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
break;
//MODE 14 MFTS TO GFTS
case MAVLINK_MODE_MFTS_GFTS:
configure_stream_local("GROUND_FTS_RESPONSE", 0.0f);
break;
//MODE 15 AFTS TO MFTS
case MAVLINK_MODE_AFTS_MFTS:

6
src/modules/mavlink/mavlink_messages.cpp
View File

@ -82,6 +82,7 @@
#include "streams/GPS_RAW_INT.hpp"
#include "streams/GPS_RTCM_DATA.hpp"
#include "streams/GPS_STATUS.hpp"
#include "streams/GROUND_FTS_RESPONSE.hpp"
#include "streams/HEARTBEAT.hpp"
#include "streams/HIGHRES_IMU.hpp"
#include "streams/HIL_ACTUATOR_CONTROLS.hpp"
@ -485,8 +486,11 @@ static const StreamListItem streams_list[] = {
create_stream_list_item<MavlinkStreamUavionixADSBOutDynamic>(),
#endif // UAVIONIX_ADSB_OUT_DYNAMIC_HPP
#if defined(AUTO_FTS_MESSAGE_HPP)
create_stream_list_item<MavlinkStreamAftsStatus>()
create_stream_list_item<MavlinkStreamAftsStatus>(),
#endif // AUTO_FTS_MESSAGE_HPP
#if defined(GROUND_FTS_RESPONSE_HPP)
create_stream_list_item<MavlinkStreamGroundftsResponse>()
#endif // GROUND_FTS_RESPONSE_HPP
};
const char *get_stream_name(const uint16_t msg_id)

42
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -316,6 +316,12 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
case MAVLINK_MSG_ID_AUTO_FTS_MESSAGE:
handle_message_auto_fts_message(msg);
break;
case MAVLINK_MSG_ID_FTS_MESSAGE:
handle_message_ground_fts_message(msg);
break;
case MAVLINK_MSG_ID_EMERGENCY_COMMAND:
handle_message_emergency_command(msg);
break;
default:
break;
@ -3066,16 +3072,34 @@ MavlinkReceiver::handle_message_gimbal_device_attitude_status(mavlink_message_t
void
MavlinkReceiver::handle_message_auto_fts_message(mavlink_message_t *msg)
{
mavlink_auto_fts_message_t auto_fts_message_msg;
mavlink_msg_auto_fts_message_decode(msg, &auto_fts_message_msg);
mavlink_auto_fts_message_t auto_fts_message_msg;
mavlink_msg_auto_fts_message_decode(msg, &auto_fts_message_msg);
auto_fts_message_s auto_fts_message{};
auto_fts_message.timestamp = hrt_absolute_time();
auto_fts_message.config_status=auto_fts_message_msg.config_status;
auto_fts_message.geofence_loaded=auto_fts_message_msg.geofence_loaded;
auto_fts_message.gps_lock=auto_fts_message_msg.gps_lock;
auto_fts_message.geofence_violated=auto_fts_message_msg.geofence_violated;
_auto_fts_message_pub.publish(auto_fts_message);
}
void
MavlinkReceiver::handle_message_ground_fts_message(mavlink_message_t *msg)
{
mavlink_fts_message_t ground_fts_message_mavlink;
mavlink_msg_fts_message_decode(msg, &ground_fts_message_mavlink);
ground_fts_message_s ground_fts_message{};
ground_fts_message.timestamp = hrt_absolute_time();
ground_fts_message.message = ground_fts_message_mavlink.message;
_ground_fts_message_pub.publish(ground_fts_message);
}
void
MavlinkReceiver::handle_message_emergency_command(mavlink_message_t *msg) {
auto_fts_message_s auto_fts_message{};
auto_fts_message.timestamp = hrt_absolute_time();
auto_fts_message.config_status=auto_fts_message_msg.config_status;
auto_fts_message.geofence_loaded=auto_fts_message_msg.geofence_loaded;
auto_fts_message.gps_lock=auto_fts_message_msg.gps_lock;
auto_fts_message.geofence_violated=auto_fts_message_msg.geofence_violated;
_auto_fts_message_pub.publish(auto_fts_message);
}
void

6
src/modules/mavlink/mavlink_receiver.h
View File

@ -71,6 +71,7 @@
#include <uORB/topics/collision_report.h>
#include <uORB/topics/differential_pressure.h>
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/emergency_command.h>
#include <uORB/topics/follow_target.h>
#include <uORB/topics/generator_status.h>
#include <uORB/topics/gimbal_manager_set_attitude.h>
@ -84,6 +85,7 @@
#include <uORB/topics/landing_target_pose.h>
#include <uORB/topics/log_message.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/ground_fts_message.h>
#include <uORB/topics/mavlink_tunnel.h>
#include <uORB/topics/obstacle_distance.h>
#include <uORB/topics/offboard_control_mode.h>
@ -166,6 +168,7 @@ private:
void handle_message_command_int(mavlink_message_t *msg);
void handle_message_command_long(mavlink_message_t *msg);
void handle_message_distance_sensor(mavlink_message_t *msg);
void handle_message_emergency_command(mavlink_message_t *msg);
void handle_message_follow_target(mavlink_message_t *msg);
void handle_message_generator_status(mavlink_message_t *msg);
void handle_message_set_gps_global_origin(mavlink_message_t *msg);
@ -178,6 +181,7 @@ private:
void handle_message_landing_target(mavlink_message_t *msg);
void handle_message_logging_ack(mavlink_message_t *msg);
void handle_message_manual_control(mavlink_message_t *msg);
void handle_message_ground_fts_message(mavlink_message_t *msg);
void handle_message_obstacle_distance(mavlink_message_t *msg);
void handle_message_odometry(mavlink_message_t *msg);
void handle_message_onboard_computer_status(mavlink_message_t *msg);
@ -296,6 +300,7 @@ private:
uORB::Publication<cellular_status_s> _cellular_status_pub{ORB_ID(cellular_status)};
uORB::Publication<collision_report_s> _collision_report_pub{ORB_ID(collision_report)};
uORB::Publication<differential_pressure_s> _differential_pressure_pub{ORB_ID(differential_pressure)};
uORB::Publication<emergency_command_s> _emergency_command_pub{ORB_ID(emergency_command)};
uORB::Publication<follow_target_s> _follow_target_pub{ORB_ID(follow_target)};
uORB::Publication<gimbal_manager_set_attitude_s> _gimbal_manager_set_attitude_pub{ORB_ID(gimbal_manager_set_attitude)};
uORB::Publication<gimbal_manager_set_manual_control_s> _gimbal_manager_set_manual_control_pub{ORB_ID(gimbal_manager_set_manual_control)};
@ -304,6 +309,7 @@ private:
uORB::Publication<irlock_report_s> _irlock_report_pub{ORB_ID(irlock_report)};
uORB::Publication<landing_target_pose_s> _landing_target_pose_pub{ORB_ID(landing_target_pose)};
uORB::Publication<log_message_s> _log_message_pub{ORB_ID(log_message)};
uORB::Publication<ground_fts_message_s> _ground_fts_message_pub{ORB_ID(ground_fts_message)};
uORB::Publication<mavlink_tunnel_s> _mavlink_tunnel_pub{ORB_ID(mavlink_tunnel)};
uORB::Publication<obstacle_distance_s> _obstacle_distance_pub{ORB_ID(obstacle_distance)};
uORB::Publication<offboard_control_mode_s> _offboard_control_mode_pub{ORB_ID(offboard_control_mode)};

70
src/modules/mavlink/streams/GROUND_FTS_RESPONSE.hpp Normal file
View File

@ -0,0 +1,70 @@
#ifndef GROUND_FTS_RESPONSE_HPP
#define GROUND_FTS_RESPONSE_HPP
#include <lib/geo/geo.h>
#include <lib/mathlib/mathlib.h>
#include <lib/matrix/matrix/math.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/ground_fts_response.h>
#include <px4_platform_common/events.h>
class MavlinkStreamGroundftsResponse : public MavlinkStream
{
public:
static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamGroundftsResponse(mavlink); }
static constexpr const char *get_name_static() { return "GROUND_FTS_RESPONSE"; }
static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_FTS_MESSAGE; }
const char *get_name() const override { return get_name_static(); }
uint16_t get_id() override { return get_id_static(); }
uORB::Subscription _ground_fts_response_sub{ORB_ID(ground_fts_response)};
hrt_abstime _last_reset_time{0};
hrt_abstime _last_update_time{0};
unsigned get_size() override
{
return MAVLINK_MSG_ID_FTS_MESSAGE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
}
bool const_rate() override { return true; }
private:
explicit MavlinkStreamGroundftsResponse(Mavlink *mavlink) :
MavlinkStream(mavlink)
{
}
bool send() override
{
// only send the struct if transmitting is allowed
// this assures that the stream timer is only reset when actually a message is transmitted
if (_mavlink->should_transmit()) {
mavlink_fts_message_t msg{};
ground_fts_response_s response;
if (_ground_fts_response_sub.update(&response)) {
msg.message = response.message;
msg.FTS_UID = response.fts_uid;
return true;
}
return false;
mavlink_msg_fts_message_send_struct(_mavlink->get_channel(), &msg);
return true;
}
return false;
}
};
#endif