FailureDetector: check if ESCs have current

And increase the timeout to 300ms, as some ESCs only update with 10Hz.

Tools/px_uploader.py

@@ -136,8 +136,11 @@ class firmware(object):
 
         self.image = bytearray(zlib.decompress(base64.b64decode(self.desc['image'])))
 
-        # pad image to 4-byte length
-        while ((len(self.image) % 4) != 0):
+        # PX4 bootloader in theory requires only 4-byte padding,
+        # but a bug exists with the flash block caching where if the data
+        # does not fill up the block, it will not be written (left as all 1s).
+        # So pad up to the flash block size (8 words).
+        while ((len(self.image) % (4 * 8)) != 0):
             self.image.extend(b'\xff')
 
     def property(self, propname):

src/modules/commander/failure_detector/FailureDetector.cpp

@@ -221,8 +221,8 @@ void FailureDetector::updateAttitudeStatus()
 		const float max_roll(fabsf(math::radians(max_roll_deg)));
 		const float max_pitch(fabsf(math::radians(max_pitch_deg)));
 
-		const bool roll_status = (max_roll > 0.0f) && (fabsf(roll) > max_roll);
-		const bool pitch_status = (max_pitch > 0.0f) && (fabsf(pitch) > max_pitch);
+		const bool roll_status = (max_roll > FLT_EPSILON) && (fabsf(roll) > max_roll);
+		const bool pitch_status = (max_pitch > FLT_EPSILON) && (fabsf(pitch) > max_pitch);
 
 		hrt_abstime time_now = hrt_absolute_time();
 
