From 7d7712063760b0f0f472934aa6945a378d78b0ee Mon Sep 17 00:00:00 2001
From: Noah Dormann <n.dormann@tum.de>
Date: Wed, 21 Jun 2023 15:04:01 +0200
Subject: [PATCH] refactor: add defines and remove constants from cpp files
---
firmware/build_config/config_spacelock.h | 17 ++-
firmware/src/cpp_main.cpp | 27 ++--
firmware/src/hardware.cpp | 167 +++++++++++++++--------
firmware/src/motor.cpp | 14 +-
firmware/src/motor.h | 32 ++++-
5 files changed, 173 insertions(+), 84 deletions(-)
diff --git a/firmware/build_config/config_spacelock.h b/firmware/build_config/config_spacelock.h
index 2f057be..ae0a8b9 100644
--- a/firmware/build_config/config_spacelock.h
+++ b/firmware/build_config/config_spacelock.h
@@ -2,9 +2,18 @@
// super secure!
#define WITH_BACKDOOR 0
+#define WITH_DEBUG_OUTPUT 1
-#define ENDSTOP_DEAD_WINDOW_USTEPS 10000 // ignore motor start-up current
-#define ENDSTOP_ROTATE_AFTER_EDGE_USTEPS 200000 // how many microsteps to rotate after endstop detection (revolution of key)
+//parameters for lock
+#define UNLOCKING_UREVS 2'000'000 //2 key rotations
+#define UNLOCK_SPEED_UREVS_PER_SEC 1'000'000
+#define UNLOCK_FINAL_SPEED_UREVS_PER_SEC 500'000
+#define UNLOCK_HOLD_DELAY_US 1'000'000
+#define LOCKING_UREVS 250'000 //quarter rotation of the key
+#define LOCKING_SPEED_UREVS_PER_SEC 1'000'000
+
+#define ENDSTOP_DEAD_WINDOW_UREVS 400'000
+#define ENDSTOP_ROTATE_AFTER_EDGE_USTEPS 200'000 // how many microsteps to rotate after endstop detection (revolution of key)
// parameters for endstop: edge detection, it integral... read the code (hardware.h)!
#define EDGE_DETECTION_THRESHOLD_UA 210000
@@ -12,3 +21,7 @@
#define FALLING_EDGE_HEIGHT_RESET_THRESHOLD_RATIO 4
#define IT_INTEGRAL_DECAY_UA 300000
#define IT_INTEGRAL_THRESHOLD_UA_US 10000000000
+
+// motor parameters
+#define MOTOR_REVS_TO_DECIKEY_REVS 4 // actually 4,0 but we divide later in the code
+#define UREV_PER_STEP (uint32_t)(1000000 / 200) // 200 steps per revolution
\ No newline at end of file
diff --git a/firmware/src/cpp_main.cpp b/firmware/src/cpp_main.cpp
index 545849d..87b9b32 100644
--- a/firmware/src/cpp_main.cpp
+++ b/firmware/src/cpp_main.cpp
@@ -13,35 +13,38 @@
static void cpp_main_in_cpp();
static void open_door(StepperMotor &motor);
+static void lock_door(StepperMotor &motor);
static void open_door(StepperMotor &motor)
{
// uart_writeline("opening door \U0001f308");
motor.set_mode(1);
- // TODO: find the correct number which does a full rotation
- uart_writeline("L");
- auto [end_stop_state, urevs_since_endstop] = motor.rotate(5000000, 1000000 * 4, 400000);
+
+ auto [end_stop_state, urevs_since_endstop] = motor.rotate(UNLOCKING_UREVS, UNLOCK_SPEED_UREVS_PER_SEC, ENDSTOP_DEAD_WINDOW_UREVS);
if (end_stop_state == EndStopState::RISING_EDGE)
{
// keep moving for a few extra usteps
- uart_writeline("K");
if (urevs_since_endstop < ENDSTOP_ROTATE_AFTER_EDGE_USTEPS)
{
- motor.rotate(ENDSTOP_ROTATE_AFTER_EDGE_USTEPS - urevs_since_endstop, 1000000 * 2, -1);
+ motor.rotate(ENDSTOP_ROTATE_AFTER_EDGE_USTEPS - urevs_since_endstop, UNLOCK_FINAL_SPEED_UREVS_PER_SEC, -1);
}
}
else
{
- uart_writeline("N");
+ uart_writeline("over current detected");
}
motor.set_mode(0);
- sleep_us(1000000);
+ sleep_us(UNLOCK_HOLD_DELAY_US);
+ lock_door(motor);
+}
+
+static void lock_door(StepperMotor &motor){
+ //rotate backwards
motor.set_mode(-1);
- // TODO: find the correct number which does a quarter or so backrotation
- uart_writeline("B");
- motor.rotate(1000000, 1000000 * 4, -1);
- uart_writeline("BD");
+
+ motor.rotate(LOCKING_UREVS, LOCKING_SPEED_UREVS_PER_SEC, 2*ENDSTOP_DEAD_WINDOW_UREVS);
+ uart_writeline("locking door");
motor.set_mode(0);
}
@@ -134,7 +137,7 @@ static void cpp_main_in_cpp()
#if WITH_BACKDOOR
uart_writeline("you used the \x1b[32;1;5msuper-secret\x1b[m backdoor!");
#else
- uart_writeline("lol noob");
+ uart_writeline("backdoor disabled");
#endif
#if WITH_BACKDOOR
open_door(motor);
diff --git a/firmware/src/hardware.cpp b/firmware/src/hardware.cpp
index 15b578d..cd92079 100644
--- a/firmware/src/hardware.cpp
+++ b/firmware/src/hardware.cpp
@@ -9,14 +9,14 @@
static uint64_t timer_extended_bits = 0;
-uint64_t time_get_64() {
+uint64_t time_get_64()
+{
CriticalSectionLock lk;
return time_get_64_isr();
}
static uint64_t prev_result = 0;
-
// extended bits timer
// 00000000000000000000000000 00000000
// 00000000000000000000000000 01111111
@@ -26,22 +26,22 @@ static uint64_t prev_result = 0;
// 00000000000000000000000010 00000000 inc
// 00000000000000000000000010 00000010
-
-uint64_t time_get_64_isr() {
+uint64_t time_get_64_isr()
+{
uint16_t timer_val = time_get_16();
bool timer_msb = timer_val & 0x8000;
- if ((timer_extended_bits & 1) != timer_msb) {
+ if ((timer_extended_bits & 1) != timer_msb)
+ {
timer_extended_bits += 1;
}
- uint64_t result = (
- static_cast<uint64_t>(timer_extended_bits << 15) |
- static_cast<uint64_t>(timer_val)
- );
+ uint64_t result = (static_cast<uint64_t>(timer_extended_bits << 15) |
+ static_cast<uint64_t>(timer_val));
- if (result < prev_result) {
+ if (result < prev_result)
+ {
// wtf this is supposed to be monotonic
// this can happen if interrupts were blocked for > 30ms
@@ -54,7 +54,8 @@ uint64_t time_get_64_isr() {
return result;
}
-void timer_update_extended_bits() {
+void timer_update_extended_bits()
+{
/**
bool timer_msb = time_get_16() & 0x8000;
@@ -74,35 +75,45 @@ UARTRxBuffer *current_rxbuf = &rxbuf_a;
// this is the buffer which is being processed
UARTRxBuffer *current_procbuf = &rxbuf_b;
-void uart_data_received(uint8_t byte) {
- if (byte == '\0' || byte == '\r' || byte == '\n') {
+void uart_data_received(uint8_t byte)
+{
+ if (byte == '\0' || byte == '\r' || byte == '\n')
+ {
current_rxbuf->finish();
- } else {
+ }
+ else
+ {
current_rxbuf->add(byte);
}
}
UARTTxBuffer txbuf;
-void uart_transmit_next() {
- if (!(huart1.Instance->SR & USART_SR_TXE)) {
+void uart_transmit_next()
+{
+ if (!(huart1.Instance->SR & USART_SR_TXE))
+ {
// currently transmitting
return;
}
int byte = txbuf.get_next();
- if (byte < 0) {
+ if (byte < 0)
+ {
// we're done. manually reset the TC bit,
// otherwise we'll get an infinite interrupt loop.
huart1.Instance->SR &= ~USART_SR_TC;
- } else {
+ }
+ else
+ {
// send the next byte
huart1.Instance->DR = byte;
}
}
-void uart_write_u16(uint16_t data){
- uint8_t a,b;
+void uart_write_u16(uint16_t data)
+{
+ uint8_t a, b;
uint8_t mask = 0b11000000;
a = mask | (data & ~mask);
b = mask | (data >> 6 & ~mask);
@@ -111,15 +122,21 @@ void uart_write_u16(uint16_t data){
uart_transmit_next();
}
-void uart_writeline(const char *text, const uint64_t *param) {
+void uart_writeline(const char *text, const uint64_t *param)
+{
txbuf.add(text);
- if (param != nullptr) {
+ if (param != nullptr)
+ {
bool started = false;
for (int8_t shift = 60; shift >= 0; shift -= 4)
{
uint8_t digit = (*param >> shift) & 0xf;
- if (digit != 0) { started = true; }
- if (started || (shift == 0)) {
+ if (digit != 0)
+ {
+ started = true;
+ }
+ if (started || (shift == 0))
+ {
txbuf.add("0123456789abcdef"[digit]);
}
}
@@ -131,7 +148,8 @@ void uart_writeline(const char *text, const uint64_t *param) {
EndStopDetector endstop_detector;
-void adc_value_received(uint16_t adc_value) {
+void adc_value_received(uint16_t adc_value)
+{
// This is called at 125 kHz / 7 = 17857 Hz
// ADC range 12 bits, reference 3.3V, 1 V/A -> 3.3 / (2**12 - 1) * 1.0 * 1e6
uint32_t current_nA = adc_value * 806;
@@ -150,10 +168,14 @@ void adc_value_received(uint16_t adc_value) {
// }
}
-UARTRxBuffer *uart_poll_message() {
+UARTRxBuffer *uart_poll_message()
+{
CriticalSectionLock lk;
- if (!current_rxbuf->finished) { return nullptr; }
+ if (!current_rxbuf->finished)
+ {
+ return nullptr;
+ }
UARTRxBuffer *tmp = current_rxbuf;
@@ -170,33 +192,43 @@ void EndStopDetector::current_received(uint32_t current_uA)
// currents are coming in at 125 / 7 kHz.
// we want to filter them with a time constant of 10 ms
// -> alpha = 1/(1-exp(-delta_t/tau)) = 179
- uint32_t filtered_current_uA_new = (filtered_current_uA * 178 + current_uA)/179;
+ uint32_t filtered_current_uA_new = (filtered_current_uA * 178 + current_uA) / 179;
uint64_t filtered_current_timestamp_new = time_get_64_isr();
uint32_t delta_t = filtered_current_timestamp_new - filtered_current_timestamp;
filtered_current_uA = filtered_current_uA_new;
filtered_current_timestamp = filtered_current_timestamp_new;
- it_integral_uA_us += (uint64_t) filtered_current_uA_new * (uint64_t) delta_t;
- uint64_t it_integral_decay_uA_us = (uint64_t) IT_INTEGRAL_DECAY_UA * (uint64_t) delta_t;
- if (it_integral_uA_us < it_integral_decay_uA_us) {
+ it_integral_uA_us += (uint64_t)filtered_current_uA_new * (uint64_t)delta_t;
+ uint64_t it_integral_decay_uA_us = (uint64_t)IT_INTEGRAL_DECAY_UA * (uint64_t)delta_t;
+ if (it_integral_uA_us < it_integral_decay_uA_us)
+ {
it_integral_uA_us = 0;
- } else {
+ }
+ else
+ {
it_integral_uA_us -= it_integral_decay_uA_us;
}
- if (it_integral_uA_us > IT_INTEGRAL_THRESHOLD_UA_US) {
+ if (it_integral_uA_us > IT_INTEGRAL_THRESHOLD_UA_US)
+ {
+#if WITH_DEBUG_OUTPUT
// it integral threshold reached
- if (state == EndStopState::NONE) {
+ if (state == EndStopState::NONE)
+ {
uart_writeline("i*t limit reached");
}
+#endif
it_integral_uA_us = 0;
state = EndStopState::OVERCURRENT;
end_stop_timestamp = filtered_current_timestamp_new;
}
static uint8_t ctr = 0;
- if (++ctr == 20) {
+ if (++ctr == 20)
+ {
ctr = 0;
- } else {
+ }
+ else
+ {
return;
}
@@ -206,10 +238,13 @@ void EndStopDetector::current_received(uint32_t current_uA)
if (filtered_current_uA >= edge_start_uA + EDGE_DETECTION_THRESHOLD_UA)
{
+#if WITH_DEBUG_OUTPUT
// edge detected
- if (state == EndStopState::NONE) {
+ if (state == EndStopState::NONE)
+ {
uart_writeline("edge detected");
}
+#endif
state = EndStopState::RISING_EDGE;
end_stop_timestamp = edge_start_timestamp;
edge_start_uA = filtered_current_uA;
@@ -219,31 +254,35 @@ void EndStopDetector::current_received(uint32_t current_uA)
}
if (
- filtered_current_uA - edge_start_uA
- <=
- EDGE_STEEPNESS_THRESHOLD_A_PER_S * (filtered_current_timestamp - edge_start_timestamp)
- ) {
+ filtered_current_uA - edge_start_uA <=
+ EDGE_STEEPNESS_THRESHOLD_A_PER_S * (filtered_current_timestamp - edge_start_timestamp))
+ {
// the edge was not steep enough, reset its start to here
edge_start_uA = filtered_current_uA;
edge_start_timestamp = filtered_current_timestamp;
edge_max_uA = filtered_current_uA;
}
- if (filtered_current_uA < filtered_current_uA_old) {
+ if (filtered_current_uA < filtered_current_uA_old)
+ {
uint32_t falling_edge_height = edge_max_uA - filtered_current_uA;
- if (falling_edge_height * FALLING_EDGE_HEIGHT_RESET_THRESHOLD_RATIO > (edge_max_uA - edge_start_uA)) {
+ if (falling_edge_height * FALLING_EDGE_HEIGHT_RESET_THRESHOLD_RATIO > (edge_max_uA - edge_start_uA))
+ {
edge_max_uA = filtered_current_uA;
edge_start_uA = filtered_current_uA;
edge_start_timestamp = filtered_current_timestamp;
}
}
- if (filtered_current_uA > edge_max_uA) {
+ if (filtered_current_uA > edge_max_uA)
+ {
edge_max_uA = filtered_current_uA;
}
+#if WITH_DEBUG_OUTPUT
uart_write_u16((uint16_t)(filtered_current_uA / 1000));
uart_write_u16((uint16_t)(it_integral_uA_us / 100000000));
+#endif
}
EndStopState EndStopDetector::get_end_stop_state(uint64_t *timestamp)
@@ -251,7 +290,8 @@ EndStopState EndStopDetector::get_end_stop_state(uint64_t *timestamp)
CriticalSectionLock lk;
auto result = state;
- if (state != EndStopState::NONE) {
+ if (state != EndStopState::NONE)
+ {
*timestamp = end_stop_timestamp;
state = EndStopState::NONE;
}
@@ -266,18 +306,24 @@ void EndStopDetector::clear_end_stop_state()
state = EndStopState::NONE;
}
-int UARTTxBuffer::get_next() {
- if (this->start_pos == this->end_pos) { return -1; }
+int UARTTxBuffer::get_next()
+{
+ if (this->start_pos == this->end_pos)
+ {
+ return -1;
+ }
uint8_t result = this->buf[this->start_pos];
start_pos = (start_pos + 1) % this->buf.size();
return result;
}
-bool UARTTxBuffer::add(uint8_t byte) {
+bool UARTTxBuffer::add(uint8_t byte)
+{
CriticalSectionLock lk;
uint32_t new_end_pos = (this->end_pos + 1) % this->buf.size();
- if (new_end_pos == this->start_pos) {
+ if (new_end_pos == this->start_pos)
+ {
// the tx buffer is full
return false;
}
@@ -287,17 +333,26 @@ bool UARTTxBuffer::add(uint8_t byte) {
return true;
}
-bool UARTTxBuffer::add(const char *buf) {
- while (*buf) {
- if (!this->add(static_cast<uint8_t>(*(buf++)))) { return false; }
+bool UARTTxBuffer::add(const char *buf)
+{
+ while (*buf)
+ {
+ if (!this->add(static_cast<uint8_t>(*(buf++))))
+ {
+ return false;
+ }
}
return true;
-
}
-bool UARTTxBuffer::add(const uint8_t *buf, uint32_t len) {
- while (len-- > 0) {
- if (!this->add(*(buf++))) { return false; }
+bool UARTTxBuffer::add(const uint8_t *buf, uint32_t len)
+{
+ while (len-- > 0)
+ {
+ if (!this->add(*(buf++)))
+ {
+ return false;
+ }
}
return true;
}
diff --git a/firmware/src/motor.cpp b/firmware/src/motor.cpp
index 8cb159c..9adf643 100644
--- a/firmware/src/motor.cpp
+++ b/firmware/src/motor.cpp
@@ -83,19 +83,7 @@ std::pair<EndStopState, uint32_t> StepperMotor::rotate(uint32_t urevs, uint32_t
if (microsteps == 0) { return { EndStopState::NONE, 0}; }
- uint32_t microstep_period_us = (
- static_cast<uint64_t>(
- static_cast<uint64_t>(1000000)
- *
- static_cast<uint64_t>(this->urev_per_step)
- )
- /
- static_cast<uint64_t>(
- static_cast<uint64_t>(this->microsteps_per_step)
- *
- static_cast<uint64_t>(urev_per_second)
- )
- );
+ uint32_t microstep_period_us = urev_per_second_to_microstep_period_us(urev_per_second);
if (microstep_period_us < 1) { microstep_period_us = 1; }
diff --git a/firmware/src/motor.h b/firmware/src/motor.h
index 93060ed..6bff87c 100644
--- a/firmware/src/motor.h
+++ b/firmware/src/motor.h
@@ -5,6 +5,7 @@
#include "pin.h"
#include "hardware.h"
+#include "config.h"
#ifndef __cplusplus
#error lolnope
@@ -23,9 +24,10 @@ public:
~StepperMotor();
- static constexpr uint32_t urev_per_step = (uint32_t)(1000000 / 200);
+ static constexpr uint32_t urev_per_step = UREV_PER_STEP;
static constexpr uint32_t sleep_wakeup_time_us = 1700;
static constexpr uint32_t pin_hold_time_us = 2;
+ static constexpr uint32_t motor_revs_to_decikey_revs = MOTOR_REVS_TO_DECIKEY_REVS;
void set_mode(int8_t mode);
// returns a tuple of
@@ -54,6 +56,10 @@ private:
static_cast<uint64_t>(this->microsteps_per_step)
*
static_cast<uint64_t>(urev)
+ *
+ static_cast<uint64_t>(motor_revs_to_decikey_revs)
+ /
+ 10
)
/
static_cast<uint64_t>(this->urev_per_step)
@@ -66,9 +72,33 @@ private:
static_cast<uint64_t>(this->urev_per_step)
*
static_cast<uint64_t>(microsteps)
+ *
+ 10
+ /
+ static_cast<uint64_t>(motor_revs_to_decikey_revs)
)
/
static_cast<uint64_t>(this->microsteps_per_step)
);
}
+
+ inline uint32_t urev_per_second_to_microstep_period_us(uint32_t urev_per_second) {
+ return (
+ static_cast<uint64_t>(
+ static_cast<uint64_t>(1'000'000)
+ *
+ static_cast<uint64_t>(this->urev_per_step)
+ )
+ /
+ static_cast<uint64_t>(
+ static_cast<uint64_t>(this->microsteps_per_step)
+ *
+ static_cast<uint64_t>(urev_per_second)
+ *
+ static_cast<uint64_t>(motor_revs_to_decikey_revs)
+ /
+ 10
+ )
+ );
+ }
};
--
GitLab