diff --git a/src/modules/airship_att_control/airship_att_control.hpp b/src/modules/airship_att_control/airship_att_control.hpp index dbb876ec62..b6ff54a464 100644 --- a/src/modules/airship_att_control/airship_att_control.hpp +++ b/src/modules/airship_att_control/airship_att_control.hpp @@ -82,9 +82,9 @@ private: void publishTorqueSetpoint(const hrt_abstime ×tamp_sample); void publishThrustSetpoint(const hrt_abstime ×tamp_sample); - uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; /**< parameter updates subscription */ - uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */ - uORB::Subscription _manual_control_sp_sub{ORB_ID(manual_control_setpoint)}; /**< manual control setpoint subscription */ + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)}; @@ -92,10 +92,10 @@ private: uORB::Publication _vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)}; uORB::Publication _vehicle_torque_setpoint_pub{ORB_ID(vehicle_torque_setpoint)}; - manual_control_setpoint_s _manual_control_sp {}; /**< manual control setpoint */ - vehicle_status_s _vehicle_status {}; /**< vehicle status */ - actuator_controls_s _actuator_controls {}; /**< actuator controls */ + manual_control_setpoint_s _manual_control_setpoint{}; + vehicle_status_s _vehicle_status{}; + actuator_controls_s _actuator_controls{}; - perf_counter_t _loop_perf; /**< loop performance counter */ + perf_counter_t _loop_perf; }; diff --git a/src/modules/airship_att_control/airship_att_control_main.cpp b/src/modules/airship_att_control/airship_att_control_main.cpp index e76cdf582c..d66f82840e 100644 --- a/src/modules/airship_att_control/airship_att_control_main.cpp +++ b/src/modules/airship_att_control/airship_att_control_main.cpp @@ -91,9 +91,9 @@ AirshipAttitudeControl::publish_actuator_controls() } else { _actuator_controls.control[0] = 0.0f; - _actuator_controls.control[1] = _manual_control_sp.x; - _actuator_controls.control[2] = _manual_control_sp.r; - _actuator_controls.control[3] = _manual_control_sp.z; + _actuator_controls.control[1] = _manual_control_setpoint.x; + _actuator_controls.control[2] = _manual_control_setpoint.r; + _actuator_controls.control[3] = _manual_control_setpoint.z; } // note: _actuator_controls.timestamp_sample is set in AirshipAttitudeControl::Run() @@ -150,7 +150,7 @@ AirshipAttitudeControl::Run() publishThrustSetpoint(angular_velocity.timestamp_sample); /* check for updates in manual control topic */ - _manual_control_sp_sub.update(&_manual_control_sp); + _manual_control_setpoint_sub.update(&_manual_control_setpoint); /* check for updates in vehicle status topic */ _vehicle_status_sub.update(&_vehicle_status); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index af3fae9163..dc89b44ae8 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -2129,22 +2129,22 @@ MavlinkReceiver::handle_message_rc_channels_override(mavlink_message_t *msg) void MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) { - mavlink_manual_control_t man; - mavlink_msg_manual_control_decode(msg, &man); + mavlink_manual_control_t mavlink_manual_control; + mavlink_msg_manual_control_decode(msg, &mavlink_manual_control); // Check target - if (man.target != 0 && man.target != _mavlink->get_system_id()) { + if (mavlink_manual_control.target != 0 && mavlink_manual_control.target != _mavlink->get_system_id()) { return; } - manual_control_setpoint_s manual{}; - manual.x = man.x / 1000.0f; - manual.y = man.y / 1000.0f; - manual.r = man.r / 1000.0f; - manual.z = man.z / 1000.0f; - manual.data_source = manual_control_setpoint_s::SOURCE_MAVLINK_0 + _mavlink->get_instance_id(); - manual.timestamp = manual.timestamp_sample = hrt_absolute_time(); - _manual_control_input_pub.publish(manual); + manual_control_setpoint_s manual_control_setpoint{}; + manual_control_setpoint.x = mavlink_manual_control.x / 1000.0f; + manual_control_setpoint.y = mavlink_manual_control.y / 1000.0f; + manual_control_setpoint.r = mavlink_manual_control.r / 1000.0f; + manual_control_setpoint.z = mavlink_manual_control.z / 1000.0f; + manual_control_setpoint.data_source = manual_control_setpoint_s::SOURCE_MAVLINK_0 + _mavlink->get_instance_id(); + manual_control_setpoint.timestamp = manual_control_setpoint.timestamp_sample = hrt_absolute_time(); + _manual_control_input_pub.publish(manual_control_setpoint); } void