Expanded on stdio logging functionality, also configuration of logging is now entirely in CMakeLists.txt for convenience, also warns now for logging configurations that make no sense; Clarification in CV table comment; Moved pin definitions into board definition files; two_std_dev() now works mostly with integers; Added function for resetting all CVs saved in flash; Added various LOG() messages;

This commit is contained in:
gab-k committed 2024-12-22 21:48:41 +01:00
1 parent 7b1d5ed152
commit f8b99de801
9 files changed
+311 -119

No files matched your search

+126 -74
View File
@@ -63,43 +63,43 @@ static void call_flash_range_program(void *param) {
}
float two_std_dev(const float arr[], const uint32_t length) {
uint16_t two_std_dev(const uint16_t arr[], const uint32_t length) {
// Calculate arithmetic average
float sum = 0;
uint64_t sum = 0;
for (uint32_t i = 0; i < length; ++i) {
sum += arr[i];
}
const float x_avg = sum / (float)length;
const uint64_t x_avg = sum / length;
// Calculate sample variance and sampled standard deviation
sum = 0;
for (uint32_t i = 0; i < length; ++i) {
sum += (arr[i] - x_avg) * (arr[i] - x_avg);
}
const float variance = sum / (float)(length-1);
const float std_dev = sqrtf(variance);
const uint16_t variance = sum / (length-1);
const uint16_t std_dev = (uint16_t) sqrtf((float) variance);
// Sum every element that is within two times the sampled standard deviation and compute new average
sum = 0;
uint32_t counter = 0;
for (uint32_t i = 0; i < length; ++i) {
const float diff = fabsf(arr[i] - x_avg);
if ( diff < std_dev * 2.0f ){
const uint32_t diff = abs(arr[i] - x_avg);
if ( diff <= 2*std_dev ){
sum += arr[i];
counter++;
}
}
return sum / (float) counter;
return sum / counter;
}
// Measures constant adc offset and programs the offset into flash
// TODO: check necessity and correctness of adc offset function
void adc_offset_adjustment(const uint32_t n) {
LOG(1, "adc_offset_adjustment");
LOG(1, "ADC offset adjustment...\n");
float offsets_fwd[n];
float offsets_rev[n];
uint16_t offsets_fwd[n];
uint16_t offsets_rev[n];
// Set PWM level to 0 just to be sure
pwm_set_gpio_level(MOTOR_FWD_PIN, 0);
pwm_set_gpio_level(MOTOR_REV_PIN, 0);
@@ -109,7 +109,7 @@ void adc_offset_adjustment(const uint32_t n) {
adc_select_input(2); //TODO: "2" as input argument?
adc_run(true);
for (uint32_t i = 0; i < n; i++) {
offsets_rev[i] = (float) adc_fifo_get_blocking();
offsets_rev[i] = adc_fifo_get_blocking();
}
adc_run(false);
adc_fifo_drain();
@@ -119,23 +119,22 @@ void adc_offset_adjustment(const uint32_t n) {
adc_select_input(3);
adc_run(true);
for (uint32_t i = 0; i < n; i++) {
offsets_fwd[i] = (float) adc_fifo_get_blocking();
offsets_fwd[i] = adc_fifo_get_blocking();
}
adc_run(false);
adc_fifo_drain();
// Find overall average discarding outliers in measurement
const float offset_avg_fwd = two_std_dev(offsets_fwd, n);
const float offset_avg_rev = two_std_dev(offsets_rev, n);
const float overall_avg = (offset_avg_fwd + offset_avg_rev) / 2;
const uint8_t offset = (uint8_t) roundf(overall_avg);
const uint16_t offset_avg_fwd = two_std_dev(offsets_fwd, n);
const uint16_t offset_avg_rev = two_std_dev(offsets_rev, n);
const uint16_t overall_avg_offset = (offset_avg_fwd + offset_avg_rev) / 2;
LOG(1, "new adc offset CV[171] (%f): (uint8_t)%d\n", overall_avg, offset);
LOG(1, "New ADC Offset value CV_172 = %u\n", overall_avg_offset);
// Create temporary array -> change CV 172 in temp array -> erase flash -> write temp array to flash
uint8_t CV_ARRAY_TEMP[CV_ARRAY_SIZE];
memcpy(CV_ARRAY_TEMP, CV_ARRAY_FLASH, sizeof(CV_ARRAY_TEMP));
CV_ARRAY_TEMP[171] = offset;
CV_ARRAY_TEMP[171] = overall_avg_offset;
uintptr_t params_erase[] = {FLASH_TARGET_OFFSET, FLASH_SECTOR_SIZE};
int ret_val = flash_safe_execute(call_flash_range_erase, params_erase, UINT32_MAX);
if (ret_val != PICO_OK){
@@ -198,6 +197,20 @@ void write_cv_byte(uint16_t cv_address, uint8_t cv_data) {
acknowledge();
}
void reset_cv_array_to_default(){
uintptr_t params_erase[] = {FLASH_TARGET_OFFSET, FLASH_SECTOR_SIZE};
int ret_val = flash_safe_execute(call_flash_range_erase, params_erase, UINT32_MAX);
if (ret_val != PICO_OK){
set_error(FLASH_SAFE_EXECUTE_ERASE_FAILURE);
return;
}
uintptr_t params_program[] = {FLASH_TARGET_OFFSET, sizeof(CV_ARRAY_DEFAULT), (uintptr_t)CV_ARRAY_DEFAULT};
ret_val = flash_safe_execute(call_flash_range_program, params_program, UINT32_MAX);
if(ret_val != PICO_OK){
set_error(FLASH_SAFE_EXECUTE_PROGRAM_FAILURE);
return;
}
}
// There are some exceptions to writing CVs - e.g. writing to a read only CV is not valid - this gets handled here
void write_cv_handler(const uint16_t cv_index, const uint8_t cv_data) {
@@ -213,29 +226,18 @@ void write_cv_handler(const uint16_t cv_index, const uint8_t cv_data) {
break;
case 6: //CV_7
// Read only (CV_7 - Version no.)
// ADC offset Adjustment (CV_7; Value = 7);
// ADC offset Adjustment is triggered when setting CV_7 = 7
if (cv_data == 7) {
LOG(1, "trigger adc offset adjustment via cv7 => 7\n");
adc_offset_adjustment(8192);
LOG(1, "Triggered ADC offset adjustment via CV_7 = 7\n");
adc_offset_adjustment(ADC_CALIBRATION_ITERATIONS);
}
break;
case 7: //CV_8
// Read only (CV_8 - Manufacturer ID)
// Reset all CVs to Default (CV_8; Value = 8)
// Reset all CVs to default (CV_ARRAY_DEFAULT) when setting CV_8 = 8)
if (cv_data == 8) {
LOG(1, "reset of flash triggered via cv8 => 8\n");
uintptr_t params_erase[] = {FLASH_TARGET_OFFSET, FLASH_SECTOR_SIZE};
int ret_val = flash_safe_execute(call_flash_range_erase, params_erase, UINT32_MAX);
if (ret_val != PICO_OK){
set_error(FLASH_SAFE_EXECUTE_ERASE_FAILURE);
return;
}
uintptr_t params_program[] = {FLASH_TARGET_OFFSET, sizeof(CV_ARRAY_DEFAULT), (uintptr_t)CV_ARRAY_DEFAULT};
ret_val = flash_safe_execute(call_flash_range_program, params_program, UINT32_MAX);
if(ret_val != PICO_OK){
set_error(FLASH_SAFE_EXECUTE_PROGRAM_FAILURE);
return;
}
LOG(1, "reset of flash triggered via CV_8 = 8\n");
reset_cv_array_to_default();
}
break;
// CV_17 must have a value between 11000000->(192dec) and 11100111->(231dec) inclusive
@@ -575,6 +577,7 @@ void track_signal_fall( const unsigned int gpio, const long unsigned int events)
}
void init_outputs() {
LOG(1, "Initializing Outputs...\n");
gpio_init_mask(GPIO_ALLOWED_OUTPUTS);
gpio_set_dir_out_masked(GPIO_ALLOWED_OUTPUTS); // Note: This might also disable UART on GPIO0 & GPIO1
const uint32_t PWM_enabled_outputs = get_32bit_CV(111) & GPIO_ALLOWED_OUTPUTS;
@@ -592,33 +595,32 @@ void init_outputs() {
level_table[i] = level;
pwm_set_clkdiv_int_frac(slice, clock_divider, 0);
pwm_set_enabled(slice, true);
LOG(1, "Set GPIO pin: %u as PWM output; PWM slice: %u; PWM channel: %u; Duty cycle: %f; Clock divider: %u\n", i, slice, channel, (float) level/wrap, clock_divider);
}
mask <<= 1;
}
}
void cv_setup_check() {
LOG(1, "Checking CV array for factory state of flash or missing ADC offset setup...\n");
// Check for flash factory setting and set CV_FLASH_ARRAY to default values when factory condition ("0xFF") is found.
if (true || CV_ARRAY_FLASH[64] == 0xFF) {
LOG(1, "found cv[64] equals to 0xff, reseting CVs (and CV[8] = 8)\n");
const uint8_t arr[4] = {125, 8, 7, 124};
program_mode(4, arr); //reset to CV_ARRAY_DEFAULT (write CV_8 = 8)
if (CV_ARRAY_FLASH[64] == 0xFF) {
LOG(1, "Detected flash memory factory condidition (CV_65 == 0xFF), resetting all CVs to default values...\n");
reset_cv_array_to_default();
}
// Check for existing ADC offset setup TODO: always measure?
if (false && CV_ARRAY_FLASH[171] == 0xFF) {
// Write a value of 7 to read-only CV_7 in order to trigger a ADC offset measurement
LOG(1, "found cv[171] equals to 0xff, adc offset adjstments (and cr[7] = 7)\n");
const uint8_t arr[4] = {125, 7, 6, 124};
program_mode(4, arr); // ADC offset adjustment (write CV_7 = 7)
// Check for existing ADC offset setup
if (CV_ARRAY_FLASH[171] == 0xFF) {
LOG(1, "Detected ADC offset factory condidition (CV_172 == 0xFF), running offset adjustment measurement function...\n");
adc_offset_adjustment(ADC_CALIBRATION_ITERATIONS);
}
LOG(1, "int_lim_max %d\n", CV_ARRAY_FLASH[51]);
LOG(1, "int_lim_min %d\n", CV_ARRAY_FLASH[52]);
LOG(1, "CV check done!\n");
}
// Motor PWM initialization
void init_motor_pwm(const uint8_t gpio) {
LOG(2, "Initializing Motor PWM on GPIO pin: %u...\n", gpio);
// Set GPIO pin to PWM functionality
gpio_set_function(gpio, GPIO_FUNC_PWM);
// Set wrap counter value saved in CV
@@ -630,10 +632,11 @@ void init_motor_pwm(const uint8_t gpio) {
pwm_set_gpio_level(gpio, 0);
// Enable PWM
pwm_set_enabled(slice_num, true);
LOG(2, "Initialized Motor PWM on pin: %d; wrap_counter %d; clkdiv %d;\n", gpio, wrap_counter, CV_ARRAY_FLASH[173]);
LOG(2, "Initialized motor PWM on pin: %u; wrap_counter %u; clkdiv %d;\n", gpio, wrap_counter, CV_ARRAY_FLASH[173]);
}
void init_adc() {
LOG(1, "Initializing ADC...\n")
// Initialize ADC
adc_init();
// Set ADC GPIO pins to ADC functionality
@@ -641,35 +644,84 @@ void init_adc() {
adc_gpio_init(REV_V_EMF_ADC_PIN);
// Configure ADC FIFO
adc_fifo_setup(true, false, 0, false, false);
LOG(1, "ADC initialization done!\n")
}
int main() {
if(stdio_init_all() != true){
set_error(STDIO_INIT_FAILURE);
}
multicore_launch_core1(core1_entry);
LOG(1, "core0 Initialization...\n");
// Wait 1ms to give core1 some time to call flash_safe_execute_core_init()
busy_wait_ms(1);
// Check for error calling flash_safe_execute_core_init() on core1
if(get_error_state() & FLASH_SAFE_EXECUTE_CORE_INIT_FAILURE){
panic("Error calling flash_safe_execute_core_init() on core1!");
}
// Check CV array for factory state of flash or missing ADC offset setup
LOG(1, "Check CV array\n");
cv_setup_check();
LOG(1, "Initializing Motor PWM...\n");
init_motor_pwm(MOTOR_FWD_PIN);
init_motor_pwm(MOTOR_REV_PIN);
LOG(1, "Initializing ADC...")
init_adc();
LOG(1, "Initializing Outputs...\n");
init_outputs();
LOG(1, "Initializing GPIO...\n");
void init_digital_inputs() {
LOG(1, "Initializing digital inputs and track signal irq...\n");
gpio_init(DCC_INPUT_PIN);
gpio_set_dir(DCC_INPUT_PIN, GPIO_IN);
gpio_pull_up(DCC_INPUT_PIN);
gpio_set_irq_enabled_with_callback(DCC_INPUT_PIN, GPIO_IRQ_EDGE_RISE, true, &track_signal_rise);
LOG(1, "core0 Initialized!\n");
while (true); //sleep_ms(100); //TODO: any potential issues when using sleep?
LOG(1, "Digital inputs and track signal irq initialization done!\n");
}
void wait_for_input() {
while (true) {
// Send a message to the user asking them to acknowledge it by sending a reply
LOG(1, "Send any character to continue.\n");
// Wait for a character (with a 1-second timeout)
int received_char = getchar_timeout_us(1000000); // 1 second timeout
if (received_char != PICO_ERROR_TIMEOUT) {
// If a character is received, exit the function
LOG(1, "Character '%c' received. Continuing...\n", (char)received_char);
return;
}
// If no character is received, retry...
}
}
int main() {
// Initialize LED GPIO pin
gpio_init(RP2040_DECODER_DEFAULT_LED_PIN);
gpio_set_dir(RP2040_DECODER_DEFAULT_LED_PIN, GPIO_OUT);
// Initialize stdio
if(stdio_init_all() != true){
set_error(STDIO_INIT_FAILURE);
}
// Wait for user input before continuing, this is only enabled when LOG_WAIT is set to 1 and logging is enabled (LOGLEVEL > 0)
// Also disabled when no STDIO is configured as the user would not be able to send anything
# if (LOGLEVEL > 0 && LOG_WAIT == 1) && (STDIO_UART_ENABLED || STDIO_USB_ENABLED)
wait_for_input();
printf("%u", STDIO_UART_ENABLED);
#endif
LOG(1, "core0 Initialization...\n");
// Launch core1
multicore_launch_core1(core1_entry);
// Wait 10ms to give core1 some time to call flash_safe_execute_core_init()
busy_wait_ms(10);
// Check for error calling flash_safe_execute_core_init() on core1
if(get_error_state() & FLASH_SAFE_EXECUTE_CORE_INIT_FAILURE){
panic("Error calling flash_safe_execute_core_init() on core1!");
}
// Initialize Motor PWM pins
init_motor_pwm(MOTOR_FWD_PIN);
init_motor_pwm(MOTOR_REV_PIN);
// Initialize outputs
init_outputs();
// Initialize ADC
init_adc();
// Check CV array for factory state of flash or missing ADC offset setup
cv_setup_check();
// Initialize digital inputs and track signal irq
init_digital_inputs();
LOG(1, "core0 initialization done!\n");
// Endless loop
while (true) {
tight_loop_contents();
}
}