diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 852351f46f..730871ecd2 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -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(¶m_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(); } } diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 37d9129de6..050b758388 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -43,6 +43,7 @@ #pragma once #include +#include #include #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) _param_system_id, + (ParamInt) _param_component_id, + (ParamInt) _param_mav_proto_version, + (ParamInt) _param_radio_id, + (ParamInt) _param_system_type, + (ParamBool) _param_use_hil_gps, + (ParamBool) _param_forward_externalsp, + (ParamInt) _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);