@@ -384,7 +384,7 @@ void FailureDetector::updateMotorStatus(const vehicle_status_s &vehicle_status,
 
 			// Check for telemetry timeout
 			const hrt_abstime telemetry_age = time_now - cur_esc_report.timestamp;
-			const bool esc_timed_out = telemetry_age > 100_ms;  // TODO: magic number
+			const bool esc_timed_out = telemetry_age > 300_ms;
 
 			const bool esc_was_valid = _motor_failure_esc_valid_current_mask & (1 << i_esc);
 			const bool esc_timeout_currently_flagged = _motor_failure_esc_timed_out_mask & (1 << i_esc);
@@ -399,35 +399,40 @@ void FailureDetector::updateMotorStatus(const vehicle_status_s &vehicle_status,
 			}
 
 			// Check if ESC current is too low
-			float esc_throttle = 0.f;
-
-			if (PX4_ISFINITE(actuator_motors.control[i_esc])) {
-				esc_throttle = fabsf(actuator_motors.control[i_esc]);
+			if (cur_esc_report.esc_current > FLT_EPSILON) {
+				_motor_failure_escs_have_current = true;
 			}
 
-			const bool throttle_above_threshold = esc_throttle > _param_fd_motor_throttle_thres.get();
-			const bool current_too_low = cur_esc_report.esc_current < esc_throttle *
-						     _param_fd_motor_current2throttle_thres.get();
+			if (_motor_failure_escs_have_current) {
+				float esc_throttle = 0.f;
 
-			if (throttle_above_threshold && current_too_low && !esc_timed_out) {
-				if (_motor_failure_undercurrent_start_time[i_esc] == 0) {
-					_motor_failure_undercurrent_start_time[i_esc] = time_now;
+				if (PX4_ISFINITE(actuator_motors.control[i_esc])) {
+					esc_throttle = fabsf(actuator_motors.control[i_esc]);
 				}
 
-			} else {
-				if (_motor_failure_undercurrent_start_time[i_esc] != 0) {
-					_motor_failure_undercurrent_start_time[i_esc] = 0;
+				const bool throttle_above_threshold = esc_throttle > _param_fd_motor_throttle_thres.get();
+				const bool current_too_low = cur_esc_report.esc_current < esc_throttle *
+							     _param_fd_motor_current2throttle_thres.get();
+
+				if (throttle_above_threshold && current_too_low && !esc_timed_out) {
+					if (_motor_failure_undercurrent_start_time[i_esc] == 0) {
+						_motor_failure_undercurrent_start_time[i_esc] = time_now;
+					}
+
+				} else {
+					if (_motor_failure_undercurrent_start_time[i_esc] != 0) {
+						_motor_failure_undercurrent_start_time[i_esc] = 0;
+					}
 				}
+
+				if (_motor_failure_undercurrent_start_time[i_esc] != 0
+				    && (time_now - _motor_failure_undercurrent_start_time[i_esc]) > _param_fd_motor_time_thres.get() * 1_ms
+				    && (_motor_failure_esc_under_current_mask & (1 << i_esc)) == 0) {
+					// Set flag
+					_motor_failure_esc_under_current_mask |= (1 << i_esc);
+
+				} // else: this flag is never cleared, as the motor is stopped, so throttle < threshold
 			}
-
-			if (_motor_failure_undercurrent_start_time[i_esc] != 0
-			    && (time_now - _motor_failure_undercurrent_start_time[i_esc]) > _param_fd_motor_time_thres.get() * 1_ms
-			    && (_motor_failure_esc_under_current_mask & (1 << i_esc)) == 0) {
-				// Set flag
-				_motor_failure_esc_under_current_mask |= (1 << i_esc);
-
-			} // else: this flag is never cleared, as the motor is stopped, so throttle < threshold
-
 		}
 
 		bool critical_esc_failure = (_motor_failure_esc_timed_out_mask != 0 || _motor_failure_esc_under_current_mask != 0);

src/modules/commander/failure_detector/FailureDetector.hpp

@@ -129,6 +129,7 @@ private:
 	uint8_t _motor_failure_esc_valid_current_mask{};  // ESC 1-8, true if ESC telemetry was valid at some point
 	uint8_t _motor_failure_esc_timed_out_mask{};      // ESC telemetry no longer available -> failure
 	uint8_t _motor_failure_esc_under_current_mask{};  // ESC drawing too little current -> failure
+	bool _motor_failure_escs_have_current{false}; // true if some ESC had non-zero current (some don't support it)
 	hrt_abstime _motor_failure_undercurrent_start_time[actuator_motors_s::NUM_CONTROLS] {};
 
 	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};

src/modules/commander/failure_detector/failure_detector_params.c

@@ -53,7 +53,7 @@
  *
  * Setting this parameter to 0 disables the check
  *
- * @min 60
+ * @min 0
  * @max 180
  * @unit deg
  * @group Failure Detector
@@ -71,7 +71,7 @@ PARAM_DEFINE_INT32(FD_FAIL_R, 60);
  *
  * Setting this parameter to 0 disables the check
  *
- * @min 60
+ * @min 0
  * @max 180
  * @unit deg
  * @group Failure Detector

src/modules/control_allocator/module.yaml

@@ -556,6 +556,18 @@ mixer:
                             label: 'Direction CCW'
                             function: 'spin-dir'
                             show_as: 'true-if-positive'
+                          - name: 'CA_ROTOR${i}_AX'
+                            label: 'Axis X'
+                            function: 'axisx'
+                            advanced: true
+                          - name: 'CA_ROTOR${i}_AY'
+                            label: 'Axis Y'
+                            function: 'axisy'
+                            advanced: true
+                          - name: 'CA_ROTOR${i}_AZ'
+                            label: 'Axis Z'
+                            function: 'axisz'
+                            advanced: true
 
             1: # Fixed Wing
                 title: 'Fixed Wing'

src/modules/mc_pos_control/MulticopterPositionControl.cpp

@@ -531,7 +531,7 @@ void MulticopterPositionControl::Run()
 
 			// Publish attitude setpoint output
 			vehicle_attitude_setpoint_s attitude_setpoint{};
-			_control.getAttitudeSetpoint(attitude_setpoint);
+			_control.getAttitudeSetpoint(attitude_setpoint, _vehicle_land_detected.landed);
 			attitude_setpoint.timestamp = hrt_absolute_time();
 			_vehicle_attitude_setpoint_pub.publish(attitude_setpoint);
 

src/modules/mc_pos_control/PositionControl/PositionControl.cpp

@@ -117,6 +117,7 @@ bool PositionControl::update(const float dt)
 	// There has to be a valid output acceleration and thrust setpoint otherwise something went wrong
 	valid = valid && PX4_ISFINITE(_acc_sp(0)) && PX4_ISFINITE(_acc_sp(1)) && PX4_ISFINITE(_acc_sp(2));
 	valid = valid && PX4_ISFINITE(_thr_sp(0)) && PX4_ISFINITE(_thr_sp(1)) && PX4_ISFINITE(_thr_sp(2));
+	_dt = dt;
 
 	return valid;
 }
@@ -249,8 +250,35 @@ void PositionControl::getLocalPositionSetpoint(vehicle_local_position_setpoint_s
 	_thr_sp.copyTo(local_position_setpoint.thrust);
 }
 
-void PositionControl::getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_setpoint) const
+void PositionControl::getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_setpoint, bool landed)
 {
-	ControlMath::thrustToAttitude(_thr_sp, _yaw_sp, attitude_setpoint);
-	attitude_setpoint.yaw_sp_move_rate = _yawspeed_sp;
+
+	manual_control_setpoint_s manual_control_setpoint;
+	_manual_control_setpoint_sub.copy(&manual_control_setpoint);
+
+	if (manual_control_setpoint.aux2 > 0.3f) {
+		ControlMath::thrustToAttitude(_thr_sp, _yaw_sp, attitude_setpoint);
+		_roll_angle = 0.f;
+		_pitch_angle = 0.f;
+
+	} else {
+
+		if (landed) {
+			_roll_angle = 0.f;
+			_pitch_angle = 0.f;
+
+		} else {
+			_roll_angle += _dt * manual_control_setpoint.aux1 * 2.f * M_PI_F / 2.f;
+			// _pitch_angle += _dt * powf(manual_control_setpoint.aux1, 2.f) * 2.f * M_PI_F / 2.f; // TODO: aux2
+		}
+
+		Quatf q_sp = Eulerf(_roll_angle, _pitch_angle, _yaw_sp);
+		q_sp.copyTo(attitude_setpoint.q_d);
+		attitude_setpoint.yaw_sp_move_rate = _yawspeed_sp;
+
+		// Rotate thrust by negative attitude
+		Dcmf att_sp_dcm{q_sp};
+		Vector3f thrust_sp_body = att_sp_dcm.transpose() * _thr_sp;
+		thrust_sp_body.copyTo(attitude_setpoint.thrust_body);
+	}
 }

