Merge branch 'master' of github.com:wingtra/Firmware into control_state

src/modules/commander/PreflightCheck.cpp

@@ -71,7 +71,50 @@
 
 namespace Commander
 {
-static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional)
+
+static int check_calibration(int fd, const char* param_template, int &devid);
+
+int check_calibration(int fd, const char* param_template, int &devid)
+{
+	bool calibration_found;
+
+	/* new style: ask device for calibration state */
+	int ret = px4_ioctl(fd, SENSORIOCCALTEST, 0);
+
+	calibration_found = (ret == OK);
+	
+	devid = px4_ioctl(fd, DEVIOCGDEVICEID, 0);
+
+	char s[20];
+	int instance = 0;
+
+	/* old style transition: check param values */
+	while (!calibration_found) {
+		sprintf(s, param_template, instance);
+		param_t parm = param_find(s);
+
+		/* if the calibration param is not present, abort */
+		if (parm == PARAM_INVALID) {
+			break;
+		}
+
+		/* if param get succeeds */
+		int calibration_devid;
+		if (!param_get(parm, &(calibration_devid))) {
+
+			/* if the devid matches, exit early */
+			if (devid == calibration_devid) {
+				calibration_found = true;
+				break;
+			}
+		}
+		instance++;
+	}
+
+	return !calibration_found;
+}
+
+static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id)
 {
 	bool success = true;
 
@@ -88,13 +131,9 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional)
 		return false;
 	}
 
-	int calibration_devid;
-	int ret;
-	int devid = px4_ioctl(fd, DEVIOCGDEVICEID, 0);
-	sprintf(s, "CAL_MAG%u_ID", instance);
-	param_get(param_find(s), &(calibration_devid));
+	int ret = check_calibration(fd, "CAL_MAG%u_ID", device_id);
 
-	if (devid != calibration_devid) {
+	if (ret) {
 		mavlink_and_console_log_critical(mavlink_fd,
 						 "PREFLIGHT FAIL: MAG #%u UNCALIBRATED", instance);
 		success = false;
@@ -115,7 +154,7 @@ out:
 	return success;
 }
 
-static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, bool dynamic)
+static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, bool dynamic, int &device_id)
 {
 	bool success = true;
 
@@ -132,13 +171,9 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional,
 		return false;
 	}
 
-	int calibration_devid;
-	int ret;
-	int devid = px4_ioctl(fd, DEVIOCGDEVICEID, 0);
-	sprintf(s, "CAL_ACC%u_ID", instance);
-	param_get(param_find(s), &(calibration_devid));
+	int ret = check_calibration(fd, "CAL_ACC%u_ID", device_id);
 
-	if (devid != calibration_devid) {
+	if (ret) {
 		mavlink_and_console_log_critical(mavlink_fd,
 						 "PREFLIGHT FAIL: ACCEL #%u UNCALIBRATED", instance);
 		success = false;
@@ -184,7 +219,7 @@ out:
 	return success;
 }
 
-static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional)
+static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id)
 {
 	bool success = true;
 
@@ -201,13 +236,9 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional)
 		return false;
 	}
 
-	int calibration_devid;
-	int ret;
-	int devid = px4_ioctl(fd, DEVIOCGDEVICEID, 0);
-	sprintf(s, "CAL_GYRO%u_ID", instance);
-	param_get(param_find(s), &(calibration_devid));
+	int ret = check_calibration(fd, "CAL_GYRO%u_ID", device_id);
 
-	if (devid != calibration_devid) {
+	if (ret) {
 		mavlink_and_console_log_critical(mavlink_fd,
 						 "PREFLIGHT FAIL: GYRO #%u UNCALIBRATED", instance);
 		success = false;
@@ -228,7 +259,7 @@ out:
 	return success;
 }
 
-static bool baroCheck(int mavlink_fd, unsigned instance, bool optional)
+static bool baroCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id)
 {
 	bool success = true;
 
@@ -245,6 +276,20 @@ static bool baroCheck(int mavlink_fd, unsigned instance, bool optional)
 		return false;
 	}
 
+	device_id = -1000;
+
+	// TODO: There is no baro calibration yet, since no external baros exist
+	// int ret = check_calibration(fd, "CAL_BARO%u_ID");
+
+	// if (ret) {
+	// 	mavlink_and_console_log_critical(mavlink_fd,
+	// 					 "PREFLIGHT FAIL: BARO #%u UNCALIBRATED", instance);
+	// 	success = false;
+	// 	goto out;
+	// }
+
+//out:
+
 	px4_close(fd);
 	return success;
 }
