Update Matrix.hpp (#4966 )

This was horribly wrong. Matrix is first cast into a matrix of size NxM (which is supposed to be the size of the result - NOT the starting point) so the transpose result becomes garbage. Instead make "Me" an MxN matrix as the original. Took me a whole evening to figure out this problem. Now my Kalman filter finally returns good results.
px4io: set safety on before going into bootloader (#4860 )
2026-05-23 18:07:34 +08:00 · 2016-07-03 12:30:24 +02:00 · 2016-06-21 09:18:45 +02:00 · 2016-06-01 17:16:29 +02:00 · 2016-05-26 20:00:46 +02:00 · 2016-05-21 15:55:32 +02:00
18 changed files with 114 additions and 62 deletions
@@ -6,6 +6,7 @@ float32 current_filtered_a	# Battery current in amperes, filtered, 0 if unknown
 float32 discharged_mah		# Discharged amount in mAh, -1 if unknown
 float32 remaining		# From 1 to 0, -1 if unknown
 int32 cell_count		# Number of cells
+bool connected			# Wether or not a battery is connected
 #bool is_powering_off		# Power off event imminent indication, false if unknown


@@ -687,7 +687,7 @@ PX4IO::init()
 	_max_transfer  = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER) - 2;
 	_max_rc_input  = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RC_INPUT_COUNT);

-	if ((_max_actuators < 1) || (_max_actuators > 255) ||
+	if ((_max_actuators < 1) || (_max_actuators > 16) ||
 	    (_max_relays > 32)   ||
 	    (_max_transfer < 16) || (_max_transfer > 255)  ||
 	    (_max_rc_input < 1)  || (_max_rc_input > 255)) {
@@ -699,6 +699,12 @@ PX4IO::init()
 		// be due to mismatched firmware versions and we want
 		// the startup script to be able to load a new IO
 		// firmware
+
+		// If IO has already safety off it won't accept going into bootloader mode,
+		// therefore we need to set safety on first.
+		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_ON, PX4IO_FORCE_SAFETY_MAGIC);
+
+		// Now the reboot into bootloader mode should succeed.
 		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_REBOOT_BL, PX4IO_REBOOT_BL_MAGIC);
 		return -1;
 	}
@@ -2475,16 +2481,19 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 			break;
 		}

-	case PWM_SERVO_GET_FAILSAFE_PWM:
+	case PWM_SERVO_GET_FAILSAFE_PWM: {
+			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
+			pwm->channel_count = _max_actuators;

-		ret = io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, 0, (uint16_t *)arg, _max_actuators);
+			ret = io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, 0, pwm->values, _max_actuators);

-		if (ret != OK) {
-			ret = -EIO;
+			if (ret != OK) {
+				ret = -EIO;
+			}
+
+			break;
 		}

-		break;
-
 	case PWM_SERVO_SET_DISARMED_PWM: {
 			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;

@@ -2499,16 +2508,19 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 			break;
 		}

