MultiCopterRateControl: refactor setpoint naming

src/modules/mc_rate_control/MulticopterRateControl.cpp

@@ -180,31 +180,33 @@ MulticopterRateControl::Run()
 					math::superexpo(-manual_control_setpoint.x, _param_mc_acro_expo.get(), _param_mc_acro_supexpo.get()),
 					math::superexpo(manual_control_setpoint.r, _param_mc_acro_expo_y.get(), _param_mc_acro_supexpoy.get())};
 
-				_rates_sp = man_rate_sp.emult(_acro_rate_max);
+				_rates_setpoint = man_rate_sp.emult(_acro_rate_max);
-				_thrust_sp = math::constrain(manual_control_setpoint.z, 0.0f, 1.0f);
+				_thrust_setpoint = math::constrain(manual_control_setpoint.z, 0.0f, 1.0f);
 
 				// publish rate setpoint
-				vehicle_rates_setpoint_s v_rates_sp{};
+				vehicle_rates_setpoint_s v_rates_setpoint{};
-				v_rates_sp.roll = _rates_sp(0);
+				v_rates_setpoint.roll = _rates_setpoint(0);
-				v_rates_sp.pitch = _rates_sp(1);
+				v_rates_setpoint.pitch = _rates_setpoint(1);
-				v_rates_sp.yaw = _rates_sp(2);
+				v_rates_setpoint.yaw = _rates_setpoint(2);
-				v_rates_sp.thrust_body[0] = 0.0f;
+				v_rates_setpoint.thrust_body[0] = 0.0f;
-				v_rates_sp.thrust_body[1] = 0.0f;
+				v_rates_setpoint.thrust_body[1] = 0.0f;
-				v_rates_sp.thrust_body[2] = -_thrust_sp;
+				v_rates_setpoint.thrust_body[2] = _thrust_setpoint;
-				v_rates_sp.timestamp = hrt_absolute_time();
+				v_rates_setpoint.timestamp = hrt_absolute_time();
-
+
-				_v_rates_sp_pub.publish(v_rates_sp);
+				_v_rates_setpoint_pub.publish(v_rates_setpoint);
 			}
 
 		} else {
 			// use rates setpoint topic
-			vehicle_rates_setpoint_s v_rates_sp;
+			vehicle_rates_setpoint_s vehicle_rates_setpoint;
-
+
-			if (_v_rates_sp_sub.update(&v_rates_sp)) {
+			if (_v_rates_setpoint_sub.update(&vehicle_rates_setpoint)) {
-				_rates_sp(0) = PX4_ISFINITE(v_rates_sp.roll)  ? v_rates_sp.roll  : rates(0);
+				if (_v_rates_setpoint_sub.copy(&vehicle_rates_setpoint)) {
-				_rates_sp(1) = PX4_ISFINITE(v_rates_sp.pitch) ? v_rates_sp.pitch : rates(1);
+					_rates_setpoint(0) = PX4_ISFINITE(vehicle_rates_setpoint.roll)  ? vehicle_rates_setpoint.roll  : rates(0);
-				_rates_sp(2) = PX4_ISFINITE(v_rates_sp.yaw)   ? v_rates_sp.yaw   : rates(2);
+					_rates_setpoint(1) = PX4_ISFINITE(vehicle_rates_setpoint.pitch) ? vehicle_rates_setpoint.pitch : rates(1);
-				_thrust_sp = -v_rates_sp.thrust_body[2];
+					_rates_setpoint(2) = PX4_ISFINITE(vehicle_rates_setpoint.yaw)   ? vehicle_rates_setpoint.yaw   : rates(2);
+					_thrust_setpoint = -vehicle_rates_setpoint.thrust_body[2];
+				}
 			}
 		}
 
@@ -239,7 +241,7 @@ MulticopterRateControl::Run()
 			}
 
 			// run rate controller