src/modules/mc_pos_control/PositionControl/PositionControl.hpp

@@ -43,6 +43,8 @@
 #include <matrix/matrix/math.hpp>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/Subscription.hpp>
 
 struct PositionControlStates {
 	matrix::Vector3f position;
@@ -176,7 +178,7 @@ public:
 	 * It needs to be executed by the attitude controller to achieve velocity and position tracking.
 	 * @param attitude_setpoint reference to struct to fill up
 	 */
-	void getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_setpoint) const;
+	void getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_setpoint, bool landed);
 
 private:
 	bool _inputValid();
@@ -216,4 +218,8 @@ private:
 	matrix::Vector3f _thr_sp; /**< desired thrust */
 	float _yaw_sp{}; /**< desired heading */
 	float _yawspeed_sp{}; /** desired yaw-speed */
+	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};	/**< manual control setpoint subscription */
+	float _roll_angle{0.f};
+	float _pitch_angle{0.f};
+	float _dt{0.f};
 };

src/modules/mc_rate_control/MulticopterRateControl.cpp

@@ -184,16 +184,15 @@ MulticopterRateControl::Run()
 					math::superexpo(manual_control_setpoint.r, _param_mc_acro_expo_y.get(), _param_mc_acro_supexpoy.get())};
 
 				_rates_setpoint = man_rate_sp.emult(_acro_rate_max);
-				_thrust_setpoint = math::constrain(manual_control_setpoint.z, 0.0f, 1.0f);
+				_thrust_setpoint(2) = -math::constrain(manual_control_setpoint.z, 0.0f, 1.0f);
+				_thrust_setpoint(0) = _thrust_setpoint(1) = 0.f;
 
 				// publish rate setpoint