-	case PWM_SERVO_GET_DISARMED_PWM:
+	case PWM_SERVO_GET_DISARMED_PWM: {
+			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
+			pwm->channel_count = _max_actuators;

-		ret = io_reg_get(PX4IO_PAGE_DISARMED_PWM, 0, (uint16_t *)arg, _max_actuators);
+			ret = io_reg_get(PX4IO_PAGE_DISARMED_PWM, 0, pwm->values, _max_actuators);

-		if (ret != OK) {
-			ret = -EIO;
+			if (ret != OK) {
+				ret = -EIO;
+			}
+
+			break;
 		}

-		break;
-
 	case PWM_SERVO_SET_MIN_PWM: {
 			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;

@@ -2523,16 +2535,19 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 			break;
 		}

-	case PWM_SERVO_GET_MIN_PWM:
+	case PWM_SERVO_GET_MIN_PWM: {
+			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
+			pwm->channel_count = _max_actuators;

-		ret = io_reg_get(PX4IO_PAGE_CONTROL_MIN_PWM, 0, (uint16_t *)arg, _max_actuators);
+			ret = io_reg_get(PX4IO_PAGE_CONTROL_MIN_PWM, 0, pwm->values, _max_actuators);

-		if (ret != OK) {
-			ret = -EIO;
+			if (ret != OK) {
+				ret = -EIO;
+			}
+
+			break;
 		}

-		break;
-
 	case PWM_SERVO_SET_MAX_PWM: {
 			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;

@@ -2547,12 +2562,15 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 			break;
 		}

-	case PWM_SERVO_GET_MAX_PWM:
+	case PWM_SERVO_GET_MAX_PWM: {
+			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
+			pwm->channel_count = _max_actuators;

-		ret = io_reg_get(PX4IO_PAGE_CONTROL_MAX_PWM, 0, (uint16_t *)arg, _max_actuators);
+			ret = io_reg_get(PX4IO_PAGE_CONTROL_MAX_PWM, 0, pwm->values, _max_actuators);

-		if (ret != OK) {
-			ret = -EIO;
+			if (ret != OK) {
+				ret = -EIO;
+			}
 		}

 		break;
@@ -305,7 +305,7 @@ public:
 	 * transpose the matrix
 	 */
 	Matrix<N, M> transposed(void) const {
-		matrix::Matrix<float, N, M> Me(this->arm_mat.pData);
+		matrix::Matrix<float, M, N> Me(this->arm_mat.pData);
 		Matrix<N, M> res(Me.transpose().data());
 		return res;
 	}
@@ -606,9 +606,9 @@ sbus_decode(uint64_t frame_time, uint8_t *frame, uint16_t *values, uint16_t *num
 		chancount = 18;

 		/* channel 17 (index 16) */
-		values[16] = (frame[SBUS_FLAGS_BYTE] & (1 << 0)) * 1000 + 998;
+		values[16] = (((frame[SBUS_FLAGS_BYTE] & (1 << 0)) > 0) ? 1 : 0) * 1000 + 998;
 		/* channel 18 (index 17) */
-		values[17] = (frame[SBUS_FLAGS_BYTE] & (1 << 1)) * 1000 + 998;
+		values[17] = (((frame[SBUS_FLAGS_BYTE] & (1 << 1)) > 0) ? 1 : 0) * 1000 + 998;
 	}

 	/* note the number of channels decoded */
@@ -174,6 +174,7 @@ BottleDrop::BottleDrop() :

 	_task_should_exit(false),
 	_main_task(-1),
+	_mavlink_log_pub(nullptr),
 	_command_sub(-1),
 	_wind_estimate_sub(-1),
 	_command {},
@@ -1041,9 +1041,9 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 	case vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF: {
 			/* ok, home set, use it to take off */
 			if (TRANSITION_CHANGED == main_state_transition(&status, commander_state_s::MAIN_STATE_AUTO_TAKEOFF, main_state_prev, &status_flags, &internal_state)) {
-				warnx("taking off!");
+				mavlink_and_console_log_info(&mavlink_log_pub, "Taking off");
 			} else {
-				warnx("takeoff denied");
+				mavlink_and_console_log_critical(&mavlink_log_pub, "Takeoff denied, disarm and re-try");
 			}

 		}
@@ -1052,9 +1052,9 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 	case vehicle_command_s::VEHICLE_CMD_NAV_LAND: {
 			/* ok, home set, use it to take off */
 			if (TRANSITION_CHANGED == main_state_transition(&status, commander_state_s::MAIN_STATE_AUTO_LAND, main_state_prev, &status_flags, &internal_state)) {
-				warnx("landing!");
+				mavlink_and_console_log_info(&mavlink_log_pub, "Landing at current position");
 			} else {
-				warnx("landing denied");
+				mavlink_and_console_log_critical(&mavlink_log_pub, "Landing denied, land manually.");
 			}

 		}
@@ -58,7 +58,6 @@
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
 #include <systemlib/systemlib.h>
-#include <systemlib/mavlink_log.h>
 #include <mathlib/mathlib.h>
 #include <mathlib/math/filter/LowPassFilter2p.hpp>
 #include <platforms/px4_defines.h>
@@ -221,6 +221,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
 	_newAdsData(false),
 	_newDataMag(false),
 	_newRangeData(false),
+	_mavlink_log_pub(nullptr),

 	_mag_offset_x(this, "MAGB_X"),
 	_mag_offset_y(this, "MAGB_Y"),
@@ -421,8 +422,7 @@ int AttitudePositionEstimatorEKF::check_filter_state()

 		// Do not warn about accel offset if we have no position updates
 		if (!(warn_index == 5 && _ekf->staticMode)) {
-			PX4_WARN("reset: %s", feedback[warn_index]);
-			mavlink_log_critical(&_mavlink_log_pub, "[ekf check] %s", feedback[warn_index]);
+			mavlink_and_console_log_critical(&_mavlink_log_pub, "[ekf check] %s", feedback[warn_index]);
 		}
 	}

@@ -699,8 +699,6 @@ void AttitudePositionEstimatorEKF::task_main()

 					_filter_ref_offset = -_baro.altitude;

-					PX4_INFO("filter ref off: baro_alt: %8.4f", (double)_filter_ref_offset);
-
 				} else {

 					if (!_gps_initialized && _gpsIsGood) {
@@ -785,7 +783,6 @@ void AttitudePositionEstimatorEKF::initReferencePosition(hrt_abstime timestamp,
 		_local_pos.ref_timestamp = timestamp;

 		map_projection_init(&_pos_ref, lat, lon);
-		mavlink_and_console_log_info(&_mavlink_log_pub, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt);
 	}
 }

@@ -140,7 +140,7 @@ static int land_detector_start(const char *mode)
 	}

 	/* avoid memory fragmentation by not exiting start handler until the task has fully started */
-	const uint32_t timeout = hrt_absolute_time() + 5000000; //5 second timeout
+	const uint64_t timeout = hrt_absolute_time() + 5000000; //5 second timeout

 	/* avoid printing dots just yet and do one sleep before the first check */
 	usleep(10000);
@@ -4,7 +4,7 @@
 #include <systemlib/err.h>
 #include <matrix/math.hpp>

-orb_advert_t mavlink_log_pub;
+orb_advert_t mavlink_log_pub = nullptr;

 // timeouts for sensors in microseconds
 static const uint32_t 		EST_SRC_TIMEOUT = 10000; // 0.01 s
@@ -588,8 +588,8 @@ protected:

 	void send(const hrt_abstime t)
 	{
-		struct vehicle_status_s status;
-		struct battery_status_s battery_status;
+		struct vehicle_status_s status = {};
+		struct battery_status_s battery_status = {};

 		const bool updated_status = _status_sub->update(&status);
 		const bool updated_battery = _battery_status_sub->update(&battery_status);
@@ -605,14 +605,13 @@ protected:
 		if (updated_status || updated_battery) {
 			mavlink_sys_status_t msg;

-
 			msg.onboard_control_sensors_present = status.onboard_control_sensors_present;
 			msg.onboard_control_sensors_enabled = status.onboard_control_sensors_enabled;
 			msg.onboard_control_sensors_health = status.onboard_control_sensors_health;
 			msg.load = status.load * 1000.0f;
-			msg.voltage_battery = battery_status.voltage_filtered_v * 1000.0f;
-			msg.current_battery = battery_status.current_filtered_a * 100.0f;
-			msg.battery_remaining = battery_status.remaining >= 0 ? battery_status.remaining * 100.0f : -1;
+			msg.voltage_battery = (battery_status.connected) ? battery_status.voltage_filtered_v * 1000.0f : UINT16_MAX;
+			msg.current_battery = (battery_status.connected) ? battery_status.current_filtered_a * 100.0f : -1;
+			msg.battery_remaining = (battery_status.connected) ? battery_status.remaining * 100.0f : -1;
 			// TODO: fill in something useful in the fields below
 			msg.drop_rate_comm = 0;
 			msg.errors_comm = 0;
@@ -628,13 +627,13 @@ protected:
 			bat_msg.id = 0;
 			bat_msg.battery_function = MAV_BATTERY_FUNCTION_ALL;
 			bat_msg.type = MAV_BATTERY_TYPE_LIPO;
-			bat_msg.current_consumed = battery_status.discharged_mah;
+			bat_msg.current_consumed = (battery_status.connected) ? battery_status.discharged_mah : -1;
 			bat_msg.energy_consumed = -1;
-			bat_msg.current_battery = battery_status.current_a * 100;
-			bat_msg.battery_remaining = battery_status.remaining >= 0 ? battery_status.remaining * 100.0f : -1;
+			bat_msg.current_battery = (battery_status.connected) ? battery_status.current_filtered_a * 100 : -1;
+			bat_msg.battery_remaining = (battery_status.connected) ? battery_status.remaining * 100.0f : -1;
 			bat_msg.temperature = INT16_MAX;
 			for (unsigned int i = 0; i < (sizeof(bat_msg.voltages) / sizeof(bat_msg.voltages[0])); i++) {
-				if ((int)i < battery_status.cell_count) {
+				if ((int)i < battery_status.cell_count && battery_status.connected) {
 					bat_msg.voltages[i] = (battery_status.voltage_v / battery_status.cell_count) * 1000.0f;
 				} else {
 					bat_msg.voltages[i] = UINT16_MAX;
@@ -2614,7 +2613,7 @@ protected:

 	void send(const hrt_abstime t)
 	{
-		struct vehicle_status_s status;
+		struct vehicle_status_s status = {};
 		(void)_status_sub->update(&status);

 		mavlink_command_long_t msg;
@@ -86,7 +86,7 @@ MavlinkOrbSubscription::update(uint64_t *time, void* data)
 	}

 	if (orb_copy(_topic, _fd, data)) {
-		if (data) {
+		if (data != nullptr) {
 			/* error copying topic data */
 			memset(data, 0, _topic->o_size);
 		}
@@ -1991,8 +1991,40 @@ MulticopterPositionControl::task_main()

 			math::Matrix<3, 3> R_sp;

-			/* construct attitude setpoint rotation matrix */
-			R_sp.from_euler(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body);
+			// construct attitude setpoint rotation matrix. modify the setpoints for roll
+			// and pitch such that they reflect the user's intention even if a yaw error
+			// (yaw_sp - yaw) is present. In the presence of a yaw error constructing a rotation matrix
+			// from the pure euler angle setpoints will lead to unexpected attitude behaviour from
+			// the user's view as the euler angle sequence uses the  yaw setpoint and not the current
+			// heading of the vehicle.
+
+			// calculate our current yaw error
+			float yaw_error = _wrap_pi(_att_sp.yaw_body - _yaw);
+
+			// compute the vector obtained by rotating a z unit vector by the rotation
+			// given by the roll and pitch commands of the user
+			math::Vector<3> zB = {0, 0, 1};
+			math::Matrix<3,3> R_sp_roll_pitch;
+			R_sp_roll_pitch.from_euler(_att_sp.roll_body, _att_sp.pitch_body, 0);
+			math::Vector<3> z_roll_pitch_sp = R_sp_roll_pitch * zB;
+
+
+			// transform the vector into a new frame which is rotated around the z axis
+			// by the current yaw error. this vector defines the desired tilt when we look
+			// into the direction of the desired heading
+			math::Matrix<3,3> R_yaw_correction;
+			R_yaw_correction.from_euler(0.0f, 0.0f, -yaw_error);
+			z_roll_pitch_sp = R_yaw_correction * z_roll_pitch_sp;
+
+			// use the formula z_roll_pitch_sp = R_tilt * [0;0;1]
+			// to calculate the new desired roll and pitch angles
+			// R_tilt can be written as a function of the new desired roll and pitch
+			// angles. we get three equations and have to solve for 2 unknowns
+			float pitch_new = asinf(z_roll_pitch_sp(0));
+			float roll_new = -atan2f(z_roll_pitch_sp(1), z_roll_pitch_sp(2));
+
+			R_sp.from_euler(roll_new, pitch_new, _att_sp.yaw_body);
+
 			memcpy(&_att_sp.R_body[0], R_sp.data, sizeof(_att_sp.R_body));

 			/* reset the acceleration set point for all non-attitude flight modes */
@@ -684,6 +684,11 @@ Navigator::publish_position_setpoint_triplet()
 	/* update navigation state */
 	_pos_sp_triplet.nav_state = _vstatus.nav_state;

+	/* do not publish an empty triplet */
+	if (!_pos_sp_triplet.current.valid) {
+		return;
+	}
+
 	/* lazily publish the position setpoint triplet only once available */
 	if (_pos_sp_triplet_pub != nullptr) {
 		orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub, &_pos_sp_triplet);
@@ -75,6 +75,7 @@ Battery::reset(battery_status_s *battery_status)
 	battery_status->cell_count = _param_n_cells.get();
 	// TODO: check if it is sane to reset warning to NONE
 	battery_status->warning = battery_status_s::BATTERY_WARNING_NONE;
+	battery_status->connected = false;
 }

 void
@@ -97,7 +98,7 @@ Battery::updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float curre
 		battery_status->discharged_mah = _discharged_mah;
 		battery_status->warning = _warning;
 		battery_status->remaining = _remaining;
-
+		battery_status->connected = true;
 	}
 }

@@ -103,7 +103,7 @@ class UavcanNode : public device::CDev
 	 */

 	static constexpr unsigned RxQueueLenPerIface	= FramePerMSecond * PollTimeoutMs; // At
-	static constexpr unsigned StackSize		= 1800;
+	static constexpr unsigned StackSize		= 2400;

 public:
 	typedef uavcan_stm32::CanInitHelper<RxQueueLenPerIface> CanInitHelper;
@@ -923,7 +923,6 @@ void UavcanServers::unpackFwFromROMFS(const char* sd_path, const char* romfs_pat

 	DIR* const romfs_dir = opendir(romfs_path);
 	if (!romfs_dir) {
-		warnx("base: couldn't open %s", romfs_path);
 		return;
 	}

@@ -350,11 +350,11 @@ pwm_main(int argc, char *argv[])
 	} else if (!strcmp(argv[1], "min")) {

 		if (set_mask == 0) {
-			usage("no channels set");
+			usage("min: no channels set");
 		}

 		if (pwm_value == 0) {
-			usage("no PWM value provided");
+			usage("min: no PWM value provided");
 		}

 		struct pwm_output_values pwm_values;
@@ -381,7 +381,7 @@ pwm_main(int argc, char *argv[])
 		}

 		if (pwm_values.channel_count == 0) {
-			usage("no PWM values added");
+			usage("min: no channels provided");

 		} else {

@@ -428,7 +428,7 @@ pwm_main(int argc, char *argv[])
 		}

 		if (pwm_values.channel_count == 0) {
-			usage("no PWM values added");
+			usage("max: no PWM channels");

 		} else {

@@ -475,7 +475,7 @@ pwm_main(int argc, char *argv[])
 		}

 		if (pwm_values.channel_count == 0) {
-			usage("no PWM values added");
+			usage("disarmed: no PWM channels");

 		} else {

@@ -495,7 +495,7 @@ pwm_main(int argc, char *argv[])
 		}

 		if (pwm_value == 0) {
-			usage("no PWM provided");
+			usage("failsafe: no PWM provided");
 		}

 		struct pwm_output_values pwm_values;
@@ -522,7 +522,7 @@ pwm_main(int argc, char *argv[])
 		}

 		if (pwm_values.channel_count == 0) {
-			usage("no PWM values added");
+			usage("failsafe: no PWM channels");

 		} else {
Author	SHA1	Message	Date
Martin K. Schröder	cb1528008f	Update Matrix.hpp (#4966 ) This was horribly wrong. Matrix is first cast into a matrix of size NxM (which is supposed to be the size of the result - NOT the starting point) so the transpose result becomes garbage. Instead make "Me" an MxN matrix as the original. Took me a whole evening to figure out this problem. Now my Kalman filter finally returns good results.	2016-07-03 12:30:24 +02:00
Julian Oes	403d76c7a7	px4io: set safety on before going into bootloader (#4860 ) Sometimes when flashing new firmware, the IO update fails because safety is off. In this case, we should set safety on first before putting the IO board into bootloader mode.	2016-06-21 09:18:45 +02:00
Julian Oes	a031f6eec8	land_detector: fix timestamp type (#4710 ) The overflow of the uint32_t lead to the land_detector start getting aborted.	2016-06-01 17:16:29 +02:00
Pavel Kirienko	0749b04c9f	Fixed stack overflow in UAVCAN process (#4643 ) * Increased uavcan stack size; the old value of 1800 was insufficient * Removed a misleading warning message from uavcan servers initialization	2016-05-26 20:00:46 +02:00
Lorenz Meier	0902401115	SBUS decoding: Fix channel 18 return value	2016-05-21 15:55:32 +02:00
Lorenz Meier	87a6d36341	EKF2: Remove unused header	2016-05-21 14:13:30 +02:00
Lorenz Meier	bdf01061d0	Bottle drop: Fix uninitialized member	2016-05-21 14:13:22 +02:00
Lorenz Meier	62c76d62cd	LPE: Fix stack smashing	2016-05-21 14:13:15 +02:00
Lorenz Meier	65c4a9190c	EKF1: Simplify output	2016-05-21 14:13:08 +02:00
Lorenz Meier	5070238504	EKF1: Fix stack smashing resulting from uninitialized publication	2016-05-21 14:13:00 +02:00
Lorenz Meier	2f52c90885	Fix compile error in MAVLink app	2016-05-19 15:34:25 +02:00
Roman Bapst	9b4b67644d	WIP: Manual attitude setpoint for large heading errors (#4564 ) * mc pos control: in manual mode calculate attitude setpoint such that it reflects the users intuition of roll and pitch for any given heading error * added some comments on the new manual attitude setpoint generation * make calculation shorter	2016-05-19 15:32:08 +02:00
Lorenz Meier	556c33cf8c	Battery lib: Set valid flag	2016-05-19 15:32:08 +02:00
Lorenz Meier	ac256eb64d	MAVLink: Use valid flag to initialize fields Conflicts: src/modules/mavlink/mavlink_messages.cpp	2016-05-19 15:32:08 +02:00
Lorenz Meier	fd3efe168a	Battery status: Add valid flag	2016-05-19 15:32:08 +02:00
Lorenz Meier	d091af4fc0	EKF: Be less verbose, avoid floating ng point printing stack smashing	2016-05-14 18:22:58 +02:00
Lorenz Meier	3ebea78d91	IO driver: Fix PWM load	2016-05-14 18:06:32 +02:00
Lorenz Meier	8361b8b3c6	PWM cmd: better reporting	2016-05-14 18:06:24 +02:00
Lorenz Meier	2807e53838	Navigator: Do not publish an empty triplet	2016-05-13 11:05:18 +02:00
Lorenz Meier	2cc613cf64	Commander: Better status feedback	2016-05-13 11:05:12 +02:00