-			const Vector3f att_control = _rate_control.update(rates, _rates_sp, angular_accel, dt, _maybe_landed || _landed);
+			const Vector3f att_control = _rate_control.update(rates, _rates_setpoint, angular_accel, dt, _maybe_landed || _landed);
 
 			// publish rate controller status
 			rate_ctrl_status_s rate_ctrl_status{};
@@ -252,13 +254,13 @@ MulticopterRateControl::Run()
 			actuators.control[actuator_controls_s::INDEX_ROLL] = PX4_ISFINITE(att_control(0)) ? att_control(0) : 0.0f;
 			actuators.control[actuator_controls_s::INDEX_PITCH] = PX4_ISFINITE(att_control(1)) ? att_control(1) : 0.0f;
 			actuators.control[actuator_controls_s::INDEX_YAW] = PX4_ISFINITE(att_control(2)) ? att_control(2) : 0.0f;
-			actuators.control[actuator_controls_s::INDEX_THROTTLE] = PX4_ISFINITE(_thrust_sp) ? _thrust_sp : 0.0f;
+			actuators.control[actuator_controls_s::INDEX_THROTTLE] = PX4_ISFINITE(_thrust_setpoint) ? _thrust_setpoint : 0.0f;
 			actuators.control[actuator_controls_s::INDEX_LANDING_GEAR] = _landing_gear;
 			actuators.timestamp_sample = angular_velocity.timestamp_sample;
 
 			if (!_vehicle_status.is_vtol) {
 				publishTorqueSetpoint(att_control, angular_velocity.timestamp_sample);
-				publishThrustSetpoint(angular_velocity.timestamp_sample);
+				publishThrustSetpoint(_thrust_setpoint, angular_velocity.timestamp_sample);
 			}
 
 			// scale effort by battery status if enabled
@@ -298,26 +300,26 @@ MulticopterRateControl::Run()
 
 void MulticopterRateControl::publishTorqueSetpoint(const Vector3f &torque_sp, const hrt_abstime &timestamp_sample)
 {
-	vehicle_torque_setpoint_s v_torque_sp = {};
+	vehicle_torque_setpoint_s vehicle_torque_setpoint{};
-	v_torque_sp.timestamp = hrt_absolute_time();
+	vehicle_torque_setpoint.timestamp = hrt_absolute_time();
-	v_torque_sp.timestamp_sample = timestamp_sample;
+	vehicle_torque_setpoint.timestamp_sample = timestamp_sample;
-	v_torque_sp.xyz[0] = (PX4_ISFINITE(torque_sp(0))) ? torque_sp(0) : 0.0f;
+	vehicle_torque_setpoint.xyz[0] = (PX4_ISFINITE(torque_sp(0))) ? torque_sp(0) : 0.0f;
-	v_torque_sp.xyz[1] = (PX4_ISFINITE(torque_sp(1))) ? torque_sp(1) : 0.0f;
+	vehicle_torque_setpoint.xyz[1] = (PX4_ISFINITE(torque_sp(1))) ? torque_sp(1) : 0.0f;
-	v_torque_sp.xyz[2] = (PX4_ISFINITE(torque_sp(2))) ? torque_sp(2) : 0.0f;
+	vehicle_torque_setpoint.xyz[2] = (PX4_ISFINITE(torque_sp(2))) ? torque_sp(2) : 0.0f;
-
+
-	_vehicle_torque_setpoint_pub.publish(v_torque_sp);
+	_vehicle_torque_setpoint_pub.publish(vehicle_torque_setpoint);
 }
 
