Implement parameters subsystem

* Unified parameters storage.
* Store parameters in flash on the hardware.
* Store parameters in text file in simulation.
* Work with parameters in command line.
* Support parameters in MAVLink for working with parameters in QGC.

flix/cli.ino

@@ -19,8 +19,10 @@ const char* motd =
 "|__|     |_______||__| /__/ \\__\\\n\n"
 "Commands:\n\n"
 "help - show help\n"
-"show - show all parameters\n"
-"<name> <value> - set parameter\n"
+"p - show all parameters\n"
+"p <name> - show parameter\n"
+"p <name> <value> - set parameter\n"
+"preset - reset parameters\n"
 "ps - show pitch/roll/yaw\n"
 "psq - show attitude quaternion\n"
 "imu - show IMU data\n"
@@ -34,36 +36,22 @@ const char* motd =
 "reset - reset drone's state\n"
 "reboot - reboot the drone\n";
 
-const struct Param {
-	const char* name;
-	float* value;
-	float* value2;
-} params[] = {
-	{"rp", &rollRatePID.p, &pitchRatePID.p},
-	{"ri", &rollRatePID.i, &pitchRatePID.i},
-	{"rd", &rollRatePID.d, &pitchRatePID.d},
-
-	{"ap", &rollPID.p, &pitchPID.p},
-	{"ai", &rollPID.i, &pitchPID.i},
-	{"ad", &rollPID.d, &pitchPID.d},
-
-	{"yp", &yawRatePID.p, nullptr},
-	{"yi", &yawRatePID.i, nullptr},
-	{"yd", &yawRatePID.d, nullptr},
-
-	{"lpr", &ratesFilter.alpha, nullptr},
-	{"lpd", &rollRatePID.lpf.alpha, &pitchRatePID.lpf.alpha},
-
-	{"ss", &loopRate, nullptr},
-	{"dt", &dt, nullptr},
-	{"t", &t, nullptr},
-};
-
-void doCommand(String& command, String& value) {
+void doCommand(String& command, String& arg0, String& arg1) {
 	if (command == "help" || command == "motd") {
 		Serial.println(motd);
-	} else if (command == "show") {
-		showTable();
+	} else if (command == "p" && arg0 == "") {
+		printParameters();
+	} else if (command == "p" && arg0 != "" && arg1 == "") {
+		Serial.printf("%s = %g\n", arg0.c_str(), getParameter(arg0.c_str()));
+	} else if (command == "p") {
+		bool success = setParameter(arg0.c_str(), arg1.toFloat());
+		if (success) {
+			Serial.printf("%s = %g\n", arg0.c_str(), arg1.toFloat());
+		} else {
+			Serial.printf("Parameter not found: %s\n", arg0.c_str());
+		}
+	} else if (command == "preset") {
+		resetParameters();
 	} else if (command == "ps") {
 		Vector a = attitude.toEulerZYX();
 		Serial.printf("roll: %f pitch: %f yaw: %f\n", a.x * RAD_TO_DEG, a.y * RAD_TO_DEG, a.z * RAD_TO_DEG);
@@ -106,32 +94,13 @@ void doCommand(String& command, String& value) {
 		attitude = Quaternion();
 	} else if (command == "reboot") {
 		ESP.restart();
+	} else if (command == "") {
+		// do nothing
 	} else {
-		float val = value.toFloat();
-		// TODO: on error returns 0, check invalid value
-
-		for (uint8_t i = 0; i < sizeof(params) / sizeof(params[0]); i++) {
-			if (command == params[i].name) {
-				*params[i].value = val;
-				if (params[i].value2 != nullptr) *params[i].value2 = val;
-				Serial.print(command);
-				Serial.print(" = ");
-				Serial.println(val, 4);
-				return;
-			}
-		}
 		Serial.println("Invalid command: " + command);
 	}
 }
 
-void showTable() {
-	for (uint8_t i = 0; i < sizeof(params) / sizeof(params[0]); i++) {
-		Serial.print(params[i].name);
-		Serial.print(" ");
-		Serial.println(*params[i].value, 5);
-	}
-}
-
 void cliTestMotor(uint8_t n) {
 	Serial.printf("Testing motor %d\n", n);
 	motors[n] = 1;
@@ -145,9 +114,7 @@ void cliTestMotor(uint8_t n) {
 
 void parseInput() {
 	static bool showMotd = true;
-	static String command;
-	static String value;
-	static bool parsingCommand = true;
+	static String input;
 
 	if (showMotd) {
 		Serial.println(motd);
@@ -157,16 +124,21 @@ void parseInput() {
 	while (Serial.available()) {
 		char c = Serial.read();
 		if (c == '\n') {
-			parsingCommand = true;
-			if (!command.isEmpty()) {
-				doCommand(command, value);
-			}
-			command.clear();
-			value.clear();
-		} else if (c == ' ') {
-			parsingCommand = false;
+			char chars[input.length() + 1];
+			input.toCharArray(chars, input.length() + 1);
+			String command = stringToken(chars, " ");
+			String arg0 = stringToken(NULL, " ");
+			String arg1 = stringToken(NULL, "");
+			doCommand(command, arg0, arg1);
+			input.clear();
 		} else {
-			(parsingCommand ? command : value) += c;
+			input += c;
 		}
 	}
 }
+
+// Helper function for parsing input
+String stringToken(char* str, const char* delim) {
+	char* token = strtok(str, delim);
+	return token == NULL ? "" : token;
+}

flix/flix.ino

@@ -41,6 +41,7 @@ void setup() {
 	Serial.begin(SERIAL_BAUDRATE);
 	Serial.println("Initializing flix");
 	disableBrownOut();
+	setupParameters();
 	setupLED();
 	setupMotors();
 	setLED(true);
@@ -66,4 +67,5 @@ void loop() {
 	processMavlink();
 #endif
 	logData();
+	flushParameters();
 }

flix/imu.ino

@@ -6,12 +6,11 @@
 #include <SPI.h>
 #include <MPU9250.h>
 
-// NOTE: use 'ca' command to calibrate the accelerometer and put the values here
-Vector accBias(0, 0, 0);
-Vector accScale(1, 1, 1);
-
 MPU9250 IMU(SPI);
+
+Vector accBias;
 Vector gyroBias;
+Vector accScale(1, 1, 1);
 
 void setupIMU() {
 	Serial.println("Setup IMU");

flix/mavlink.ino

@@ -84,6 +84,7 @@ void receiveMavlink() {
 
 void handleMavlink(const void *_msg) {
 	mavlink_message_t *msg = (mavlink_message_t *)_msg;
+
 	if (msg->msgid == MAVLINK_MSG_ID_MANUAL_CONTROL) {
 		mavlink_manual_control_t manualControl;
 		mavlink_msg_manual_control_decode(msg, &manualControl);
@@ -97,6 +98,49 @@ void handleMavlink(const void *_msg) {
 
 		if (abs(controls[RC_CHANNEL_YAW]) < MAVLINK_CONTROL_YAW_DEAD_ZONE) controls[RC_CHANNEL_YAW] = 0;
 	}
+
+	if (msg->msgid == MAVLINK_MSG_ID_PARAM_REQUEST_LIST) {
+		mavlink_message_t msg;
+		for (int i = 0; i < parametersCount(); i++) {
+			mavlink_msg_param_value_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg,
+				getParameterName(i), getParameter(i), MAV_PARAM_TYPE_REAL32, parametersCount(), i);
+			sendMessage(&msg);
+		}
+	}
+
+	if (msg->msgid == MAVLINK_MSG_ID_PARAM_REQUEST_READ) {
+		mavlink_param_request_read_t paramRequestRead;
+		mavlink_msg_param_request_read_decode(msg, &paramRequestRead);
+		char name[16 + 1];
+		strlcpy(name, paramRequestRead.param_id, sizeof(name)); // param_id might be not null-terminated
+		float value = strlen(name) == 0 ? getParameter(paramRequestRead.param_index) : getParameter(name);
+		if (paramRequestRead.param_index != -1) {
+			memcpy(name, getParameterName(paramRequestRead.param_index), 16);
+		}
+		mavlink_message_t msg;
+		mavlink_msg_param_value_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg,
+			name, value, MAV_PARAM_TYPE_REAL32, parametersCount(), paramRequestRead.param_index);
+		sendMessage(&msg);
+	}
+
+	if (msg->msgid == MAVLINK_MSG_ID_PARAM_SET) {
+		mavlink_param_set_t paramSet;
+		mavlink_msg_param_set_decode(msg, &paramSet);
+		char name[16 + 1];
+		strlcpy(name, paramSet.param_id, sizeof(name)); // param_id might be not null-terminated
+		setParameter(name, paramSet.param_value);
+		// send ack
+		mavlink_message_t msg;
+		mavlink_msg_param_value_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg,
+			paramSet.param_id, paramSet.param_value, MAV_PARAM_TYPE_REAL32, parametersCount(), 0); // index is unknown
+		sendMessage(&msg);
+	}
+
+	if (msg->msgid == MAVLINK_MSG_ID_MISSION_REQUEST_LIST) { // handle to make qgc happy
+		mavlink_message_t msg;
+		mavlink_msg_mission_count_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, 0, 0, 0, MAV_MISSION_TYPE_MISSION, 0);
+		sendMessage(&msg);
+	}
 }
 
 // Convert Forward-Left-Up to Forward-Right-Down quaternion

flix/parameters.ino

@@ -0,0 +1,133 @@
+#pragma once
+
+#include <Preferences.h>
+#include <vector>
+
+extern float channelNeutral[RC_CHANNELS];
+extern float channelMax[RC_CHANNELS];
+
+Preferences storage;
+
+struct Parameter {
+	const char *name;
+	float *variable;
+	float value; // cache
+};
+
+Parameter parameters[] = {
+	// control
+	{"ROLLRATE_P", &rollRatePID.p},
+	{"ROLLRATE_I", &rollRatePID.i},
+	{"ROLLRATE_D", &rollRatePID.d},
+	{"ROLLRATE_I_LIM", &rollRatePID.windup},
+	{"PITCHRATE_P", &pitchRatePID.p},
+	{"PITCHRATE_I", &pitchRatePID.i},
+	{"PITCHRATE_D", &pitchRatePID.d},
+	{"PITCHRATE_I_LIM", &pitchRatePID.windup},
+	{"YAWRATE_P", &yawRatePID.p},
+	{"YAWRATE_I", &yawRatePID.i},
+	{"YAWRATE_D", &yawRatePID.d},
+	{"ROLL_P", &rollPID.p},
+	{"ROLL_I", &rollPID.i},
+	{"ROLL_D", &rollPID.d},
+	{"PITCH_P", &pitchPID.p},
+	{"PITCH_I", &pitchPID.i},
+	{"PITCH_D", &pitchPID.d},
+	{"YAW_P", &yawPID.p},
+	// imu
+	{"ACC_BIAS_X", &accBias.x},
+	{"ACC_BIAS_Y", &accBias.y},
+	{"ACC_BIAS_Z", &accBias.z},
+	{"ACC_SCALE_X", &accScale.x},
+	{"ACC_SCALE_Y", &accScale.y},
+	{"ACC_SCALE_Z", &accScale.z},
+	{"GYRO_BIAS_X", &gyroBias.x},
+	{"GYRO_BIAS_Y", &gyroBias.y},
+	{"GYRO_BIAS_Z", &gyroBias.z},
+	// rc
+	{"RC_NEUTRAL_0", &channelNeutral[0]},
+	{"RC_NEUTRAL_1", &channelNeutral[1]},
+	{"RC_NEUTRAL_2", &channelNeutral[2]},
+	{"RC_NEUTRAL_3", &channelNeutral[3]},
+	{"RC_NEUTRAL_4", &channelNeutral[4]},
+	{"RC_NEUTRAL_5", &channelNeutral[5]},
+	{"RC_NEUTRAL_6", &channelNeutral[6]},
+	{"RC_NEUTRAL_7", &channelNeutral[7]},
+	{"RC_MAX_0", &channelMax[0]},
+	{"RC_MAX_1", &channelMax[1]},
+	{"RC_MAX_2", &channelMax[2]},
+	{"RC_MAX_3", &channelMax[3]},
+	{"RC_MAX_4", &channelMax[4]},
+	{"RC_MAX_5", &channelMax[5]},
+	{"RC_MAX_6", &channelMax[6]},
+	{"RC_MAX_7", &channelMax[7]}
+};
+
+void setupParameters() {
+	storage.begin("flix", false);
+	// Read parameters from storage
+	for (auto &parameter : parameters) {
+		if (!storage.isKey(parameter.name)) {
+			Serial.printf("Define new parameter %s = %f\n", parameter.name, *parameter.variable);
+			storage.putFloat(parameter.name, *parameter.variable);
+		}
+		*parameter.variable = storage.getFloat(parameter.name, *parameter.variable);
+		parameter.value = *parameter.variable;
+	}
+}
+
+int parametersCount() {
+	return sizeof(parameters) / sizeof(parameters[0]);
+}
+
+const char *getParameterName(int index) {
+	return parameters[index].name;
+}
+
+float getParameter(int index) {
+	return *parameters[index].variable;
+}
+
+float getParameter(const char *name) {
+	for (auto &parameter : parameters) {
+		if (strcmp(parameter.name, name) == 0) {
+			return *parameter.variable;
+		}
+	}
+	return NAN;
+}
+
+bool setParameter(const char *name, const float value) {
+	for (auto &parameter : parameters) {
+		if (strcmp(parameter.name, name) == 0) {
+			*parameter.variable = value;
+			return true;
+		}
+	}
+	return false;
+}
+
+void flushParameters() {
+	static float lastFlush = 0;
+	if (t - lastFlush < 1) return; // flush once per second
+	if (motorsActive()) return; // don't use flash while flying, it may cause a delay
+	lastFlush = t;
+
+	for (auto &parameter : parameters) {
+		if (parameter.value == *parameter.variable) continue;
+		if (isnan(parameter.value) && isnan(*parameter.variable)) continue; // handle NAN != NAN
+		storage.putFloat(parameter.name, *parameter.variable);
+		parameter.value = *parameter.variable;
+	}
+}
+
+void printParameters() {
+	for (auto &parameter : parameters) {
+		Serial.printf("%s = %g\n", parameter.name, *parameter.variable);
+	}
+}
+
+void resetParameters() {
+	storage.clear();
+	ESP.restart();
+}

flix/rc.ino

@@ -5,9 +5,8 @@
 
 #include <SBUS.h>
 
-// NOTE: use 'cr' command to calibrate the RC and put the values here
-int channelNeutral[] = {995, 883, 200, 972, 512, 512, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
-int channelMax[] = {1651, 1540, 1713, 1630, 1472, 1472, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
+float channelNeutral[RC_CHANNELS] = {NAN}; // first element NAN means not calibrated
+float channelMax[RC_CHANNELS];
 
 SBUS RC(Serial2); // NOTE: Use RC(Serial2, 16, 17) if you use the old UART2 pins
 
@@ -26,6 +25,7 @@ void readRC() {
 }
 
 void normalizeRC() {
+	if (isnan(channelNeutral[0])) return; // skip if not calibrated
 	for (uint8_t i = 0; i < RC_CHANNELS; i++) {
 		controls[i] = mapf(channels[i], channelNeutral[i], channelMax[i], 0, 1);
 	}