Merge pull request #22 from PX4/beta_2.0

v2.0

firmware/Makefile

@@ -32,9 +32,9 @@
 
 PROJECT = io.px4.sapog
 
-HW_VERSION = 1
-FW_VERSION_MAJOR = 1
-FW_VERSION_MINOR = 8
+HW_VERSION_MAJOR = 1
+FW_VERSION_MAJOR = 2
+FW_VERSION_MINOR = 0
 
 export SAPOG_BASE := $(realpath $(dir $(lastword $(MAKEFILE_LIST))))
 export SAPOG_SRC_DIR = $(abspath $(SAPOG_BASE)/src)
@@ -57,7 +57,7 @@ UINCDIR = src           \
 
 UDEFS = -DFW_VERSION_MAJOR=$(FW_VERSION_MAJOR)           \
         -DFW_VERSION_MINOR=$(FW_VERSION_MINOR)           \
-        -DHW_VERSION=$(HW_VERSION)                       \
+        -DHW_VERSION_MAJOR=$(HW_VERSION_MAJOR)           \
         -DNODE_NAME=\"$(PROJECT)\"
 
 # MAVLink v1 compliance
@@ -116,9 +116,10 @@ DEBUG_OPT = -O2 -g3 -DDISABLE_WATCHDOG=1
 CWARN = -Wshadow -Wpointer-arith -Wno-packed -Wno-attributes -Wno-error=undef -Wno-error=shadow -Werror
 CPPWARN = $(CWARN)
 
-HW_VERSION_MAJOR_MINOR := $(HW_VERSION).0
+# This firmware is compatible with all versions of hardware where the major number is 1 (i.e. 1.x),
+# therefore, the minor version is not specified here.
 FW_VERSION_MAJOR_MINOR_VCS_HASH := $(FW_VERSION_MAJOR).$(FW_VERSION_MINOR).$(GIT_HASH)
-COMPOUND_IMAGE_FILE := $(PROJECT)-$(HW_VERSION_MAJOR_MINOR)-$(FW_VERSION_MAJOR_MINOR_VCS_HASH).compound.bin
+COMPOUND_IMAGE_FILE := $(PROJECT)-$(HW_VERSION_MAJOR)-$(FW_VERSION_MAJOR_MINOR_VCS_HASH).compound.bin
 BOOTLOADER_DIR := $(abspath $(SAPOG_BASE)/../bootloader)
 BOOTLOADER_IMAGE := $(BOOTLOADER_DIR)/build/bootloader.bin
 
@@ -136,7 +137,7 @@ POST_MAKE_ALL_RULE_HOOK: build/$(PROJECT).bin build/$(PROJECT).elf
 	cd build && cat padded_bootloader.tmp.bin $(PROJECT).bin >$(COMPOUND_IMAGE_FILE)
 
 	# Generating the signed image for the bootloader
-	cd build && ../zubax_chibios/tools/make_boot_descriptor.py $(PROJECT).bin $(PROJECT) $(HW_VERSION_MAJOR_MINOR) \
+	cd build && ../zubax_chibios/tools/make_boot_descriptor.py $(PROJECT).bin $(PROJECT) $(HW_VERSION_MAJOR)       \
 	                                                           --also-patch-descriptor-in=$(PROJECT).elf           \
 	                                                           --also-patch-descriptor-in=$(COMPOUND_IMAGE_FILE)
 

firmware/src/board/board.cpp

@@ -37,6 +37,7 @@
 #include <unistd.h>
 #include <zubax_chibios/util/software_i2c.hpp>
 #include <zubax_chibios/platform/stm32/flash_writer.hpp>
+#include <zubax_chibios/platform/stm32/config_storage.hpp>
 
 // Clock config validation
 #if STM32_PREDIV1_VALUE != 2