-void MulticopterRateControl::publishThrustSetpoint(const hrt_abstime &timestamp_sample)
+void MulticopterRateControl::publishThrustSetpoint(const float thrust_setpoint, const hrt_abstime &timestamp_sample)
 {
-	vehicle_thrust_setpoint_s v_thrust_sp = {};
+	vehicle_thrust_setpoint_s vehicle_thrust_setpoint{};
-	v_thrust_sp.timestamp = hrt_absolute_time();
+	vehicle_thrust_setpoint.timestamp = hrt_absolute_time();
-	v_thrust_sp.timestamp_sample = timestamp_sample;
+	vehicle_thrust_setpoint.timestamp_sample = timestamp_sample;
-	v_thrust_sp.xyz[0] = 0.0f;
+	vehicle_thrust_setpoint.xyz[0] = 0.0f;
-	v_thrust_sp.xyz[1] = 0.0f;
+	vehicle_thrust_setpoint.xyz[1] = 0.0f;
-	v_thrust_sp.xyz[2] = PX4_ISFINITE(_thrust_sp) ? -_thrust_sp : 0.0f; // Z is Down
+	vehicle_thrust_setpoint.xyz[2] = PX4_ISFINITE(thrust_setpoint) ? -thrust_setpoint : 0.0f; // Z is Down
-
+
-	_vehicle_thrust_setpoint_pub.publish(v_thrust_sp);
+	_vehicle_thrust_setpoint_pub.publish(vehicle_thrust_setpoint);
 }
 
 void MulticopterRateControl::updateActuatorControlsStatus(const actuator_controls_s &actuators, float dt)

src/modules/mc_rate_control/MulticopterRateControl.hpp

@@ -94,7 +94,7 @@ private:
 	void updateActuatorControlsStatus(const actuator_controls_s &actuators, float dt);
 
 	void publishTorqueSetpoint(const matrix::Vector3f &torque_sp, const hrt_abstime &timestamp_sample);
-	void publishThrustSetpoint(const hrt_abstime &timestamp_sample);
+	void publishThrustSetpoint(const float thrust_setpoint, const hrt_abstime &timestamp_sample);
 
 	RateControl _rate_control; ///< class for rate control calculations
 
@@ -103,7 +103,7 @@ private:
 	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
 	uORB::Subscription _control_allocator_status_sub{ORB_ID(control_allocator_status)};
 	uORB::Subscription _v_control_mode_sub{ORB_ID(vehicle_control_mode)};
-	uORB::Subscription _v_rates_sp_sub{ORB_ID(vehicle_rates_setpoint)};
+	uORB::Subscription _v_rates_setpoint_sub{ORB_ID(vehicle_rates_setpoint)};
 	uORB::Subscription _vehicle_angular_acceleration_sub{ORB_ID(vehicle_angular_acceleration)};
 	uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
 	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
@@ -115,7 +115,7 @@ private:
 	uORB::Publication<actuator_controls_s>		_actuators_0_pub;
 	uORB::Publication<actuator_controls_status_s>	_actuator_controls_status_0_pub{ORB_ID(actuator_controls_status_0)};
 	uORB::PublicationMulti<rate_ctrl_status_s>	_controller_status_pub{ORB_ID(rate_ctrl_status)};
-	uORB::Publication<vehicle_rates_setpoint_s>	_v_rates_sp_pub{ORB_ID(vehicle_rates_setpoint)};
+	uORB::Publication<vehicle_rates_setpoint_s>	_v_rates_setpoint_pub{ORB_ID(vehicle_rates_setpoint)};
 	uORB::Publication<vehicle_thrust_setpoint_s>	_vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)};
 	uORB::Publication<vehicle_torque_setpoint_s>	_vehicle_torque_setpoint_pub{ORB_ID(vehicle_torque_setpoint)};
 
@@ -132,9 +132,9 @@ private:
 
 	perf_counter_t	_loop_perf;			/**< loop duration performance counter */
 
-	matrix::Vector3f _rates_sp;			/**< angular rates setpoint */
+	// keep setpoint values between updates
-
+	matrix::Vector3f _rates_setpoint{};
-	float		_thrust_sp{0.0f};		/**< thrust setpoint */
+	float _thrust_setpoint{0.0f};
 
 	hrt_abstime _last_run{0};