-				vehicle_rates_setpoint.roll           = _rates_setpoint(0);
-				vehicle_rates_setpoint.pitch          = _rates_setpoint(1);
-				vehicle_rates_setpoint.yaw            = _rates_setpoint(2);
-				vehicle_rates_setpoint.thrust_body[0] = 0.0f;
-				vehicle_rates_setpoint.thrust_body[1] = 0.0f;
-				vehicle_rates_setpoint.thrust_body[2] = _thrust_setpoint;
-				vehicle_rates_setpoint.timestamp      = hrt_absolute_time();
+				vehicle_rates_setpoint.roll = _rates_setpoint(0);
+				vehicle_rates_setpoint.pitch = _rates_setpoint(1);
+				vehicle_rates_setpoint.yaw = _rates_setpoint(2);
+				_thrust_setpoint.copyTo(vehicle_rates_setpoint.thrust_body);
+				vehicle_rates_setpoint.timestamp = hrt_absolute_time();
 
 				_vehicle_rates_setpoint_pub.publish(vehicle_rates_setpoint);
 			}
@@ -203,7 +202,9 @@ MulticopterRateControl::Run()
 				_rates_setpoint(0) = PX4_ISFINITE(vehicle_rates_setpoint.roll)  ? vehicle_rates_setpoint.roll  : rates(0);
 				_rates_setpoint(1) = PX4_ISFINITE(vehicle_rates_setpoint.pitch) ? vehicle_rates_setpoint.pitch : rates(1);
 				_rates_setpoint(2) = PX4_ISFINITE(vehicle_rates_setpoint.yaw)   ? vehicle_rates_setpoint.yaw   : rates(2);
-				_thrust_setpoint = -vehicle_rates_setpoint.thrust_body[2];
+				_thrust_setpoint(0) = vehicle_rates_setpoint.thrust_body[0];
+				_thrust_setpoint(1) = vehicle_rates_setpoint.thrust_body[1];
+				_thrust_setpoint(2) = vehicle_rates_setpoint.thrust_body[2];
 			}
 		}
 
@@ -251,13 +252,14 @@ 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_setpoint) ? _thrust_setpoint : 0.0f;
+			actuators.control[actuator_controls_s::INDEX_THROTTLE] = PX4_ISFINITE(_thrust_setpoint(2)) ? -_thrust_setpoint(
+						2) : 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(_thrust_setpoint, angular_velocity.timestamp_sample);
+				publishThrustSetpoint(angular_velocity.timestamp_sample);
 			}
 
 			// scale effort by battery status if enabled
@@ -307,14 +309,12 @@ void MulticopterRateControl::publishTorqueSetpoint(const Vector3f &torque_sp, co
 	_vehicle_torque_setpoint_pub.publish(vehicle_torque_setpoint);
 }
 
-void MulticopterRateControl::publishThrustSetpoint(const float thrust_setpoint, const hrt_abstime &timestamp_sample)
+void MulticopterRateControl::publishThrustSetpoint(const hrt_abstime &timestamp_sample)
 {
 	vehicle_thrust_setpoint_s vehicle_thrust_setpoint{};
 	vehicle_thrust_setpoint.timestamp = hrt_absolute_time();
 	vehicle_thrust_setpoint.timestamp_sample = timestamp_sample;
-	vehicle_thrust_setpoint.xyz[0] = 0.0f;
-	vehicle_thrust_setpoint.xyz[1] = 0.0f;
-	vehicle_thrust_setpoint.xyz[2] = PX4_ISFINITE(thrust_setpoint) ? -thrust_setpoint : 0.0f; // Z is Down
+	_thrust_setpoint.copyTo(vehicle_thrust_setpoint.xyz);
 
 	_vehicle_thrust_setpoint_pub.publish(vehicle_thrust_setpoint);
 }

src/modules/mc_rate_control/MulticopterRateControl.hpp

@@ -94,8 +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 float thrust_setpoint, const hrt_abstime &timestamp_sample);
+	void publishThrustSetpoint(const hrt_abstime &timestamp_sample);
 
 	RateControl _rate_control; ///< class for rate control calculations
 
@@ -138,7 +137,7 @@ private:
 	matrix::Vector3f _rates_setpoint{};
 
 	float _battery_status_scale{0.0f};
-	float _thrust_setpoint{0.0f};
+	matrix::Vector3f _thrust_setpoint{};
 
 	float _energy_integration_time{0.0f};
 	float _control_energy[4] {};