Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-20 11:23:06 +08:00
Code Issues Actions 21 Packages Projects Releases Wiki Activity

mavlink: moved to ModuleParams

Browse Source 
Signed-off-by: Roman <bapstroman@gmail.com>
This commit is contained in:
Roman
2018-08-28 21:25:41 +02:00
committed by Daniel Agar
parent d131fea899
commit 844671ed6d
2 changed files with 60 additions and 109 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_main.cpp
+43 -87
View File
@@ -189,6 +189,7 @@ static void usage();
bool Mavlink::_boot_complete = false;
Mavlink::Mavlink() :
ModuleParams(nullptr),
_device_name("/dev/ttyS1"),
_task_should_exit(false),
next(nullptr),
@@ -201,8 +202,6 @@ Mavlink::Mavlink() :
_mavlink_status{},
_hil_enabled(false),
_generate_rc(false),
_use_hil_gps(false),
_forward_externalsp(false),
_is_usb_uart(false),
_wait_to_transmit(false),
_received_messages(false),
@@ -214,7 +213,6 @@ Mavlink::Mavlink() :
_mavlink_ulog_stop_requested(false),
_mode(MAVLINK_MODE_NORMAL),
_channel(MAVLINK_COMM_0),
_radio_id(0),
_logbuffer(5, sizeof(mavlink_log_s)),
_receive_thread{},
_forwarding_on(false),
@@ -256,16 +254,6 @@ Mavlink::Mavlink() :
_message_buffer {},
_message_buffer_mutex {},
_send_mutex {},
_param_initialized(false),
_broadcast_mode(Mavlink::BROADCAST_MODE_OFF),
_param_system_id(PARAM_INVALID),
_param_component_id(PARAM_INVALID),
_param_radio_id(PARAM_INVALID),
_param_system_type(PARAM_INVALID),
_param_use_hil_gps(PARAM_INVALID),
_param_forward_externalsp(PARAM_INVALID),
_param_broadcast(PARAM_INVALID),
_system_type(0),
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el"))
@@ -315,6 +303,23 @@ Mavlink::Mavlink() :
}
_tstatus.type = telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_GENERIC;
// initialise parameter cache
mavlink_update_parameters();
// save the current system- and component ID because we don't allow them to change during operation
int sys_id = _param_system_id.get();
if (sys_id > 0 && sys_id < 255) {
mavlink_system.sysid = sys_id;
}
int comp_id = _param_component_id.get();
if (comp_id > 0 && comp_id < 255) {
mavlink_system.compid = comp_id;
}
}
Mavlink::~Mavlink()
@@ -342,6 +347,24 @@ Mavlink::~Mavlink()
}
}
void Mavlink::mavlink_update_parameters()
{
updateParams();
int32_t proto = _param_mav_proto_version.get();
if (_protocol_version_switch != proto) {
_protocol_version_switch = proto;
set_proto_version(proto);
}
if (_param_system_type.get() < 0 || _param_system_type.get() >= MAV_TYPE_ENUM_END) {
_param_system_type.set(0);
_param_system_type.commit_no_notification();
PX4_ERR("MAV_TYPE parameter invalid, resetting to 0.");
}
}
void
Mavlink::set_proto_version(unsigned version)
{
@@ -565,70 +588,6 @@ Mavlink::get_channel()
return _channel;
}
void Mavlink::mavlink_update_system()
{
if (!_param_initialized) {
_param_system_id = param_find("MAV_SYS_ID");
_param_component_id = param_find("MAV_COMP_ID");
_param_proto_ver = param_find("MAV_PROTO_VER");
_param_radio_id = param_find("MAV_RADIO_ID");
_param_system_type = param_find("MAV_TYPE");
_param_use_hil_gps = param_find("MAV_USEHILGPS");
_param_forward_externalsp = param_find("MAV_FWDEXTSP");
_param_broadcast = param_find("MAV_BROADCAST");
}
/* update system and component id */
int32_t system_id;
param_get(_param_system_id, &system_id);
int32_t component_id;
param_get(_param_component_id, &component_id);
int32_t proto = 0;
param_get(_param_proto_ver, &proto);
if (_protocol_version_switch != proto) {
_protocol_version_switch = proto;
set_proto_version(proto);
}
param_get(_param_radio_id, &_radio_id);
/* only allow system ID and component ID updates
* after reboot - not during operation */
if (!_param_initialized) {
if (system_id > 0 && system_id < 255) {
mavlink_system.sysid = system_id;
}
if (component_id > 0 && component_id < 255) {
mavlink_system.compid = component_id;
}
_param_initialized = true;
}
int32_t system_type;
param_get(_param_system_type, &system_type);
if (system_type >= 0 && system_type < MAV_TYPE_ENUM_END) {
_system_type = system_type;
}
int32_t use_hil_gps;
param_get(_param_use_hil_gps, &use_hil_gps);
_use_hil_gps = (bool)use_hil_gps;
int32_t forward_externalsp;
param_get(_param_forward_externalsp, &forward_externalsp);
param_get(_param_broadcast, &_broadcast_mode);
_forward_externalsp = (bool)forward_externalsp;
}
int Mavlink::get_system_id()
{
return mavlink_system.sysid;
@@ -2105,9 +2064,6 @@ Mavlink::task_main(int argc, char *argv[])
pthread_mutex_init(&_message_buffer_mutex, nullptr);
}
/* Initialize system properties */
mavlink_update_system();
MavlinkOrbSubscription *cmd_sub = add_orb_subscription(ORB_ID(vehicle_command), 0, true);
MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update));
uint64_t param_time = 0;
@@ -2193,8 +2149,7 @@ Mavlink::task_main(int argc, char *argv[])
update_rate_mult();
if (param_sub->update(&param_time, nullptr)) {
/* parameters updated */
mavlink_update_system();
mavlink_update_parameters();
}
check_radio_config();
@@ -2557,7 +2512,8 @@ void Mavlink::publish_telemetry_status()
void Mavlink::check_radio_config()
{
/* radio config check */
if (_uart_fd >= 0 && _radio_id != 0 && _tstatus.type == telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO) {
if (_uart_fd >= 0 && _param_radio_id.get() != 0
&& _tstatus.type == telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO) {
/* request to configure radio and radio is present */
FILE *fs = fdopen(_uart_fd, "w");
@@ -2567,9 +2523,9 @@ void Mavlink::check_radio_config()
fprintf(fs, "+++\n");
usleep(1200000);
if (_radio_id > 0) {
if (_param_radio_id.get() > 0) {
/* set channel */
fprintf(fs, "ATS3=%u\n", _radio_id);
fprintf(fs, "ATS3=%u\n", _param_radio_id.get());
usleep(200000);
} else {
@@ -2600,8 +2556,8 @@ void Mavlink::check_radio_config()
}
/* reset param and save */
_radio_id = 0;
param_set_no_notification(_param_radio_id, &_radio_id);
_param_radio_id.set(0);
_param_radio_id.commit_no_notification();
}
}
src/modules/mavlink/mavlink_main.h
+17 -22
View File
@@ -43,6 +43,7 @@
#pragma once
#include <px4_posix.h>
#include <px4_module_params.h>
#include <stdbool.h>
#ifdef __PX4_NUTTX
@@ -86,7 +87,7 @@ using namespace time_literals;
#define HASH_PARAM "_HASH_CHECK"
class Mavlink
class Mavlink : public ModuleParams
{
public:
@@ -228,9 +229,9 @@ public:
bool get_hil_enabled() { return _hil_enabled; }
bool get_use_hil_gps() { return _use_hil_gps; }
bool get_use_hil_gps() { return _param_use_hil_gps.get(); }
bool get_forward_externalsp() { return _forward_externalsp; }
bool get_forward_externalsp() { return _param_forward_externalsp.get(); }
bool get_flow_control_enabled() { return _flow_control_mode; }
@@ -238,7 +239,7 @@ public:
bool is_connected() { return ((_tstatus.heartbeat_time > 0) && (hrt_absolute_time() - _tstatus.heartbeat_time < 3_s)); }
bool broadcast_enabled() { return _broadcast_mode == BROADCAST_MODE_ON; }
bool broadcast_enabled() { return _param_broadcast_mode.get() == BROADCAST_MODE_ON; }
/**
* Set the boot complete flag on all instances
@@ -426,7 +427,7 @@ public:
ringbuffer::RingBuffer *get_logbuffer() { return &_logbuffer; }
unsigned get_system_type() { return _system_type; }
unsigned get_system_type() { return _param_system_type.get(); }
Protocol get_protocol() { return _protocol; }
@@ -523,8 +524,6 @@ private:
/* states */
bool _hil_enabled; /**< Hardware In the Loop mode */
bool _generate_rc; /**< Generate RC messages from manual input MAVLink messages */
bool _use_hil_gps; /**< Accept GPS HIL messages (for example from an external motion capturing system to fake indoor gps) */
bool _forward_externalsp; /**< Forward external setpoint messages to controllers directly if in offboard mode */
bool _is_usb_uart; /**< Port is USB */
bool _wait_to_transmit; /**< Wait to transmit until received messages. */
bool _received_messages; /**< Whether we've received valid mavlink messages. */
@@ -541,7 +540,6 @@ private:
MAVLINK_MODE _mode;
mavlink_channel_t _channel;
int32_t _radio_id;
ringbuffer::RingBuffer _logbuffer;
@@ -621,23 +619,20 @@ private:
pthread_mutex_t _message_buffer_mutex;
pthread_mutex_t _send_mutex;
bool _param_initialized;
int32_t _broadcast_mode;
param_t _param_system_id;
param_t _param_component_id;
param_t _param_proto_ver;
param_t _param_radio_id;
param_t _param_system_type;
param_t _param_use_hil_gps;
param_t _param_forward_externalsp;
param_t _param_broadcast;
unsigned _system_type;
DEFINE_PARAMETERS(
(ParamInt<px4::params::MAV_SYS_ID>) _param_system_id,
(ParamInt<px4::params::MAV_COMP_ID>) _param_component_id,
(ParamInt<px4::params::MAV_PROTO_VER>) _param_mav_proto_version,
(ParamInt<px4::params::MAV_RADIO_ID>) _param_radio_id,
(ParamInt<px4::params::MAV_TYPE>) _param_system_type,
(ParamBool<px4::params::MAV_USEHILGPS>) _param_use_hil_gps,
(ParamBool<px4::params::MAV_FWDEXTSP>) _param_forward_externalsp,
(ParamInt<px4::params::MAV_BROADCAST>) _param_broadcast_mode
)
perf_counter_t _loop_perf; /**< loop performance counter */
void mavlink_update_system();
void mavlink_update_parameters();
int mavlink_open_uart(int baudrate, const char *uart_name, bool force_flow_control);