@@ -64,6 +65,10 @@ const extern std::uint8_t DeviceSignatureStorage[];
 namespace board
 {
 
+// This can't be constexpr because of reinterpret_cast<>
+static void* const ConfigStorageAddress = reinterpret_cast<void*>(0x08000000 + (256 * 1024) - 1024);
+constexpr unsigned ConfigStorageSize = 1024;
+
 extern void init_led();
 
 os::watchdog::Timer init(unsigned watchdog_timeout_ms)
@@ -84,7 +89,8 @@ os::watchdog::Timer init(unsigned watchdog_timeout_ms)
 	init_led();
 
 	// Config
-	const int config_init_res = os::config::init();
+	static os::stm32::ConfigStorageBackend config_storage_backend(ConfigStorageAddress, ConfigStorageSize);
+	const int config_init_res = os::config::init(&config_storage_backend);
 	if (config_init_res < 0)
 	{
 		die(config_init_res);
@@ -138,7 +144,7 @@ HardwareVersion detect_hardware_version()
 {
 	auto v = HardwareVersion();
 
-	v.major = HW_VERSION;
+	v.major = HW_VERSION_MAJOR;
 	v.minor = std::uint8_t(GPIOC->IDR & 0x0F);
 
 	return v;

firmware/src/console.cpp

@@ -194,7 +194,7 @@ static void cmd_startstop(BaseSequentialStream *chp, int argc, char *argv[])
 		printf("Cycle %d of %d, dc %f...\n", current_cycle + 1, num_cycles, dc);
 
 		// Waiting for the motor to spin down
-		sleep(3);
+		sleep(5);
 		if (!motor_is_idle()) {
 			puts("NOT STOPPED");
 			break;

firmware/src/main.cpp

@@ -107,6 +107,59 @@ void do_startup_beep()
 	motor_beep(1000, 100);
 }
 
+/**
+ * Managing configuration parameters in the background from the main thread.
+ * This code was borrowed from PX4ESC.
+ */
+class BackgroundConfigManager
+{
+	static constexpr float SaveDelay = 1.0F;
+	static constexpr float SaveDelayAfterError = 10.0F;
+
+	os::Logger logger { "BackgroundConfigManager" };
+
+	unsigned modification_counter_ = os::config::getModificationCounter();
+	::systime_t last_modification_ts_ = chVTGetSystemTimeX();
+	bool pending_save_ = false;
+	bool last_save_failed_ = false;
+
+	float getTimeSinceModification() const
+	{
+		return float(ST2MS(chVTTimeElapsedSinceX(last_modification_ts_))) / 1e3F;
+	}
+
+public:
+	void poll()
+	{
+		const auto new_mod_cnt = os::config::getModificationCounter();
+
+		if (new_mod_cnt != modification_counter_) {
+			modification_counter_ = new_mod_cnt;
+			last_modification_ts_ = chVTGetSystemTimeX();
+			pending_save_ = true;
+		}
+
+		if (pending_save_) {
+			if (getTimeSinceModification() > (last_save_failed_ ? SaveDelayAfterError : SaveDelay)) {
+				os::TemporaryPriorityChanger priority_changer(HIGHPRIO);
+				if (motor_is_idle()) {
+					logger.println("Saving [modcnt=%u]", modification_counter_);
+
+					const int res = os::config::save();
+					if (res >= 0) {
+						pending_save_ = false;
+						last_save_failed_ = false;
+					} else {
+						last_save_failed_ = true;
+						logger.println("SAVE ERROR %d '%s'",
+						               res, std::strerror(std::abs(res)));
+					}
+				}
+			}
+		}
+	}
+};
+
 }
 
 namespace os
@@ -137,7 +190,7 @@ int main()
 	 * TODO: Refactor.
 	 * TODO: Report status flags via vendor-specific status field.
 	 */
-	auto config_modifications = os::config::getModificationCounter();
+	BackgroundConfigManager bg_config_manager;
 
 	while (!os::isRebootRequested()) {
 		wdt.reset();
@@ -150,14 +203,7 @@ int main()
 			uavcan_node::set_node_status_ok();
 		}
 
-		const auto new_config_modifications = os::config::getModificationCounter();
-		if ((new_config_modifications != config_modifications) && motor_is_idle())
-		{
-			config_modifications = new_config_modifications;
-			os::lowsyslog("Saving configuration... ");
-			const int res = ::configSave();		// TODO use C++ API
-			os::lowsyslog("Done [%d]\n", res);
-		}
+		bg_config_manager.poll();
 
 		::usleep(10 * 1000);
 	}

firmware/src/motor/motor.c

@@ -92,6 +92,7 @@ static struct params
 {
 	float dc_min_voltage;
 	float dc_spinup_voltage;
+	float spinup_voltage_ramp_duration;
 	float dc_step_max;
 	float dc_slope;
 
@@ -110,10 +111,11 @@ static struct params
 } _params;
 
 
-CONFIG_PARAM_FLOAT("mot_v_min",    2.3,    0.5,     10.0)
-CONFIG_PARAM_FLOAT("mot_v_spinup", 3.0,    0.5,     20.0)
-CONFIG_PARAM_FLOAT("mot_dc_accel", 0.09,   0.001,   0.5)
-CONFIG_PARAM_FLOAT("mot_dc_slope", 5.0,    0.1,     20.0)
+CONFIG_PARAM_FLOAT("mot_v_min",        2.5,    0.5,     10.0)
+CONFIG_PARAM_FLOAT("mot_v_spinup",     0.5,    0.01,    10.0)
+CONFIG_PARAM_FLOAT("mot_spup_vramp_t", 3.0,    0.0,     10.0)
+CONFIG_PARAM_FLOAT("mot_dc_accel",     0.09,   0.001,   0.5)
+CONFIG_PARAM_FLOAT("mot_dc_slope",     5.0,    0.1,     20.0)
 
 CONFIG_PARAM_INT("mot_num_poles",  14,     2,       100)
 CONFIG_PARAM_INT("ctl_dir",        0,      0,       1)
@@ -131,6 +133,7 @@ static void configure(void)
 {
 	_params.dc_min_voltage    = configGet("mot_v_min");
 	_params.dc_spinup_voltage = configGet("mot_v_spinup");
+	_params.spinup_voltage_ramp_duration = configGet("mot_spup_vramp_t");
 	_params.dc_step_max    = configGet("mot_dc_accel");
 	_params.dc_slope       = configGet("mot_dc_slope");
 
@@ -237,24 +240,24 @@ static void update_control_non_running(void)
 	}
 
 	// Start if necessary
-	const float spinup_dc = _params.dc_spinup_voltage / _state.input_voltage;
-
 	const bool need_start =
 		(_state.mode == MOTOR_CONTROL_MODE_OPENLOOP && (_state.dc_openloop_setpoint > 0)) ||
 		(_state.mode == MOTOR_CONTROL_MODE_RPM && (_state.rpm_setpoint > 0));
 
 	if (need_start && (_state.num_unexpected_stops < _params.num_unexpected_stops_to_latch)) {
 		const uint64_t timestamp = motor_rtctl_timestamp_hnsec();
 
-		_state.dc_actual = spinup_dc;
-		motor_rtctl_start(spinup_dc, _params.reverse, _state.num_unexpected_stops);
-		_state.rtctl_state = motor_rtctl_get_state();
+		_state.dc_actual = _params.dc_min_voltage / _state.input_voltage;
 
-		// This HACK prevents the setpoint TTL expiration in case of protracted startup
-		const int elapsed_ms = (motor_rtctl_timestamp_hnsec() - timestamp) / HNSEC_PER_MSEC;
-		_state.setpoint_ttl_ms += elapsed_ms;
+		motor_rtctl_start(_params.dc_spinup_voltage / _state.input_voltage,
+		                  _params.dc_min_voltage    / _state.input_voltage,
+		                  _params.spinup_voltage_ramp_duration,
+		                  _params.reverse, _state.num_unexpected_stops);
+
+		_state.rtctl_state = motor_rtctl_get_state();
 
-		printf("Motor: Startup %i ms, DC %f, mode %i\n", elapsed_ms, spinup_dc, _state.mode);
+		// This HACK prevents the setpoint TTL expiration in case of long startup
+		_state.setpoint_ttl_ms += (motor_rtctl_timestamp_hnsec() - timestamp) / HNSEC_PER_MSEC;
 
 		if (_state.rtctl_state == MOTOR_RTCTL_STATE_IDLE) {
 			handle_unexpected_stop();

firmware/src/motor/realtime/adc.h

@@ -44,6 +44,7 @@ extern "C" {
 
 extern const int MOTOR_ADC_SYNC_ADVANCE_NANOSEC;
 extern const int MOTOR_ADC_SAMPLE_WINDOW_NANOSEC;
+extern const int MOTOR_ADC_MIN_BLANKING_TIME_NANOSEC;
 
 
 struct motor_adc_sample

firmware/src/motor/realtime/api.h

@@ -92,11 +92,14 @@ void motor_rtctl_confirm_initialization(void);
 
 /**
  * Start the motor.
- * @param [in] duty_cycle           PWM duty cycle that will be applied once the motor has started, (0; 1]
- * @param [in] reverse              Spin direction
- * @param [in] num_prior_attempts   Number of attempts performed before this one, used to switch spinup settings
- */
-void motor_rtctl_start(float duty_cycle, bool reverse, unsigned num_prior_attempts);
+ * @param [in] initial_duty_cycle       Initial PWM duty cycle, (0; 1]
+ * @param [in] target_duty_cycle        PWM duty cycle that will be applied once the motor has started, (0; 1]
+ * @param [in] spinup_ramp_duration     Duration of the duty cycle ramp in seconds
+ * @param [in] reverse                  Spin direction
+ * @param [in] num_prior_attempts       Number of attempts performed before this one, used to switch spinup settings
+ */
+void motor_rtctl_start(float initial_duty_cycle, float target_duty_cycle,
+                       float spinup_ramp_duration, bool reverse, unsigned num_prior_attempts);
 
 /**
  * Engage freewheeling.

firmware/src/motor/realtime/motor_adc.c

@@ -47,7 +47,7 @@
 #define ADC_REF_VOLTAGE          3.3f
 #define ADC_RESOLUTION           12
 
-#define NUM_SAMPLES_PER_ADC      7
+#define NUM_SAMPLES_PER_ADC      4
 
 /**
  * One ADC sample at maximum speed takes 14 cycles; max ADC clock at 72 MHz input is 12 MHz, so one ADC sample is:
@@ -58,11 +58,16 @@
 /**
  * ADC will be triggered at this time before the PWM mid cycle.
  */
-//const int MOTOR_ADC_SYNC_ADVANCE_NANOSEC = (SAMPLE_DURATION_NANOSEC * (NUM_SAMPLES_PER_ADC - 1)) / 2;
-const int MOTOR_ADC_SYNC_ADVANCE_NANOSEC = 0;
+const int MOTOR_ADC_SYNC_ADVANCE_NANOSEC = (SAMPLE_DURATION_NANOSEC * (NUM_SAMPLES_PER_ADC - 1)) / 2;
 
 const int MOTOR_ADC_SAMPLE_WINDOW_NANOSEC = SAMPLE_DURATION_NANOSEC * NUM_SAMPLES_PER_ADC;
 
+/**
+ * This parameter is dictated by the phase voltage RC filters.
+ * Higher oversampling allows for a lower blanking time, due to stronger averaging.
+ */
+const int MOTOR_ADC_MIN_BLANKING_TIME_NANOSEC = 9000;
+
 
 CONFIG_PARAM_FLOAT("mot_i_shunt_mr",         5.0,   0.1,   100.0)
 
@@ -76,34 +81,29 @@ static struct motor_adc_sample _sample;
 __attribute__((optimize(3)))
 CH_FAST_IRQ_HANDLER(Vector88)	// ADC1 + ADC2 handler
 {
-	TESTPAD_SET(GPIO_PORT_TEST_ADC, GPIO_PIN_TEST_ADC);
-
 	_sample.timestamp = motor_timer_hnsec() -
 		((SAMPLE_DURATION_NANOSEC * NUM_SAMPLES_PER_ADC) / 2) / NSEC_PER_HNSEC;
 
 #define SMPLADC1(num)     (_adc1_2_dma_buffer[num] & 0xFFFFU)
 #define SMPLADC2(num)     (_adc1_2_dma_buffer[num] >> 16)
 	/*
 	 * ADC channel sampling:
-	 *   A B C A B C VOLT
-	 *   C A B C A B CURR
+	 *   VOLT A A C
+	 *   CURR C B B
 	 */
-	_sample.phase_values[0] = (SMPLADC1(0) + SMPLADC2(1) + SMPLADC1(3) + SMPLADC2(4)) / 4;
-	_sample.phase_values[1] = (SMPLADC1(1) + SMPLADC2(2) + SMPLADC1(4) + SMPLADC2(5)) / 4;
-	_sample.phase_values[2] = (SMPLADC2(0) + SMPLADC1(2) + SMPLADC2(3) + SMPLADC1(5)) / 4;
+	_sample.phase_values[0] = (SMPLADC1(1) + SMPLADC1(2)) / 2;
+	_sample.phase_values[1] = (SMPLADC2(2) + SMPLADC2(3)) / 2;
+	_sample.phase_values[2] = (SMPLADC2(1) + SMPLADC1(3)) / 2;
 
-	_sample.input_voltage = SMPLADC1(6);
-	_sample.input_current = SMPLADC2(6);
+	_sample.input_voltage = SMPLADC1(0);
+	_sample.input_current = SMPLADC2(0);
 
 #undef SMPLADC1
 #undef SMPLADC2
 
 	motor_adc_sample_callback(&_sample);
 
-	// TODO: check if the current/voltage/temperature channels need to be sampled
-
 	ADC1->SR = 0;         // Reset the IRQ flags
-	TESTPAD_CLEAR(GPIO_PORT_TEST_ADC, GPIO_PIN_TEST_ADC);
 }
 
 static void adc_calibrate(ADC_TypeDef* const adc)
@@ -154,28 +154,25 @@ static void enable(void)
 
 	/*
 	 * ADC channel sampling:
-	 *   A B C A B C VOLT
-	 *   C A B C A B CURR
+	 *   VOLT A A C
+	 *   CURR C B B
+	 * BEMF is sampled in the last order because the BEMF signal conditioning circuits need several
+	 * microseconds to stabilize. Moving these channels to the end of the sequence allows us to reduce
+	 * the overall sampling time.
 	 */
-	ADC1->SQR1 = ADC_SQR1_L_1 | ADC_SQR1_L_2;
+	ADC1->SQR1 = ADC_SQR1_L_0 | ADC_SQR1_L_1;
 	ADC1->SQR3 =
-		ADC_SQR3_SQ1_0 |
-		ADC_SQR3_SQ2_1 |
-		ADC_SQR3_SQ3_0 | ADC_SQR3_SQ3_1 |
-		ADC_SQR3_SQ4_0 |
-		ADC_SQR3_SQ5_1 |
-		ADC_SQR3_SQ6_0 | ADC_SQR3_SQ6_1;
-	ADC1->SQR2 = ADC_SQR2_SQ7_2;
+		ADC_SQR3_SQ1_2 |
+		ADC_SQR3_SQ2_0 |
+		ADC_SQR3_SQ3_0 |
+		ADC_SQR3_SQ4_0 | ADC_SQR3_SQ4_1;
 
 	ADC2->SQR1 = ADC1->SQR1;
 	ADC2->SQR3 =
-		ADC_SQR3_SQ1_0 | ADC_SQR3_SQ1_1 |
-		ADC_SQR3_SQ2_0 |
+		ADC_SQR3_SQ1_2 | ADC_SQR3_SQ1_0 |
+		ADC_SQR3_SQ2_0 | ADC_SQR3_SQ2_1 |
 		ADC_SQR3_SQ3_1 |
-		ADC_SQR3_SQ4_0 | ADC_SQR3_SQ4_1 |
-		ADC_SQR3_SQ5_0 |
-		ADC_SQR3_SQ6_1;
-	ADC2->SQR2 = ADC_SQR2_SQ7_2 | ADC_SQR2_SQ7_0;
+		ADC_SQR3_SQ4_1;
 
 	// SMPR registers are not configured because they have right values by default