@@ -311,49 +356,110 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
 
 	/* ---- MAG ---- */
 	if (checkMag) {
+		bool prime_found = false;
+		int32_t prime_id = 0;
+		param_get(param_find("CAL_MAG_PRIME"), &prime_id);
+
 		/* check all sensors, but fail only for mandatory ones */
 		for (unsigned i = 0; i < max_optional_mag_count; i++) {
 			bool required = (i < max_mandatory_mag_count);
+			int device_id = -1;
 
-			if (!magnometerCheck(mavlink_fd, i, !required) && required) {
+			if (!magnometerCheck(mavlink_fd, i, !required, device_id) && required) {
 				failed = true;
 			}
+
+			if (device_id == prime_id) {
+				prime_found = true;
+			}
+		}
+
+		/* check if the primary device is present */
+		if (!prime_found && prime_id != 0) {
+			mavlink_log_critical(mavlink_fd, "warning: primary compass not operational");
+			failed = true;
 		}
 	}
 
 	/* ---- ACCEL ---- */
 	if (checkAcc) {
+		bool prime_found = false;
+		int32_t prime_id = 0;
+		param_get(param_find("CAL_ACC_PRIME"), &prime_id);
+
 		/* check all sensors, but fail only for mandatory ones */
 		for (unsigned i = 0; i < max_optional_accel_count; i++) {
 			bool required = (i < max_mandatory_accel_count);
+			int device_id = -1;
 
-			if (!accelerometerCheck(mavlink_fd, i, !required, checkDynamic) && required) {
+			if (!accelerometerCheck(mavlink_fd, i, !required, checkDynamic, device_id) && required) {
 				failed = true;
 			}
+
+			if (device_id == prime_id) {
+				prime_found = true;
+			}
+		}
+
+		/* check if the primary device is present */
+		if (!prime_found && prime_id != 0) {
+			mavlink_log_critical(mavlink_fd, "warning: primary accelerometer not operational");
+			failed = true;
 		}
 	}
 
 	/* ---- GYRO ---- */
 	if (checkGyro) {
+		bool prime_found = false;
+		int32_t prime_id = 0;
+		param_get(param_find("CAL_GYRO_PRIME"), &prime_id);
+
 		/* check all sensors, but fail only for mandatory ones */
 		for (unsigned i = 0; i < max_optional_gyro_count; i++) {
 			bool required = (i < max_mandatory_gyro_count);
+			int device_id = -1;
 
-			if (!gyroCheck(mavlink_fd, i, !required) && required) {
+			if (!gyroCheck(mavlink_fd, i, !required, device_id) && required) {
 				failed = true;
 			}
+
+			if (device_id == prime_id) {
+				prime_found = true;
+			}
+		}
+
+		/* check if the primary device is present */
+		if (!prime_found && prime_id != 0) {
+			mavlink_log_critical(mavlink_fd, "warning: primary gyro not operational");
+			failed = true;
 		}
 	}
 
 	/* ---- BARO ---- */
 	if (checkBaro) {
+		bool prime_found = false;
+		int32_t prime_id = 0;
+		param_get(param_find("CAL_BARO_PRIME"), &prime_id);
+
 		/* check all sensors, but fail only for mandatory ones */
 		for (unsigned i = 0; i < max_optional_baro_count; i++) {
 			bool required = (i < max_mandatory_baro_count);
+			int device_id = -1;
 
-			if (!baroCheck(mavlink_fd, i, !required) && required) {
+			if (!baroCheck(mavlink_fd, i, !required, device_id) && required) {
 				failed = true;
 			}
+
+			if (device_id == prime_id) {
+				prime_found = true;
+			}
+		}
+
+		// TODO there is no logic in place to calibrate the primary baro yet
+		// // check if the primary device is present 
+		if (!prime_found && prime_id != 0) {
+			mavlink_log_critical(mavlink_fd, "warning: primary barometer not operational");
+			failed = true;
 		}
 	}
 
@@ -367,12 +473,13 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
 	/* ---- RC CALIBRATION ---- */
 	if (checkRC) {
 		if (rc_calibration_check(mavlink_fd) != OK) {
+			mavlink_log_critical(mavlink_fd, "RC calibration check failed");
 			failed = true;
 		}
 	}
 
 	/* ---- Global Navigation Satellite System receiver ---- */
-	if(checkGNSS) {
+	if (checkGNSS) {
 		if(!gnssCheck(mavlink_fd)) {
 			failed = true;
 		}

src/modules/commander/accelerometer_calibration.cpp

@@ -156,6 +156,10 @@ static const int ERROR = -1;
 
 static const char *sensor_name = "accel";
 
+static int32_t device_id[max_accel_sens];
+static int device_prio_max = 0;
+static int32_t device_id_primary = 0;
+
 calibrate_return do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_accel_sens][3], float (&accel_T)[max_accel_sens][3][3], unsigned *active_sensors);
 calibrate_return read_accelerometer_avg(int (&subs)[max_accel_sens], float (&accel_avg)[max_accel_sens][detect_orientation_side_count][3], unsigned orient, unsigned samples_num);
 int mat_invert3(float src[3][3], float dst[3][3]);
@@ -172,7 +176,6 @@ typedef struct  {
 int do_accel_calibration(int mavlink_fd)
 {
 	int fd;
-	int32_t device_id[max_accel_sens];
 
 	mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name);
 
@@ -259,6 +262,8 @@ int do_accel_calibration(int mavlink_fd)
 		
 		bool failed = false;
 
+		failed = failed || (OK != param_set_no_notification(param_find("CAL_ACC_PRIME"), &(device_id_primary)));
+
 		/* set parameters */
 		(void)sprintf(str, "CAL_ACC%u_XOFF", i);
 		failed |= (OK != param_set_no_notification(param_find(str), &(accel_scale.x_offset)));
@@ -370,6 +375,15 @@ calibrate_return do_accel_calibration_measurements(int mavlink_fd, float (&accel
 		struct accel_report arp = {};
 		(void)orb_copy(ORB_ID(sensor_accel), worker_data.subs[i], &arp);
 		timestamps[i] = arp.timestamp;
+
+		// Get priority
+		int32_t prio;
+		orb_priority(worker_data.subs[i], &prio);
+
+		if (prio > device_prio_max) {
+			device_prio_max = prio;
+			device_id_primary = device_id[i];
+		}
 	}
 
 	if (result == calibrate_return_ok) {

src/modules/commander/commander.cpp

@@ -802,6 +802,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 	case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION:
 	case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS:
 	case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_STORAGE:
+	case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_UAVCAN:
 	case vehicle_command_s::VEHICLE_CMD_CUSTOM_0:
 	case vehicle_command_s::VEHICLE_CMD_CUSTOM_1:
 	case vehicle_command_s::VEHICLE_CMD_CUSTOM_2:

src/modules/commander/gyro_calibration.cpp

@@ -168,6 +168,9 @@ int do_gyro_calibration(int mavlink_fd)
 		1.0f,	// z scale
 	};
 	
+	int device_prio_max = 0;
+	int32_t device_id_primary = 0;
+
 	for (unsigned s = 0; s < max_gyros; s++) {
 		char str[30];
 		
@@ -199,6 +202,15 @@ int do_gyro_calibration(int mavlink_fd)
 	
 	for (unsigned s = 0; s < max_gyros; s++) {
 		worker_data.gyro_sensor_sub[s] = orb_subscribe_multi(ORB_ID(sensor_gyro), s);
+
+		// Get priority
+		int32_t prio;
+		orb_priority(worker_data.gyro_sensor_sub[s], &prio);
+
+		if (prio > device_prio_max) {
+			device_prio_max = prio;
+			device_id_primary = worker_data.device_id[s];
+		}
 	}
 
 	int cancel_sub  = calibrate_cancel_subscribe();
@@ -258,9 +270,12 @@ int do_gyro_calibration(int mavlink_fd)
 	}
 
 	if (res == OK) {
+
 		/* set offset parameters to new values */
 		bool failed = false;
 
+		failed = failed || (OK != param_set_no_notification(param_find("CAL_GYRO_PRIME"), &(device_id_primary)));
+
 		for (unsigned s = 0; s < max_gyros; s++) {
 			if (worker_data.device_id[s] != 0) {
 				char str[30];

src/modules/commander/mag_calibration.cpp

@@ -73,6 +73,10 @@ static constexpr unsigned int calibration_sides = 6;			///< The total number of
 static constexpr unsigned int calibration_total_points = 240;		///< The total points per magnetometer
 static constexpr unsigned int calibraton_duration_seconds = 42; 	///< The total duration the routine is allowed to take
 
+int32_t	device_ids[max_mags];
+int device_prio_max = 0;
+int32_t device_id_primary = 0;
+
 calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]);
 
 /// Data passed to calibration worker routine
@@ -108,7 +112,6 @@ int do_mag_calibration(int mavlink_fd)
 	
 	// Determine which mags are available and reset each
 
-	int32_t	device_ids[max_mags];
 	char str[30];
 
 	for (size_t i=0; i<max_mags; i++) {
@@ -434,6 +437,15 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
 					result = calibrate_return_error;
 					break;
 				}
+
+				// Get priority
+				int32_t prio;
+				orb_priority(worker_data.sub_mag[cur_mag], &prio);
+
+				if (prio > device_prio_max) {
+					device_prio_max = prio;
+					device_id_primary = device_ids[cur_mag];
+				}
 			}
 		}
 	}
@@ -550,6 +562,9 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
 	}
 	
 	if (result == calibrate_return_ok) {
+
+		(void)param_set_no_notification(param_find("CAL_MAG_PRIME"), &(device_id_primary));
+
 		for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
 			if (device_ids[cur_mag] != 0) {
 				int fd_mag = -1;

src/modules/ekf_att_pos_estimator/CMakeLists.txt

@@ -39,6 +39,7 @@ endif()
 px4_add_module(
 	MODULE modules__ekf_att_pos_estimator
 	MAIN ekf_att_pos_estimator
+	STACK 3000
 	COMPILE_FLAGS ${MODULE_CFLAGS}
 	SRCS
 		ekf_att_pos_estimator_main.cpp

src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp

@@ -1194,7 +1194,7 @@ int AttitudePositionEstimatorEKF::start()
 	_estimator_task = px4_task_spawn_cmd("ekf_att_pos_estimator",
 					 SCHED_DEFAULT,
 					 SCHED_PRIORITY_MAX - 20,
-					 4200,
+					 3900,
 					 (px4_main_t)&AttitudePositionEstimatorEKF::task_main_trampoline,
 					 nullptr);
 
@@ -1235,6 +1235,7 @@ void AttitudePositionEstimatorEKF::print_status()
 	PX4_INFO("dang: %8.4f %8.4f %8.4f dang corr: %8.4f %8.4f %8.4f" , (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.y,
 	       (double)_ekf->dAngIMU.z, (double)_ekf->correctedDelAng.x, (double)_ekf->correctedDelAng.y,
 	       (double)_ekf->correctedDelAng.z);
+
 	PX4_INFO("states (quat)        [0-3]: %8.4f, %8.4f, %8.4f, %8.4f", (double)_ekf->states[0], (double)_ekf->states[1],
 	       (double)_ekf->states[2], (double)_ekf->states[3]);
 	PX4_INFO("states (vel m/s)     [4-6]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[4], (double)_ekf->states[5],
@@ -1338,7 +1339,7 @@ void AttitudePositionEstimatorEKF::pollData()
 		} else {
 			_ekf->dVelIMU.x = 0.5f * (_ekf->accel.x + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 0]);
 			_ekf->dVelIMU.y = 0.5f * (_ekf->accel.y + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 1]);
-			_ekf->dVelIMU.z = 0.5f * (_ekf->accel.z + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 2]) + 9.80665f;
+			_ekf->dVelIMU.z = 0.5f * (_ekf->accel.z + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 2]);
 		}
 
 		_ekf->accel.x = _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 0];
@@ -1414,21 +1415,21 @@ void AttitudePositionEstimatorEKF::pollData()
 	static hrt_abstime lastprint = 0;
 
 	if (hrt_elapsed_time(&lastprint) > 1000000) {
-		warnx("dt: %8.6f, dtg: %8.6f, dta: %8.6f, dt > 5 ms: %u, dt > 10 ms: %u",
+		PX4_WARN("dt: %8.6f, dtg: %8.6f, dta: %8.6f, dt > 5 ms: %u, dt > 10 ms: %u",
 			(double)deltaT, (double)deltaTIntegral, (double)deltaTIntAcc,
 			dtoverflow5, dtoverflow10);
 
-		warnx("EKF: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f",
+		PX4_WARN("EKF: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f",
 		(double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.z,
 		(double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z);
 
-		warnx("INT: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f",
+		PX4_WARN("INT: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f",
 		(double)(_sensor_combined.gyro_integral_rad[0]), (double)(_sensor_combined.gyro_integral_rad[2]),
 		(double)(_sensor_combined.accelerometer_integral_m_s[0]),
 		(double)(_sensor_combined.accelerometer_integral_m_s[1]),
 		(double)(_sensor_combined.accelerometer_integral_m_s[2]));
 
-		warnx("DRV: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f",
+		PX4_WARN("DRV: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f",
 		(double)(_sensor_combined.gyro_rad_s[0] * deltaT), (double)(_sensor_combined.gyro_rad_s[2] * deltaT),
 		(double)(_sensor_combined.accelerometer_m_s2[0] * deltaT),
 		(double)(_sensor_combined.accelerometer_m_s2[1] * deltaT),
@@ -1738,8 +1739,6 @@ int ekf_att_pos_estimator_main(int argc, char *argv[])
 	}
 
 	if (!strcmp(argv[1], "status")) {
-		PX4_INFO("running");
-
 		estimator::g_estimator->print_status();
 
 		return 0;

src/modules/fw_att_control/CMakeLists.txt

@@ -38,7 +38,6 @@ px4_add_module(
 		-Os
 	SRCS
 		fw_att_control_main.cpp
-		fw_att_control_params.c
 	DEPENDS
 		platforms__common
 	)

src/modules/fw_att_control/fw_att_control_params.c

@@ -40,11 +40,6 @@
  * @author Thomas Gubler <thomas@px4.io>
  */
 
-#include <px4_config.h>
-
-#include <systemlib/param/param.h>
-
-
 /*
  * Controller parameters, accessible via MAVLink
  *

src/modules/fw_pos_control_l1/CMakeLists.txt

@@ -39,11 +39,9 @@ px4_add_module(
 		-Os
 	SRCS
 		fw_pos_control_l1_main.cpp
-		fw_pos_control_l1_params.c
 		landingslope.cpp
 		mtecs/mTecs.cpp
 		mtecs/limitoverride.cpp
-		mtecs/mTecs_params.c
 	DEPENDS
 		platforms__common
 		lib__external_lgpl

src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c

@@ -39,9 +39,6 @@
  * @author Lorenz Meier <lm@inf.ethz.ch>
  */
 
-#include <px4_config.h>
-#include <systemlib/param/param.h>
-
 /*
  * Controller parameters, accessible via MAVLink
  */

src/modules/fw_pos_control_l1/mtecs/mTecs_params.c

@@ -39,9 +39,6 @@
  * @author Thomas Gubler <thomasgubler@gmail.com>
  */
 
-#include <px4_config.h>
-#include <systemlib/param/param.h>
-
 /*
  * Controller parameters, accessible via MAVLink
  */

src/modules/mavlink/mavlink_parameters.cpp

@@ -41,27 +41,29 @@
 
 #include <stdio.h>
 
+#include <uORB/topics/uavcan_parameter_request.h>
+#include <uORB/topics/uavcan_parameter_value.h>
+
 #include "mavlink_parameters.h"
 #include "mavlink_main.h"
 
+ORB_DEFINE(uavcan_parameter_request, struct uavcan_parameter_request_s);
+ORB_DEFINE(uavcan_parameter_value, struct uavcan_parameter_value_s);
 #define HASH_PARAM "_HASH_CHECK"
 
 MavlinkParametersManager::MavlinkParametersManager(Mavlink *mavlink) : MavlinkStream(mavlink),
 	_send_all_index(-1),
 	_rc_param_map_pub(nullptr),
-	_rc_param_map()
+	_rc_param_map(),
+	_uavcan_parameter_request_pub(nullptr),
+	_uavcan_parameter_value_sub(-1)
 {
 }
 
 unsigned
 MavlinkParametersManager::get_size()
 {
-	if (_send_all_index >= 0) {
-		return MAVLINK_MSG_ID_PARAM_VALUE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
-
-	} else {
-		return 0;
-	}
+	return MAVLINK_MSG_ID_PARAM_VALUE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
 }
 
 void
@@ -78,36 +80,75 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
 
 				_send_all_index = 0;
 			}
+
+			if (req_list.target_system == mavlink_system.sysid && req_list.target_component < 127 &&
+			    (req_list.target_component != mavlink_system.compid || req_list.target_component == MAV_COMP_ID_ALL)) {
+				// publish list request to UAVCAN driver via uORB.
+				uavcan_parameter_request_s req;
+				req.message_type = msg->msgid;
+				req.node_id = req_list.target_component;
+				req.param_index = 0;
+
+				if (_uavcan_parameter_request_pub == nullptr) {
+					_uavcan_parameter_request_pub = orb_advertise(ORB_ID(uavcan_parameter_request), &req);
+				} else {
+					orb_publish(ORB_ID(uavcan_parameter_request), _uavcan_parameter_request_pub, &req);
+				}
+			}
 			break;
 		}
 
 	case MAVLINK_MSG_ID_PARAM_SET: {
 			/* set parameter */
-			if (msg->msgid == MAVLINK_MSG_ID_PARAM_SET) {
-				mavlink_param_set_t set;
-				mavlink_msg_param_set_decode(msg, &set);
+			mavlink_param_set_t set;
+			mavlink_msg_param_set_decode(msg, &set);
 
-				if (set.target_system == mavlink_system.sysid &&
-				    (set.target_component == mavlink_system.compid || set.target_component == MAV_COMP_ID_ALL)) {
+			if (set.target_system == mavlink_system.sysid &&
+			    (set.target_component == mavlink_system.compid || set.target_component == MAV_COMP_ID_ALL)) {
 
-					/* local name buffer to enforce null-terminated string */
-					char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1];
-					strncpy(name, set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
-					/* enforce null termination */
-					name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
-					/* attempt to find parameter, set and send it */
-					param_t param = param_find_no_notification(name);
+				/* local name buffer to enforce null-terminated string */
+				char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1];
+				strncpy(name, set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
+				/* enforce null termination */
+				name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
+				/* attempt to find parameter, set and send it */
+				param_t param = param_find_no_notification(name);
 
-					if (param == PARAM_INVALID) {
-						char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN];
-						sprintf(buf, "[pm] unknown param: %s", name);
-						_mavlink->send_statustext_info(buf);
+				if (param == PARAM_INVALID) {
+					char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN];
+					sprintf(buf, "[pm] unknown param: %s", name);
+					_mavlink->send_statustext_info(buf);
 
-					} else {
-						/* set and send parameter */
-						param_set(param, &(set.param_value));
-						send_param(param);
-					}
+				} else {
+					/* set and send parameter */
+					param_set(param, &(set.param_value));
+					send_param(param);
+				}
+			}
+
+			if (set.target_system == mavlink_system.sysid && set.target_component < 127 &&
+			    (set.target_component != mavlink_system.compid || set.target_component == MAV_COMP_ID_ALL)) {
+				// publish set request to UAVCAN driver via uORB.
+				uavcan_parameter_request_s req;
+				req.message_type = msg->msgid;
+				req.node_id = set.target_component;
+				req.param_index = -1;
+				strncpy(req.param_id, set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1);
+				req.param_id[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
+				if (set.param_type == MAV_PARAM_TYPE_REAL32) {
+					req.param_type = MAV_PARAM_TYPE_REAL32;
+					req.real_value = set.param_value;
+				} else {
+					int32_t val;
+					memcpy(&val, &set.param_value, sizeof(int32_t));
+					req.param_type = MAV_PARAM_TYPE_INT64;
+					req.int_value = val;
+				}
+
+				if (_uavcan_parameter_request_pub == nullptr) {
+					_uavcan_parameter_request_pub = orb_advertise(ORB_ID(uavcan_parameter_request), &req);
+				} else {
+					orb_publish(ORB_ID(uavcan_parameter_request), _uavcan_parameter_request_pub, &req);
 				}
 			}
 			break;
@@ -160,6 +201,23 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
 					}
 				}
 			}
+
+			if (req_read.target_system == mavlink_system.sysid && req_read.target_component < 127 &&
+				    (req_read.target_component != mavlink_system.compid || req_read.target_component == MAV_COMP_ID_ALL)) {
+				// publish set request to UAVCAN driver via uORB.
+				uavcan_parameter_request_s req;
+				req.message_type = msg->msgid;
+				req.node_id = req_read.target_component;
+				req.param_index = req_read.param_index;
+				strncpy(req.param_id, req_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1);
+				req.param_id[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
+
+				if (_uavcan_parameter_request_pub == nullptr) {
+					_uavcan_parameter_request_pub = orb_advertise(ORB_ID(uavcan_parameter_request), &req);
+				} else {
+					orb_publish(ORB_ID(uavcan_parameter_request), _uavcan_parameter_request_pub, &req);
+				}
+			}
 			break;
 		}
 
@@ -208,11 +266,38 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
 void
 MavlinkParametersManager::send(const hrt_abstime t)
 {
-	/* send all parameters if requested, but only after the system has booted */
-	if (_send_all_index >= 0 && _mavlink->boot_complete()) {
+	bool space_available = _mavlink->get_free_tx_buf() >= get_size();
+
+	/* Send parameter values received from the UAVCAN topic */
+	if (_uavcan_parameter_value_sub < 0) {
+		_uavcan_parameter_value_sub = orb_subscribe(ORB_ID(uavcan_parameter_value));
+	}
+
+	bool param_value_ready;
+	orb_check(_uavcan_parameter_value_sub, &param_value_ready);
+	if (space_available && param_value_ready) {
+		struct uavcan_parameter_value_s value;
+		orb_copy(ORB_ID(uavcan_parameter_value), _uavcan_parameter_value_sub, &value);
+
+		mavlink_param_value_t msg;
+		msg.param_count = value.param_count;
+		msg.param_index = value.param_index;
+		strncpy(msg.param_id, value.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
+		if (value.param_type == MAV_PARAM_TYPE_REAL32) {
+			msg.param_type = MAVLINK_TYPE_FLOAT;
+			msg.param_value = value.real_value;
+		} else {
+			int32_t val;
+			val = (int32_t)value.int_value;
+			memcpy(&msg.param_value, &val, sizeof(int32_t));
+			msg.param_type = MAVLINK_TYPE_INT32_T;
+		}
+		_mavlink->send_message(MAVLINK_MSG_ID_PARAM_VALUE, &msg, value.node_id);
+	} else if (_send_all_index >= 0 && _mavlink->boot_complete()) {
+		/* send all parameters if requested, but only after the system has booted */
 
 		/* skip if no space is available */
-		if (_mavlink->get_free_tx_buf() < get_size()) {
+		if (!space_available) {
 			return;
 		}
 

src/modules/mavlink/mavlink_parameters.h

@@ -118,4 +118,7 @@ protected:
 
 	orb_advert_t _rc_param_map_pub;
 	struct rc_parameter_map_s _rc_param_map;
+
+	orb_advert_t _uavcan_parameter_request_pub;
+	int _uavcan_parameter_value_sub;
 };

src/modules/mavlink/mavlink_receiver.cpp

@@ -1444,12 +1444,12 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 		hil_sensors.accelerometer_raw[0] = imu.xacc / mg2ms2;
 		hil_sensors.accelerometer_raw[1] = imu.yacc / mg2ms2;
 		hil_sensors.accelerometer_raw[2] = imu.zacc / mg2ms2;
-		hil_sensors.accelerometer_m_s2[0] = ((imu.xacc * dt + _hil_prev_accel[0]) / 2.0f) / dt;
-		hil_sensors.accelerometer_m_s2[1] = ((imu.yacc * dt + _hil_prev_accel[1]) / 2.0f) / dt;
-		hil_sensors.accelerometer_m_s2[2] = (((imu.zacc + 9.80665f) * dt + _hil_prev_accel[2]) / 2.0f) / dt - 9.80665f;
+		hil_sensors.accelerometer_m_s2[0] = imu.xacc;
+		hil_sensors.accelerometer_m_s2[1] = imu.yacc;
+		hil_sensors.accelerometer_m_s2[2] = imu.zacc;
 		hil_sensors.accelerometer_integral_m_s[0] = (imu.xacc * dt + _hil_prev_accel[0]) / 2.0f;
 		hil_sensors.accelerometer_integral_m_s[1] = (imu.yacc * dt + _hil_prev_accel[1]) / 2.0f;
-		hil_sensors.accelerometer_integral_m_s[2] = ((imu.zacc + 9.80665f) * dt + _hil_prev_accel[2]) / 2.0f;
+		hil_sensors.accelerometer_integral_m_s[2] = (imu.zacc * dt + _hil_prev_accel[2]) / 2.0f;
 		memcpy(&_hil_prev_accel[0], &hil_sensors.accelerometer_integral_m_s[0], sizeof(_hil_prev_accel));
 		hil_sensors.accelerometer_integral_dt[0] = dt * 1e6f;
 		hil_sensors.accelerometer_mode[0] = 0; // TODO what is this?

src/modules/mavlink/mavlink_tests/module.mk

@@ -1,53 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2014 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# System state machine tests.
-#
-
-MODULE_COMMAND		= mavlink_tests
-SRCS			= mavlink_tests.cpp \
-			mavlink_ftp_test.cpp \
-			../mavlink_stream.cpp \
-			../mavlink_ftp.cpp \
-			../mavlink.c
-
-INCLUDE_DIRS	 += $(MAVLINK_SRC)/include/mavlink
-
-MODULE_STACKSIZE = 5000
-
-MAXOPTIMIZATION	 = -Os
-
-EXTRACXXFLAGS	= -Weffc++ -DMAVLINK_FTP_UNIT_TEST -Wno-attributes -Wno-packed
-
-EXTRACFLAGS	= -Wno-packed

src/modules/navigator/CMakeLists.txt

@@ -39,24 +39,17 @@ px4_add_module(
 		-Os
 	SRCS
 		navigator_main.cpp
-		navigator_params.c
 		navigator_mode.cpp
 		mission_block.cpp
 		mission.cpp
-		mission_params.c
 		loiter.cpp
 		rtl.cpp
-		rtl_params.c
 		mission_feasibility_checker.cpp
 		geofence.cpp
-		geofence_params.c
 		datalinkloss.cpp
-		datalinkloss_params.c
 		rcloss.cpp
-		rcloss_params.c
 		enginefailure.cpp
 		gpsfailure.cpp
-		gpsfailure_params.c
 	DEPENDS
 		platforms__common
 	)

src/modules/navigator/datalinkloss_params.c

@@ -39,10 +39,6 @@
  * @author Thomas Gubler <thomas@px4.io>
  */
 
-#include <px4_config.h>
-
-#include <systemlib/param/param.h>
-
 /*
  * Data Link Loss parameters, accessible via MAVLink
  */

src/modules/navigator/geofence_params.c

@@ -39,10 +39,6 @@
  * @author Thomas Gubler <thomasgubler@gmail.com>
  */
 
-#include <px4_config.h>
-
-#include <systemlib/param/param.h>
-
 /*
  * Geofence parameters, accessible via MAVLink
  */

src/modules/navigator/gpsfailure_params.c

@@ -39,10 +39,6 @@
  * @author Thomas Gubler <thomasgubler@gmail.com>
  */
 
-#include <px4_config.h>
-
-#include <systemlib/param/param.h>
-
 /*
  * GPS Failure Navigation Mode parameters, accessible via MAVLink
  */

src/modules/navigator/mission_feasibility_checker.cpp

@@ -336,7 +336,7 @@ MissionFeasibilityChecker::check_dist_1wp(dm_item_t dm_current, size_t nMissionI
 
 					} else {
 						/* item is too far from home */
-						mavlink_log_critical(_mavlink_fd, "Waypoint too far: %d m,[MIS_DIST_1WP=%d]", (int)dist_to_1wp, (int)dist_first_wp);
+						mavlink_log_critical(_mavlink_fd, "First waypoint too far: %d m,refusing mission", (int)dist_to_1wp, (int)dist_first_wp);
 						warning_issued = true;
 						return false;
 					}

src/modules/navigator/mission_params.c

@@ -39,10 +39,6 @@
  * @author Julian Oes <joes@student.ethz.ch>
  */
 
-#include <px4_config.h>
-
-#include <systemlib/param/param.h>
-
 /*
  * Mission parameters, accessible via MAVLink
  */

src/modules/navigator/navigator_params.c

@@ -40,10 +40,6 @@
  * @author Thomas Gubler <thomas@px4.io>
  */
 
-#include <px4_config.h>
-
-#include <systemlib/param/param.h>
-
 /**
  * Loiter radius (FW only)
  *

src/modules/navigator/rcloss_params.c

@@ -39,10 +39,6 @@
  * @author Thomas Gubler <thomasgubler@gmail.com>
  */
 
-#include <px4_config.h>
-
-#include <systemlib/param/param.h>
-
 /*
  * OBC RC Loss mode parameters, accessible via MAVLink
  */

src/modules/navigator/rtl_params.c

@@ -39,10 +39,6 @@
  * @author Julian Oes <julian@oes.ch>
  */
 
-#include <px4_config.h>
-
-#include <systemlib/param/param.h>
-
 /*
  * RTL parameters, accessible via MAVLink
  */

src/modules/position_estimator_inav/position_estimator_inav_main.c

@@ -897,7 +897,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 		}
 
 		float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f;
-		dt = fmaxf(fminf(0.02, dt), 0.002);		// constrain dt from 2 to 20 ms
+		dt = fmaxf(fminf(0.02, dt), 0.0002);		// constrain dt from 0.2 to 20 ms
 		t_prev = t;
 
 		/* increase EPH/EPV on each step */

src/modules/sdlog2/logbuffer.c

@@ -50,8 +50,8 @@ int logbuffer_init(struct logbuffer_s *lb, int size)
 	lb->size  = size;
 	lb->write_ptr = 0;
 	lb->read_ptr = 0;
-	lb->data = malloc(lb->size);
-	return (lb->data == 0) ? PX4_ERROR : PX4_OK;
+	lb->data = NULL;
+	return PX4_OK;
 }
 
 int logbuffer_count(struct logbuffer_s *lb)
@@ -72,6 +72,16 @@ int logbuffer_is_empty(struct logbuffer_s *lb)
 
 bool logbuffer_write(struct logbuffer_s *lb, void *ptr, int size)
 {
+	// allocate buffer if not yet present
+	if (lb->data == NULL) {
+		lb->data = malloc(lb->size);
+	}
+
+	// allocation failed, bail out
+	if (lb->data == NULL) {
+		return false;
+	}
+
 	// bytes available to write
 	int available = lb->read_ptr - lb->write_ptr - 1;
 

src/modules/sensors/CMakeLists.txt

@@ -40,7 +40,6 @@ px4_add_module(
 		-O3
 	SRCS
 		sensors.cpp
-		sensor_params.c
 	DEPENDS
 		platforms__common
 	)

src/modules/sensors/sensor_params.c

@@ -585,6 +585,34 @@ PARAM_DEFINE_FLOAT(CAL_ACC2_YSCALE, 1.0f);
  */
 PARAM_DEFINE_FLOAT(CAL_ACC2_ZSCALE, 1.0f);
 
+/**
+ * Primary accel ID
+ *
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_INT32(CAL_ACC_PRIME, 0);
+
+/**
+ * Primary gyro ID
+ *
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_INT32(CAL_GYRO_PRIME, 0);
+
+/**
+ * Primary mag ID
+ *
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_INT32(CAL_MAG_PRIME, 0);
+
+/**
+ * Primary baro ID
+ *
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_INT32(CAL_BARO_PRIME, 0);
+
 /**
  * Differential pressure sensor offset
  *

src/modules/sensors/sensors.cpp

@@ -1412,6 +1412,8 @@ Sensors::parameter_update_poll(bool forced)
 				(void)sprintf(str, "CAL_MAG%u_ID", i);
 				int device_id;
 				failed = failed || (OK != param_get(param_find(str), &device_id));
+				(void)sprintf(str, "CAL_MAG%u_ROT", i);
+				(void)param_find(str);
 
 				if (failed) {
 					px4_close(fd);
@@ -2020,6 +2022,11 @@ Sensors::task_main()
 	 * do subscriptions
 	 */
 
+	unsigned gcount_prev = _gyro_count;
+	unsigned mcount_prev = _mag_count;
+	unsigned acount_prev = _accel_count;
+	unsigned bcount_prev = _baro_count;
+
 	_gyro_count = init_sensor_class(ORB_ID(sensor_gyro), &_gyro_sub[0],
 		&raw.gyro_priority[0], &raw.gyro_errcount[0]);
 
@@ -2032,6 +2039,15 @@ Sensors::task_main()
 	_baro_count = init_sensor_class(ORB_ID(sensor_baro), &_baro_sub[0],
 		&raw.baro_priority[0], &raw.baro_errcount[0]);
 
+	if (gcount_prev != _gyro_count ||
+	    mcount_prev != _mag_count ||
+	    acount_prev != _accel_count ||
+	    bcount_prev != _baro_count) {
+
+		/* reload calibration params */
+		parameter_update_poll(true);
+	}
+
 	_rc_sub = orb_subscribe(ORB_ID(input_rc));
 	_diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
 	_vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));

src/modules/uavcan/CMakeLists.txt

@@ -63,7 +63,7 @@ px4_add_module(
 	COMPILE_FLAGS
 		-Wframe-larger-than=1500
 		-Wno-deprecated-declarations
-		-O3
+		-Os
 	SRCS
 		# Main
 		uavcan_main.cpp
@@ -81,7 +81,7 @@ px4_add_module(
 
 	DEPENDS
 		platforms__common
-		uavcan	
+		uavcan
 	)
 
 ## vim: set noet ft=cmake fenc=utf-8 ff=unix : 

src/modules/uavcan/uavcan_main.hpp

@@ -94,7 +94,7 @@ class UavcanNode : public device::CDev
 	 */
 
 	static constexpr unsigned RxQueueLenPerIface = FramePerMSecond * PollTimeoutMs; // At
-	static constexpr unsigned StackSize          = 1600;
+	static constexpr unsigned StackSize          = 1800;
 
 public:
 	typedef uavcan::Node<MemPoolSize> Node;

src/modules/uavcan/uavcan_servers.cpp

@@ -36,6 +36,7 @@
 #include <cstdlib>
 #include <cstring>
 #include <fcntl.h>
+#include <dirent.h>
 #include <memory>
 #include <pthread.h>
 #include <systemlib/err.h>
@@ -53,14 +54,22 @@
 #include "uavcan_servers.hpp"
 #include "uavcan_virtual_can_driver.hpp"
 
-# include <uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp>
-# include <uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp>
-# include <uavcan_posix/firmware_version_checker.hpp>
+#include <uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp>
+#include <uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp>
+#include <uavcan_posix/firmware_version_checker.hpp>
+
+#include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/uavcan_parameter_request.h>
+#include <uORB/topics/uavcan_parameter_value.h>
+
+#include <v1.0/common/mavlink.h>
 
 //todo:The Inclusion of file_server_backend is killing
 // #include <sys/types.h> and leaving OK undefined
 # define OK 0
 
+
+
 /**
  * @file uavcan_servers.cpp
  *
@@ -87,8 +96,29 @@ UavcanServers::UavcanServers(uavcan::INode &main_node) :
 	_node_info_retriever(_subnode),
 	_fw_upgrade_trigger(_subnode, _fw_version_checker),
 	_fw_server(_subnode, _fileserver_backend),
+	_count_in_progress(false),
+	_count_index(0),
+	_param_in_progress(0),
+	_param_index(0),
+	_param_list_in_progress(false),
+	_param_list_all_nodes(false),
+	_param_list_node_id(1),
+	_param_dirty_bitmap{0, 0, 0, 0},
+	_param_save_opcode(0),
+	_cmd_in_progress(false),
+	_param_response_pub(nullptr),
+	_param_getset_client(_subnode),
+	_param_opcode_client(_subnode),
+	_param_restartnode_client(_subnode),
 	_mutex_inited(false),
-	_check_fw(false)
+	_check_fw(false),
+	_esc_enumeration_active(false),
+	_esc_enumeration_index(0),
+	_beep_pub(_subnode),
+	_enumeration_indication_sub(_subnode),
+	_enumeration_client(_subnode),
+	_enumeration_getset_client(_subnode),
+	_enumeration_save_client(_subnode)
 
 {
 }
@@ -270,13 +300,40 @@ int UavcanServers::init()
 	return OK;
 }
 
-__attribute__((optimize("-O0")))
 pthread_addr_t UavcanServers::run(pthread_addr_t)
 {
 	prctl(PR_SET_NAME, "uavcan fw srv", 0);
 
 	Lock lock(_subnode_mutex);
 
+	/*
+	Copy any firmware bundled in the ROMFS to the appropriate location on the
+	SD card, unless the user has copied other firmware for that device.
+	*/
+	unpackFwFromROMFS(UAVCAN_FIRMWARE_PATH, UAVCAN_ROMFS_FW_PATH);
+
+	/* the subscribe call needs to happen in the same thread,
+	 * so not in the constructor */
+	int cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
+	int param_request_sub = orb_subscribe(ORB_ID(uavcan_parameter_request));
+	int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
+
+	/* Set up shared service clients */
+	_param_getset_client.setCallback(GetSetCallback(this, &UavcanServers::cb_getset));
+	_param_opcode_client.setCallback(ExecuteOpcodeCallback(this, &UavcanServers::cb_opcode));
+	_param_restartnode_client.setCallback(RestartNodeCallback(this, &UavcanServers::cb_restart));
+	_enumeration_client.setCallback(EnumerationBeginCallback(this, &UavcanServers::cb_enumeration_begin));
+	_enumeration_indication_sub.start(EnumerationIndicationCallback(this, &UavcanServers::cb_enumeration_indication));
+	_enumeration_getset_client.setCallback(GetSetCallback(this, &UavcanServers::cb_enumeration_getset));
+	_enumeration_save_client.setCallback(ExecuteOpcodeCallback(this, &UavcanServers::cb_enumeration_save));
+
+	_count_in_progress = _param_in_progress = _param_list_in_progress = _cmd_in_progress = _param_list_all_nodes = false;
+	memset(_param_counts, 0, sizeof(_param_counts));
+
+	_esc_enumeration_active = false;
+	memset(_esc_enumeration_ids, 0, sizeof(_esc_enumeration_ids));
+	_esc_enumeration_index = 0;
+
 	while (1) {
 
 		if (_check_fw == true) {
@@ -284,14 +341,799 @@ pthread_addr_t UavcanServers::run(pthread_addr_t)
 			_node_info_retriever.invalidateAll();
 		}
 
-		const int spin_res = _subnode.spin(uavcan::MonotonicDuration::fromMSec(100));
-
+		const int spin_res = _subnode.spin(uavcan::MonotonicDuration::fromMSec(10));
 		if (spin_res < 0) {
 			warnx("node spin error %i", spin_res);
 		}
+
+		// Check for parameter requests (get/set/list)
+		bool param_request_ready;
+		orb_check(param_request_sub, &param_request_ready);
+
+		if (param_request_ready && !_param_list_in_progress && !_param_in_progress && !_count_in_progress) {
+			struct uavcan_parameter_request_s request;
+			orb_copy(ORB_ID(uavcan_parameter_request), param_request_sub, &request);
+
+			if (_param_counts[request.node_id]) {
+				/*
+				 * We know how many parameters are exposed by this node, so
+				 * process the request.
+				 */
+				if (request.message_type == MAVLINK_MSG_ID_PARAM_REQUEST_READ) {
+					uavcan::protocol::param::GetSet::Request req;
+					if (request.param_index >= 0) {
+						req.index = request.param_index;
+					} else {
+						req.name = (char*)request.param_id;
+					}
+
+					int call_res = _param_getset_client.call(request.node_id, req);
+					if (call_res < 0) {
+						warnx("UAVCAN command bridge: couldn't send GetSet: %d", call_res);
+					} else {
+						_param_in_progress = true;
+						_param_index = request.param_index;
+					}
+				} else if (request.message_type == MAVLINK_MSG_ID_PARAM_SET) {
+					uavcan::protocol::param::GetSet::Request req;
+					if (request.param_index >= 0) {
+						req.index = request.param_index;
+					} else {
+						req.name = (char*)request.param_id;
+					}
+
+					if (request.param_type == MAV_PARAM_TYPE_REAL32) {
+						req.value.to<uavcan::protocol::param::Value::Tag::real_value>() = request.real_value;
+					} else if (request.param_type == MAV_PARAM_TYPE_UINT8) {
+						req.value.to<uavcan::protocol::param::Value::Tag::boolean_value>() = request.int_value;
+					} else {
+						req.value.to<uavcan::protocol::param::Value::Tag::integer_value>() = request.int_value;
+					}
+
+					// Set the dirty bit for this node
+					set_node_params_dirty(request.node_id);
+
+					int call_res = _param_getset_client.call(request.node_id, req);
+					if (call_res < 0) {
+						warnx("UAVCAN command bridge: couldn't send GetSet: %d", call_res);
+					} else {
+						_param_in_progress = true;
+						_param_index = request.param_index;
+					}
+				} else if (request.message_type == MAVLINK_MSG_ID_PARAM_REQUEST_LIST) {
+					// This triggers the _param_list_in_progress case below.
+					_param_index = 0;
+					_param_list_in_progress = true;
+					_param_list_node_id = request.node_id;
+					_param_list_all_nodes = false;
+
+					warnx("UAVCAN command bridge: starting component-specific param list");
+				}
+			} else if (request.node_id == MAV_COMP_ID_ALL) {
+				if (request.message_type == MAVLINK_MSG_ID_PARAM_REQUEST_LIST) {
+					/*
+					 * This triggers the _param_list_in_progress case below,
+					 * but additionally iterates over all active nodes.
+					 */
+					_param_index = 0;
+					_param_list_in_progress = true;
+					_param_list_node_id = get_next_active_node_id(1);
+					_param_list_all_nodes = true;
+
+					warnx("UAVCAN command bridge: starting global param list with node %hhu", _param_list_node_id);
+
+					if (_param_counts[_param_list_node_id] == 0) {
+						param_count(_param_list_node_id);
+					}
+				}
+			} else {
+				/*
+				 * Need to know how many parameters this node has before we can
+				 * continue; count them now and then process the request.
+				 */
+				param_count(request.node_id);
+			}
+		}
+
+		// Handle parameter listing index/node ID advancement
+		if (_param_list_in_progress && !_param_in_progress && !_count_in_progress) {
+			if (_param_index >= _param_counts[_param_list_node_id]) {
+				warnx("UAVCAN command bridge: completed param list for node %hhu", _param_list_node_id);
+				// Reached the end of the current node's parameter set.
+				_param_list_in_progress = false;
+
+				if (_param_list_all_nodes) {
+					// We're listing all parameters for all nodes -- get the next node ID
+					uint8_t next_id = get_next_active_node_id(_param_list_node_id);
+					if (next_id < 128) {
+						_param_list_node_id = next_id;
+						/*
+						 * If there is a next node ID, check if that node's parameters
+						 * have been counted before. If not, do it now.
+						 */
+						if (_param_counts[_param_list_node_id] == 0) {
+							param_count(_param_list_node_id);
+						}
+						// Keep on listing.
+						_param_index = 0;
+						_param_list_in_progress = true;
+						warnx("UAVCAN command bridge: started param list for node %hhu", _param_list_node_id);
+					}
+				}
+			}
+		}
+
+		// Check if we're still listing, and need to get the next parameter
+		if (_param_list_in_progress && !_param_in_progress && !_count_in_progress) {
+			// Ready to request the next value -- _param_index is incremented
+			// after each successful fetch by cb_getset
+			uavcan::protocol::param::GetSet::Request req;
+			req.index = _param_index;
+
+			int call_res = _param_getset_client.call(_param_list_node_id, req);
+			if (call_res < 0) {
+				_param_list_in_progress = false;
+				warnx("UAVCAN command bridge: couldn't send param list GetSet: %d", call_res);
+			} else {
+				_param_in_progress = true;
+			}
+		}
+
+		// Check for ESC enumeration commands
+		bool cmd_ready;
+		orb_check(cmd_sub, &cmd_ready);
+
+		if (cmd_ready && !_cmd_in_progress) {
+			struct vehicle_command_s cmd;
+			orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
+
+			if (cmd.command == vehicle_command_s::VEHICLE_CMD_PREFLIGHT_UAVCAN) {
+				int command_id = static_cast<int>(cmd.param1 + 0.5f);
+				int node_id = static_cast<int>(cmd.param2 + 0.5f);
+				int call_res;
+
+				warnx("UAVCAN command bridge: received UAVCAN command ID %d, node ID %d", command_id, node_id);
+
+				switch (command_id) {
+				case 0:
+				case 1: {
+						_esc_enumeration_active = command_id;
+						_esc_enumeration_index = 0;
+						_esc_count = 0;
+						uavcan::protocol::enumeration::Begin::Request req;
+						req.parameter_name = "esc_index";
+						req.timeout_sec = _esc_enumeration_active ? 65535 : 0;
+						call_res = _enumeration_client.call(get_next_active_node_id(1), req);
+						if (call_res < 0) {
+							warnx("UAVCAN ESC enumeration: couldn't send initial Begin request: %d", call_res);
+						}
+						break;
+					}
+				default: {
+						warnx("UAVCAN command bridge: unknown command ID %d", command_id);
+						break;
+					}
+				}
+			} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_PREFLIGHT_STORAGE) {
+				int command_id = static_cast<int>(cmd.param1 + 0.5f);
+
+				warnx("UAVCAN command bridge: received storage command ID %d", command_id);
+
+				switch (command_id) {
+				case 1: {
+						// Param save request
+						_param_save_opcode = uavcan::protocol::param::ExecuteOpcode::Request::OPCODE_SAVE;
+						param_opcode(get_next_dirty_node_id(1));
+						break;
+					}
+				case 2: {
+						// Command is a param erase request -- apply it to all active nodes by setting the dirty bit
+						_param_save_opcode = uavcan::protocol::param::ExecuteOpcode::Request::OPCODE_ERASE;
+						for (int i = 1; i < 128; i = get_next_active_node_id(i)) {
+							set_node_params_dirty(i);
+						}
+						param_opcode(get_next_dirty_node_id(1));
+						break;
+					}
+				}
+			}
+		}
+
+		// Shut down once armed
+		// TODO (elsewhere): start up again once disarmed?
+		bool updated;
+		orb_check(armed_sub, &updated);
+		if (updated) {
+			struct actuator_armed_s armed;
+			orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
+
+			if (armed.armed && !armed.lockdown) {
+				warnx("UAVCAN command bridge: system armed, exiting now.");
+				break;
+			}
+		}
 	}
 
 	warnx("exiting.");
 	return (pthread_addr_t) 0;
 }
 
+void UavcanServers::cb_getset(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &result)
+{
+	if (_count_in_progress) {
+		/*
+		 * Currently in parameter count mode:
+		 * Iterate over all parameters for the node to which the request was
+		 * originally sent, in order to find the maximum parameter ID. If a
+		 * request fails, set the node's parameter count to zero.
+		 */
+		uint8_t node_id = result.getCallID().server_node_id.get();
+
+		if (result.isSuccessful()) {
+			uavcan::protocol::param::GetSet::Response resp = result.getResponse();
+			if (resp.name.size()) {
+				_param_counts[node_id] = _count_index++;
+
+				uavcan::protocol::param::GetSet::Request req;
+				req.index = _count_index;
+
+				int call_res = _param_getset_client.call(result.getCallID().server_node_id, req);
+				if (call_res < 0) {
+					_count_in_progress = false;
+					_count_index = 0;
+					warnx("UAVCAN command bridge: couldn't send GetSet during param count: %d", call_res);
+				}
+			} else {
+				_count_in_progress = false;
+				_count_index = 0;
+				warnx("UAVCAN command bridge: completed param count for node %hhu: %hhu", node_id, _param_counts[node_id]);
+			}
+		} else {
+			_param_counts[node_id] = 0;
+			_count_in_progress = false;
+			_count_index = 0;
+			warnx("UAVCAN command bridge: GetSet error during param count");
+		}
+	} else {
+		/*
+		 * Currently in parameter get/set mode:
+		 * Publish a uORB uavcan_parameter_value message containing the current value
+		 * of the parameter.
+		 */
+		if (result.isSuccessful()) {
+			uavcan::protocol::param::GetSet::Response param = result.getResponse();
+
+			struct uavcan_parameter_value_s response;
+			response.node_id = result.getCallID().server_node_id.get();
+			strncpy(response.param_id, param.name.c_str(), sizeof(response.param_id) - 1);
+			response.param_id[16] = '\0';
+			response.param_index = _param_index;
+			response.param_count = _param_counts[response.node_id];
+
+			if (param.value.is(uavcan::protocol::param::Value::Tag::integer_value)) {
+				response.param_type = MAV_PARAM_TYPE_INT64;
+				response.int_value = param.value.to<uavcan::protocol::param::Value::Tag::integer_value>();
+			} else if (param.value.is(uavcan::protocol::param::Value::Tag::real_value)) {
+				response.param_type = MAV_PARAM_TYPE_REAL32;
+				response.real_value = param.value.to<uavcan::protocol::param::Value::Tag::real_value>();
+			} else if (param.value.is(uavcan::protocol::param::Value::Tag::boolean_value)) {
+				response.param_type = MAV_PARAM_TYPE_UINT8;
+				response.int_value = param.value.to<uavcan::protocol::param::Value::Tag::boolean_value>();
+			}
+
+			if (_param_response_pub == nullptr) {
+				_param_response_pub = orb_advertise(ORB_ID(uavcan_parameter_value), &response);
+			} else {
+				orb_publish(ORB_ID(uavcan_parameter_value), _param_response_pub, &response);
+			}
+		} else {
+			warnx("UAVCAN command bridge: GetSet error");
+		}
+
+		_param_in_progress = false;
+		_param_index++;
+	}
+}
+
+void UavcanServers::param_count(uavcan::NodeID node_id)
+{
+	uavcan::protocol::param::GetSet::Request req;
+	req.index = 0;
+	int call_res = _param_getset_client.call(node_id, req);
+	if (call_res < 0) {
+		warnx("UAVCAN command bridge: couldn't start parameter count: %d", call_res);
+	} else {
+		_count_in_progress = true;
+		_count_index = 0;
+		warnx("UAVCAN command bridge: starting param count");
+	}
+}
+
+void UavcanServers::param_opcode(uavcan::NodeID node_id)
+{
+	uavcan::protocol::param::ExecuteOpcode::Request opcode_req;
+	opcode_req.opcode = _param_save_opcode;
+	int call_res = _param_opcode_client.call(node_id, opcode_req);
+	if (call_res < 0) {
+		warnx("UAVCAN command bridge: couldn't send ExecuteOpcode: %d", call_res);
+	} else {
+		_cmd_in_progress = true;
+		warnx("UAVCAN command bridge: sent ExecuteOpcode");
+	}
+}
+
+void UavcanServers::cb_opcode(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &result)
+{
+	bool success = result.isSuccessful();
+	uint8_t node_id = result.getCallID().server_node_id.get();
+	uavcan::protocol::param::ExecuteOpcode::Response resp = result.getResponse();
+	success &= resp.ok;
+	_cmd_in_progress = false;
+
+	if (!result.isSuccessful()) {
+		warnx("UAVCAN command bridge: save request for node %hhu timed out.", node_id);
+	} else if (!result.getResponse().ok) {
+		warnx("UAVCAN command bridge: save request for node %hhu rejected.", node_id);
+	} else {
+		warnx("UAVCAN command bridge: save request for node %hhu completed OK, restarting.", node_id);
+
+		uavcan::protocol::RestartNode::Request restart_req;
+		restart_req.magic_number = restart_req.MAGIC_NUMBER;
+		int call_res = _param_restartnode_client.call(node_id, restart_req);
+		if (call_res < 0) {
+			warnx("UAVCAN command bridge: couldn't send RestartNode: %d", call_res);
+		} else {
+			warnx("UAVCAN command bridge: sent RestartNode");
+			_cmd_in_progress = true;
+		}
+	}
+
+	if (!_cmd_in_progress) {
+		/*
+		 * Something went wrong, so cb_restart is never going to be called as a result of this request.
+		 * To ensure we try to execute the opcode on all nodes that permit it, get the next dirty node
+		 * ID and keep processing here. The dirty bit on the current node is still set, so the
+		 * save/erase attempt will occur when the next save/erase command is received over MAVLink.
+		 */
+		node_id = get_next_dirty_node_id(node_id);
+		if (node_id < 128) {
+			param_opcode(node_id);
+		}
+	}
+}
+
+void UavcanServers::cb_restart(const uavcan::ServiceCallResult<uavcan::protocol::RestartNode> &result)
+{
+	bool success = result.isSuccessful();
+	uint8_t node_id = result.getCallID().server_node_id.get();
+	uavcan::protocol::RestartNode::Response resp = result.getResponse();
+	success &= resp.ok;
+	_cmd_in_progress = false;
+
+	if (success) {
+		warnx("UAVCAN command bridge: restart request for node %hhu completed OK.", node_id);
+		// Clear the dirty flag
+		clear_node_params_dirty(node_id);
+	} else {
+		warnx("UAVCAN command bridge: restart request for node %hhu failed.", node_id);
+	}
+
+	// Get the next dirty node ID and send the same command to it
+	node_id = get_next_dirty_node_id(node_id);
+	if (node_id < 128) {
+		param_opcode(node_id);
+	}
+}
+
+uint8_t UavcanServers::get_next_active_node_id(uint8_t base)
+{
+	base++;
+	for (; base < 128 && (!_node_info_retriever.isNodeKnown(base) ||
+						   _subnode.getNodeID().get() == base); base++);
+	return base;
+}
+
+uint8_t UavcanServers::get_next_dirty_node_id(uint8_t base)
+{
+	base++;
+	for (; base < 128 && !are_node_params_dirty(base); base++);
+	return base;
+}
+
+void UavcanServers::cb_enumeration_begin(const uavcan::ServiceCallResult<uavcan::protocol::enumeration::Begin> &result)
+{
+	uint8_t next_id = get_next_active_node_id(result.getCallID().server_node_id.get());
+
+	if (!result.isSuccessful()) {
+		warnx("UAVCAN ESC enumeration: begin request for node %hhu timed out.", result.getCallID().server_node_id.get());
+	} else if (result.getResponse().error) {
+		warnx("UAVCAN ESC enumeration: begin request for node %hhu rejected: %hhu", result.getCallID().server_node_id.get(), result.getResponse().error);
+	} else {
+		_esc_count++;
+		warnx("UAVCAN ESC enumeration: begin request for node %hhu completed OK.", result.getCallID().server_node_id.get());
+	}
+
+	if (next_id < 128) {
+		// Still other active nodes to send the request to
+		uavcan::protocol::enumeration::Begin::Request req;
+		req.parameter_name = "esc_index";
+		req.timeout_sec = _esc_enumeration_active ? 65535 : 0;
+
+		int call_res = _enumeration_client.call(next_id, req);
+		if (call_res < 0) {
+			warnx("UAVCAN ESC enumeration: couldn't send Begin request: %d", call_res);
+		} else {
+			warnx("UAVCAN ESC enumeration: sent Begin request");
+		}
+	} else {
+		warnx("UAVCAN ESC enumeration: begun enumeration on all nodes.");
+	}
+}
+
+void UavcanServers::cb_enumeration_indication(const uavcan::ReceivedDataStructure<uavcan::protocol::enumeration::Indication> &msg)
+{
+	// Called whenever an ESC thinks it has received user input.
+	warnx("UAVCAN ESC enumeration: got indication");
+
+	if (!_esc_enumeration_active) {
+		// Ignore any messages received when we're not expecting them
+		return;
+	}
+
+	// First, check if we've already seen an indication from this ESC. If so,
+	// just re-issue the previous get/set request.
+	int i;
+	for (i = 0; i < _esc_enumeration_index; i++) {
+		if (_esc_enumeration_ids[i] == msg.getSrcNodeID().get()) {
+			warnx("UAVCAN ESC enumeration: already enumerated ESC ID %hhu as index %d", _esc_enumeration_ids[i], i);
+			break;
+		}
+	}
+
+	uavcan::protocol::param::GetSet::Request req;
+	req.name = "esc_index";
+	req.value.to<uavcan::protocol::param::Value::Tag::integer_value>() = i;
+
+	int call_res = _enumeration_getset_client.call(msg.getSrcNodeID(), req);
+	if (call_res < 0) {
+		warnx("UAVCAN ESC enumeration: couldn't send GetSet: %d", call_res);
+	} else {
+		warnx("UAVCAN ESC enumeration: sent GetSet to node %hhu (index %d)", _esc_enumeration_ids[i], i);
+	}
+}
+
+void UavcanServers::cb_enumeration_getset(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &result)
+{
+	if (!result.isSuccessful()) {
+		warnx("UAVCAN ESC enumeration: save request for node %hhu timed out.", result.getCallID().server_node_id.get());
+	} else {
+		warnx("UAVCAN ESC enumeration: save request for node %hhu completed OK.", result.getCallID().server_node_id.get());
+
+		uavcan::protocol::param::GetSet::Response resp = result.getResponse();
+		uint8_t esc_index = (uint8_t)resp.value.to<uavcan::protocol::param::Value::Tag::integer_value>();
+		esc_index = std::min((uint8_t)(uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize - 1), esc_index);
+		_esc_enumeration_index = std::max(_esc_enumeration_index, (uint8_t)(esc_index + 1));
+
+		_esc_enumeration_ids[esc_index] = result.getCallID().server_node_id.get();
+
+		uavcan::protocol::param::ExecuteOpcode::Request opcode_req;
+		opcode_req.opcode = opcode_req.OPCODE_SAVE;
+		int call_res = _enumeration_save_client.call(result.getCallID().server_node_id, opcode_req);
+		if (call_res < 0) {
+			warnx("UAVCAN ESC enumeration: couldn't send ExecuteOpcode: %d", call_res);
+		} else {
+			warnx("UAVCAN ESC enumeration: sent ExecuteOpcode to node %hhu (index %hhu)", _esc_enumeration_ids[esc_index], esc_index);
+		}
+	}
+}
+
+void UavcanServers::cb_enumeration_save(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &result)
+{
+	uavcan::equipment::indication::BeepCommand beep;
+
+	if (!result.isSuccessful()) {
+		warnx("UAVCAN ESC enumeration: save request for node %hhu timed out.", result.getCallID().server_node_id.get());
+		beep.frequency = 880.0f;
+		beep.duration = 1.0f;
+	} else if (!result.getResponse().ok) {
+		warnx("UAVCAN ESC enumeration: save request for node %hhu rejected", result.getCallID().server_node_id.get());
+		beep.frequency = 880.0f;
+		beep.duration = 1.0f;
+	} else {
+		warnx("UAVCAN ESC enumeration: save request for node %hhu completed OK.", result.getCallID().server_node_id.get());
+		beep.frequency = 440.0f;
+		beep.duration = 0.25f;
+	}
+
+	(void)_beep_pub.broadcast(beep);
+
+	warnx("UAVCAN ESC enumeration: completed %hhu of %hhu", _esc_enumeration_index, _esc_count);
+
+	if (_esc_enumeration_index == uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize - 1 ||
+			_esc_enumeration_index == _esc_count) {
+		_esc_enumeration_active = false;
+
+		// Tell all ESCs to stop enumerating
+		uavcan::protocol::enumeration::Begin::Request req;
+		req.parameter_name = "esc_index";
+		req.timeout_sec = 0;
+		int call_res = _enumeration_client.call(get_next_active_node_id(1), req);
+		if (call_res < 0) {
+			warnx("UAVCAN ESC enumeration: couldn't send Begin request to stop enumeration: %d", call_res);
+		} else {
+			warnx("UAVCAN ESC enumeration: sent Begin request to stop enumeration");
+		}
+	}
+}
+
+void UavcanServers::unpackFwFromROMFS(const char* sd_path, const char* romfs_path) {
+	/*
+	Copy the ROMFS firmware directory to the appropriate location on SD, without
+	overriding any firmware the user has already loaded there.
+
+	The SD firmware directory structure is along the lines of:
+
+		/fs/microsd/fw
+		-	/c
+		-	-	/1a2b3c4d.bin			cache file (copy of /fs/microsd/fw/org.pixhawk.nodename-v1/1.1/1a2b3c4d.bin)
+		-	/org.pixhawk.nodename-v1	device directory for org.pixhawk.nodename-v1
+		-	-	/1.0					version directory for hardware 1.0
+		-	-	/1.1					version directory for hardware 1.1
+		-	-	-	/1a2b3c4d.bin		firmware file for org.pixhawk.nodename-v1 nodes, hardware version 1.1
+		-	/com.example.othernode-v3	device directory for com.example.othernode-v3
+		-	-	/1.0					version directory for hardawre 1.0
+		-	-	-	/deadbeef.bin		firmware file for com.example.othernode-v3, hardware version 1.0
+
+	The ROMFS directory structure is the same, but located at /etc/uavcan/fw
+
+	We iterate over all device directories in the ROMFS base directory, and create
+	corresponding device directories on the SD card if they don't already exist.
+
+	In each device directory, we iterate over each version directory and create a
+	corresponding version directory on the SD card if it doesn't already exist.
+
+	In each version directory, we remove any files with a name starting with "romfs_"
+	in the corresponding directory on the SD card that don't match the bundled firmware
+	filename; if the directory is empty after that process, we copy the bundled firmware.
+	*/
+	const size_t maxlen = UAVCAN_MAX_PATH_LENGTH;
+	const size_t sd_path_len = strlen(sd_path);
+	const size_t romfs_path_len = strlen(romfs_path);
+	struct stat sb;
+	int rv;
+	char dstpath[maxlen + 1];
+	char srcpath[maxlen + 1];
+
+	DIR* const romfs_dir = opendir(romfs_path);
+	if (!romfs_dir) {
+		warnx("base: couldn't open %s", romfs_path);
+		return;
+	}
+
+	memcpy(dstpath, sd_path, sd_path_len + 1);
+	memcpy(srcpath, romfs_path, romfs_path_len + 1);
+
+	// Iterate over all device directories in ROMFS
+	struct dirent* dev_dirent = NULL;
+	while ((dev_dirent = readdir(romfs_dir)) != NULL) {
+		// Skip if not a directory
+		if (!DIRENT_ISDIRECTORY(dev_dirent->d_type)) {
+			continue;
+		}
+
+		// Make sure the path fits
+		size_t dev_dirname_len = strlen(dev_dirent->d_name);
+		size_t srcpath_dev_len = romfs_path_len + 1 + dev_dirname_len;
+		if (srcpath_dev_len > maxlen) {
+			warnx("dev: srcpath '%s/%s' too long", romfs_path, dev_dirent->d_name);
+			continue;
+		}
+		size_t dstpath_dev_len = sd_path_len + 1 + dev_dirname_len;
+		if (dstpath_dev_len > maxlen) {
+			warnx("dev: dstpath '%s/%s' too long", sd_path, dev_dirent->d_name);
+			continue;
+		}
+
+		// Create the device name directory on the SD card if it doesn't already exist
+		dstpath[sd_path_len] = '/';
+		memcpy(&dstpath[sd_path_len + 1], dev_dirent->d_name, dev_dirname_len + 1);
+
+		if (stat(dstpath, &sb) != 0 || !S_ISDIR(sb.st_mode)) {
+			rv = mkdir(dstpath, S_IRWXU | S_IRWXG | S_IRWXO);
+			if (rv != 0) {
+				warnx("dev: couldn't create '%s'", dstpath);
+				continue;
+			}
+		}
+
+		// Set up the source path
+		srcpath[romfs_path_len] = '/';
+		memcpy(&srcpath[romfs_path_len + 1], dev_dirent->d_name, dev_dirname_len + 1);
+
+		DIR* const dev_dir = opendir(srcpath);
+		if (!dev_dir) {
+			warnx("dev: couldn't open '%s'", srcpath);
+			continue;
+		}
+
+		// Iterate over all version directories in the current ROMFS device directory
+		struct dirent* ver_dirent = NULL;
+		while ((ver_dirent = readdir(dev_dir)) != NULL) {
+			// Skip if not a directory
+			if (!DIRENT_ISDIRECTORY(ver_dirent->d_type)) {
+				continue;
+			}
+
+			// Make sure the path fits
+			size_t ver_dirname_len = strlen(ver_dirent->d_name);
+			size_t srcpath_ver_len = srcpath_dev_len + 1 + ver_dirname_len;
+			if (srcpath_ver_len > maxlen) {
+				warnx("ver: srcpath '%s/%s' too long", srcpath, ver_dirent->d_name);
+				continue;
+			}
+			size_t dstpath_ver_len = dstpath_dev_len + 1 + ver_dirname_len;
+			if (dstpath_ver_len > maxlen) {
+				warnx("ver: dstpath '%s/%s' too long", dstpath, ver_dirent->d_name);
+				continue;
+			}
+
+			// Create the device version directory on the SD card if it doesn't already exist
+			dstpath[dstpath_dev_len] = '/';
+			memcpy(&dstpath[dstpath_dev_len + 1], ver_dirent->d_name, ver_dirname_len + 1);
+
+			if (stat(dstpath, &sb) != 0 || !S_ISDIR(sb.st_mode)) {
+				rv = mkdir(dstpath, S_IRWXU | S_IRWXG | S_IRWXO);
+				if (rv != 0) {
+					warnx("ver: couldn't create '%s'", dstpath);
+					continue;
+				}
+			}
+
+			// Set up the source path
+			srcpath[srcpath_dev_len] = '/';
+			memcpy(&srcpath[srcpath_dev_len + 1], ver_dirent->d_name, ver_dirname_len + 1);
+
+			// Find the name of the bundled firmware file, or move on to the
+			// next directory if there's no file here.
+			DIR* const src_ver_dir = opendir(srcpath);
+			if (!src_ver_dir) {
+				warnx("ver: couldn't open '%s'", srcpath);
+				continue;
+			}
+
+			struct dirent* src_fw_dirent = NULL;
+			while ((src_fw_dirent = readdir(src_ver_dir)) != NULL &&
+					!DIRENT_ISFILE(src_fw_dirent->d_type));
+
+			if (!src_fw_dirent) {
+				(void)closedir(src_ver_dir);
+				continue;
+			}
+
+			size_t fw_len = strlen(src_fw_dirent->d_name);
+
+			bool copy_fw = true;
+
+			// Clear out any romfs_ files in the version directory on the SD card
+			DIR* const dst_ver_dir = opendir(dstpath);
+			if (!dst_ver_dir) {
+				warnx("unlink: couldn't open '%s'", dstpath);
+			} else {
+				struct dirent* fw_dirent = NULL;
+				while ((fw_dirent = readdir(dst_ver_dir)) != NULL) {
+					// Skip if not a file
+					if (!DIRENT_ISFILE(fw_dirent->d_type)) {
+						continue;
+					}
+
+					if (!memcmp(&fw_dirent->d_name[sizeof(UAVCAN_ROMFS_FW_PREFIX) - 1], src_fw_dirent->d_name, fw_len)) {
+						/*
+						 * Exact match between SD card filename and ROMFS filename; must be the same version
+						 * so don't bother deleting and rewriting it.
+						 */
+						copy_fw = false;
+					} else if (!memcmp(fw_dirent->d_name, UAVCAN_ROMFS_FW_PREFIX, sizeof(UAVCAN_ROMFS_FW_PREFIX) - 1)) {
+						size_t fw_len = strlen(fw_dirent->d_name);
+						size_t dstpath_fw_len = dstpath_ver_len + sizeof(UAVCAN_ROMFS_FW_PREFIX) + fw_len;
+						if (dstpath_fw_len > maxlen) {
+							// sizeof(prefix) includes trailing NUL, cancelling out the +1 for the path separator
+							warnx("unlink: path '%s/%s%s' too long", dstpath, UAVCAN_ROMFS_FW_PREFIX, fw_dirent->d_name);
+						} else {
+							// File name starts with "romfs_", delete it.
+							dstpath[dstpath_ver_len] = '/';
+							memcpy(&dstpath[dstpath_ver_len + 1], fw_dirent->d_name, fw_len + 1);
+							unlink(dstpath);
+
+							warnx("unlink: removed '%s/%s%s'", dstpath, UAVCAN_ROMFS_FW_PREFIX, fw_dirent->d_name);
+						}
+					} else {
+						// User file, don't copy firmware
+						copy_fw = false;
+					}
+				}
+				(void)closedir(dst_ver_dir);
+			}
+
+			// If we need to, copy the file from ROMFS to the SD card
+			if (copy_fw) {
+				size_t srcpath_fw_len = srcpath_ver_len + 1 + fw_len;
+				size_t dstpath_fw_len = dstpath_ver_len + sizeof(UAVCAN_ROMFS_FW_PREFIX) + fw_len;
+
+				if (srcpath_fw_len > maxlen) {
+					warnx("copy: srcpath '%s/%s' too long", srcpath, src_fw_dirent->d_name);
+				} else if (dstpath_fw_len > maxlen) {
+					warnx("copy: dstpath '%s/%s%s' too long", dstpath, UAVCAN_ROMFS_FW_PREFIX, src_fw_dirent->d_name);
+				} else {
+					// All OK, make the paths and copy the file
+					srcpath[srcpath_ver_len] = '/';
+					memcpy(&srcpath[srcpath_ver_len + 1], src_fw_dirent->d_name, fw_len + 1);
+
+					dstpath[dstpath_ver_len] = '/';
+					memcpy(&dstpath[dstpath_ver_len + 1], UAVCAN_ROMFS_FW_PREFIX, sizeof(UAVCAN_ROMFS_FW_PREFIX));
+					memcpy(&dstpath[dstpath_ver_len + sizeof(UAVCAN_ROMFS_FW_PREFIX)], src_fw_dirent->d_name, fw_len + 1);
+
+					rv = copyFw(dstpath, srcpath);
+					if (rv != 0) {
+						warnx("copy: '%s' -> '%s' failed: %d", srcpath, dstpath, rv);
+					} else {
+						warnx("copy: '%s' -> '%s' succeeded", srcpath, dstpath);
+					}
+				}
+			}
+
+			(void)closedir(src_ver_dir);
+		}
+
+		(void)closedir(dev_dir);
+	}
+
+	(void)closedir(romfs_dir);
+}
+
+int UavcanServers::copyFw(const char* dst, const char* src) {
+	int rv = 0;
+	int dfd, sfd;
+	uint8_t buffer[512];
+
+	dfd = open(dst, O_WRONLY | O_CREAT, 0666);
+	if (dfd < 0) {
+		warnx("copyFw: couldn't open dst");
+		return -errno;
+	}
+
+	sfd = open(src, O_RDONLY, 0);
+	if (sfd < 0) {
+		(void)close(dfd);
+		warnx("copyFw: couldn't open src");
+		return -errno;
+	}
+
+	ssize_t size = 0;
+	do {
+		size = read(sfd, buffer, sizeof(buffer));
+		if (size < 0) {
+			warnx("copyFw: couldn't read");
+			rv = -errno;
+		} else if (size > 0) {
+			rv = 0;
+			ssize_t remaining = size;
+			ssize_t total_written = 0;
+			ssize_t written = 0;
+			do {
+				written = write(dfd, &buffer[total_written], remaining);
+				if (written < 0) {
+					warnx("copyFw: couldn't write");
+					rv = -errno;
+				} else {
+					total_written += written;
+					remaining -=  written;
+				}
+			} while (written > 0 && remaining > 0);
+		}
+	} while (rv == 0 && size != 0);
+
+	(void)close(dfd);
+	(void)close(sfd);
+
+	return rv;
+}

src/modules/uavcan/uavcan_servers.hpp

@@ -42,16 +42,20 @@
 #include <uavcan/node/sub_node.hpp>
 #include <uavcan/protocol/node_status_monitor.hpp>
 
-# include <uavcan/protocol/dynamic_node_id_server/centralized.hpp>
-# include <uavcan/protocol/node_info_retriever.hpp>
-# include <uavcan_posix/basic_file_server_backend.hpp>
-# include <uavcan/protocol/firmware_update_trigger.hpp>
-# include <uavcan/protocol/file_server.hpp>
-# include <uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp>
-# include <uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp>
-# include <uavcan_posix/firmware_version_checker.hpp>
+#include <uavcan/protocol/dynamic_node_id_server/centralized.hpp>
+#include <uavcan/protocol/node_info_retriever.hpp>
+#include <uavcan_posix/basic_file_server_backend.hpp>
+#include <uavcan/protocol/firmware_update_trigger.hpp>
+#include <uavcan/protocol/file_server.hpp>
+#include <uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp>
+#include <uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp>
+#include <uavcan_posix/firmware_version_checker.hpp>
+#include <uavcan/equipment/esc/RawCommand.hpp>
+#include <uavcan/equipment/indication/BeepCommand.hpp>
+#include <uavcan/protocol/enumeration/Begin.hpp>
+#include <uavcan/protocol/enumeration/Indication.hpp>
 
-# include "uavcan_virtual_can_driver.hpp"
+#include "uavcan_virtual_can_driver.hpp"
 
 /**
  * @file uavcan_servers.hpp
@@ -65,6 +69,9 @@
 #define UAVCAN_DEVICE_PATH	"/dev/uavcan/esc"
 #define UAVCAN_NODE_DB_PATH     "/fs/microsd/uavcan.db"
 #define UAVCAN_FIRMWARE_PATH    "/fs/microsd/fw"
+#define UAVCAN_ROMFS_FW_PATH "/etc/uavcan/fw"
+#define UAVCAN_ROMFS_FW_PREFIX "romfs_"
+#define UAVCAN_MAX_PATH_LENGTH (128 + 40)
 #define UAVCAN_LOG_FILE         UAVCAN_NODE_DB_PATH"/trace.log"
 
 /**
@@ -88,7 +95,7 @@ class UavcanServers
 	static constexpr unsigned QueuePoolSize =
 		(NumIfaces * uavcan::MemPoolBlockSize * MaxCanFramesPerTransfer);
 
-	static constexpr unsigned StackSize  = 3500;
+	static constexpr unsigned StackSize  = 6000;
 	static constexpr unsigned Priority  =  120;
 
 	typedef uavcan::SubNode<MemPoolSize> SubNode;
@@ -140,7 +147,84 @@ private:
 	uavcan::FirmwareUpdateTrigger   _fw_upgrade_trigger;
 	uavcan::BasicFileServer         _fw_server;
 
+	/*
+	 * The MAVLink parameter bridge needs to know the maximum parameter index
+	 * of each node so that clients can determine when parameter listings have
+	 * finished. We do that by querying a node's entire parameter set whenever
+	 * a parameter message is received for a node with a zero _param_count,
+	 * and storing the count here. If a node doesn't respond, or doesn't have
+	 * any parameters, its count will stay at zero and we'll try to query the
+	 * set again next time.
+	 *
+	 * The node's UAVCAN ID is used as the index into the _param_counts array.
+	 */
+	uint8_t _param_counts[128];
+	bool _count_in_progress;
+	uint8_t _count_index;
+
+	bool _param_in_progress;
+	uint8_t _param_index;
+	bool _param_list_in_progress;
+	bool _param_list_all_nodes;
+	uint8_t _param_list_node_id;
+
+	uint32_t _param_dirty_bitmap[4];
+	uint8_t _param_save_opcode;
+
+	bool _cmd_in_progress;
+
+	// uORB topic handle for MAVLink parameter responses
+	orb_advert_t _param_response_pub;
+
+	typedef uavcan::MethodBinder<UavcanServers *,
+		void (UavcanServers::*)(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &)> GetSetCallback;
+	typedef uavcan::MethodBinder<UavcanServers *,
+		void (UavcanServers::*)(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &)> ExecuteOpcodeCallback;
+	typedef uavcan::MethodBinder<UavcanServers *,
+		void (UavcanServers::*)(const uavcan::ServiceCallResult<uavcan::protocol::RestartNode> &)> RestartNodeCallback;
+	void cb_getset(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &result);
+	void cb_count(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &result);
+	void cb_opcode(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &result);
+	void cb_restart(const uavcan::ServiceCallResult<uavcan::protocol::RestartNode> &result);
+
+	uavcan::ServiceClient<uavcan::protocol::param::GetSet, GetSetCallback> _param_getset_client;
+	uavcan::ServiceClient<uavcan::protocol::param::ExecuteOpcode, ExecuteOpcodeCallback> _param_opcode_client;
+	uavcan::ServiceClient<uavcan::protocol::RestartNode, RestartNodeCallback> _param_restartnode_client;
+	void param_count(uavcan::NodeID node_id);
+	void param_opcode(uavcan::NodeID node_id);
+
+	uint8_t get_next_active_node_id(uint8_t base);
+	uint8_t get_next_dirty_node_id(uint8_t base);
+	void set_node_params_dirty(uint8_t node_id) { _param_dirty_bitmap[node_id >> 5] |= 1 << (node_id & 31); }
+	void clear_node_params_dirty(uint8_t node_id) { _param_dirty_bitmap[node_id >> 5] &= ~(1 << (node_id & 31)); }
+	bool are_node_params_dirty(uint8_t node_id) const { return bool((_param_dirty_bitmap[node_id >> 5] >> (node_id & 31)) & 1); }
+
 	bool _mutex_inited;
 	volatile bool _check_fw;
 
+	// ESC enumeration
+	bool _esc_enumeration_active;
+	uint8_t _esc_enumeration_ids[uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize];
+	uint8_t _esc_enumeration_index;
+	uint8_t _esc_set_index;
+	uint8_t _esc_count;
+
+	typedef uavcan::MethodBinder<UavcanServers *,
+		void (UavcanServers::*)(const uavcan::ServiceCallResult<uavcan::protocol::enumeration::Begin> &)> EnumerationBeginCallback;
+	typedef uavcan::MethodBinder<UavcanServers *,
+		void (UavcanServers::*)(const uavcan::ReceivedDataStructure<uavcan::protocol::enumeration::Indication>&)>
+		EnumerationIndicationCallback;
+	void cb_enumeration_begin(const uavcan::ServiceCallResult<uavcan::protocol::enumeration::Begin> &result);
+	void cb_enumeration_indication(const uavcan::ReceivedDataStructure<uavcan::protocol::enumeration::Indication> &msg);
+	void cb_enumeration_getset(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &result);
+	void cb_enumeration_save(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &result);
+
+	uavcan::Publisher<uavcan::equipment::indication::BeepCommand> _beep_pub;
+	uavcan::Subscriber<uavcan::protocol::enumeration::Indication, EnumerationIndicationCallback> _enumeration_indication_sub;
+	uavcan::ServiceClient<uavcan::protocol::enumeration::Begin, EnumerationBeginCallback> _enumeration_client;
+	uavcan::ServiceClient<uavcan::protocol::param::GetSet, GetSetCallback> _enumeration_getset_client;
+	uavcan::ServiceClient<uavcan::protocol::param::ExecuteOpcode, ExecuteOpcodeCallback> _enumeration_save_client;
+
+	void unpackFwFromROMFS(const char* sd_path, const char* romfs_path);
+	int copyFw(const char* dst, const char* src);
 };