core0.h
Used exclusively by core0
Defines
-
MESSAGE_3_BYTES
Comparison bitmask for detecting 3 byte long DCC messages/packets.
-
MESSAGE_MASK_3_BYTES
Bitmask for detecting 3 byte long DCC messages/packets, input_bit_buffer is bitwise ANDed with the mask.
-
MESSAGE_4_BYTES
Comparison bitmask for detecting 4 byte long DCC messages/packets.
-
MESSAGE_MASK_4_BYTES
Bitmask for detecting 4 byte long DCC messages/packets, input_bit_buffer is bitwise ANDed with the mask.
-
MESSAGE_5_BYTES
Comparison bitmask for detecting 5 byte long DCC messages/packets.
-
MESSAGE_MASK_5_BYTES
Bitmask for detecting 5 byte long DCC messages/packets, input_bit_buffer is bitwise ANDed with the mask.
-
INVALID_PACKAGE
Return value of detect_dcc_packet() when the packet is invalid, meaning no packet was detected.
-
RING_BUFFER_PACKETS
First dimension of dcc ring buffer array - meaning RING_BUFFER_PACKETS can be retained inside the buffer.
-
RING_BUFFER_BYTES
Secoond dimension of dcc ring buffer array - meaning each packet can have a maximum size of RING_BUFFER_BYTES.
-
FLASH_CMD_READ_JEDEC_ID
Constant value of 0x9F used as a JEDEC ID read command for reading the JEDEC ID of a winbond flash memory chip.
-
FLASH_TIMEOUT_IN_MS
Timeout value for flash access.
-
ADC_CALIBRATION_ITERATIONS
Iterations for measuring adc offset.
-
WATCHDOG_TIMER_IN_MS
Watchdog timer setting, watchdog update needs to be called every WATCHDOG_TIMER_IN_MS milliseconds, otherwise decoder resets itself.
Typedefs
-
typedef struct dcc_packet_t dcc_packet_t
Structure representing a DCC packet.
This structure is used to store the data and length of a DCC packet.
-
typedef struct dcc_ring_buffer_t dcc_ring_buffer_t
Structure representing a ring buffer for DCC packets.
This structure is used to manage a ring buffer that stores DCC packets. It contains an array of packets and indices for writing and reading.
Functions
-
static void increment_ring_buffer_idx(size_t *idx, uint8_t ring_buffer_length)
Increment the index of a ring buffer.
This function increments the provided index of a ring buffer, wrapping around to zero if the end of the buffer is reached.
- Parameters:
idx – Pointer to the index to be incremented.
ring_buffer_length – The length of the ring buffer.
-
static void call_flash_range_erase(void *param)
Function for erasing a sector of flash memory.
This function erases a specified range of flash memory sectors.
- Parameters:
param – Pointer to parameters.
First parameter: Offset into flash, in bytes. Must be aligned to a 4096-byte flash sector.
Second parameter: Number of bytes to be erased. Must be a multiple of 4096 bytes (one sector).
-
static void call_flash_range_program(void *param)
Function for programming a range of flash memory.
This function programs a specified range of flash memory with the provided data.
- Parameters:
param – Pointer to parameters
First parameter: The Flash address of the first byte to be programmed. Must be aligned to a 256-byte flash page.
Second parameter: The number of bytes to program. Must be a multiple of 256 bytes (one page).
Third parameter: Pointer to the data to program into flash.
-
static void call_flash_do_cmd(void *param)
Function for executing a flash command via QSPI.
- Parameters:
param – Pointer to parameters First parameter is the pointer to a byte buffer which will be transmitted to the flash Second parameter is the pointer to a byte buffer where data received from the flash will be written. Third parameter is the length in bytes of any of the two buffers (same size).
-
static unsigned int absolute_val(int x)
Function for calculating the absolute value of an integer.
This function takes an integer as input and returns its absolute value.
- Parameters:
x – The input integer whose absolute value is to be calculated.
- Returns:
The absolute value of the input integer.
-
static uint16_t two_std_dev(const uint16_t arr[], uint32_t length)
Computes the average of values in an array that deviate less than twice the standard deviation from the original average.
This function calculates the average of the elements in the input array that are within two standard deviations of the mean of the array.
- Parameters:
arr – The input array of values.
length – The number of elements in the input array.
- Returns:
The average value of the elements that deviate less than twice the standard deviation from the original average.
-
static void adc_offset_adjustment(uint32_t n)
Measures ADC offset and saves the offset value to flash memory.
This function performs a series of ADC measurements to determine the offset and then stores this offset value in the flash memory for future use.
- Parameters:
n – The number of measurements to be performed to calculate the offset.
-
static void acknowledge()
Function for acknowledging a CV write or CV verify instruction.
Acknowledgement works by creating a spike in current consumption, normally realized by running the motor in alternating directions. This current increase is detected by the command station.
Note
Refers to NMRA S-9.2.3 Section D and RCN-216 Section 3
-
static void verify_cv_bit(uint16_t cv_address, bool bit_val, uint8_t bit_pos)
Function for verifying a single bit of a CV.
- Parameters:
cv_address – CV Address
bit_val – Value of bit to be verified
bit_pos – Position of bit to be verified
-
static void verify_cv_byte(uint16_t cv_address, uint8_t cv_data)
Function for verifying a CV byte.
This function verifies the byte of a Configuration Variable (CV) at the specified address.
- Parameters:
cv_address – The address of the CV to be verified.
cv_data – The byte value to be verified against the CV.
-
static void write_cv_byte(uint16_t cv_address, uint8_t cv_data)
Writes a byte to a Configuration Variable (CV).
This function writes a byte of data to a specified CV address.
- Parameters:
cv_address – The address of the CV to write to.
cv_data – The data byte to write to the CV.
-
static void reset_cv_array_to_default()
This function sets all the CV values stored in flash back to their default settings (CV_ARRAY_DEFAULT).
-
static void program_mode(size_t number_of_bytes, const uint8_t *const byte_array)
CV Programming mode function.
This function is used to enter programming mode in order to write CV values to persistent flash memory or verify/read bits/bytes from flash memory.
Procedure:
Checks for a valid programming command.
Evaluates the type of programming instruction (Write byte / Verify byte / Verify bit) and runs the corresponding function. See verify_cv_byte(), verify_cv_bit() and write_cv_byte().
Note
Refer to NMRA Standard 9.2.3 (“Service Mode Programming”) or RCN-216 for more details.
- Parameters:
number_of_bytes – The number of bytes in the array.
byte_array – A pointer to the byte array.
-
static void set_outputs(uint32_t functions_to_set_bitmask)
Set outputs according to function mapping in CV257 to CV512.
This function maps the functions and direction to the mapped outputs configured in CV257 to CV512. Also enables PWM when enabled according to CV112 to CV115.
- Parameters:
functions_to_set_bitmask – Function bitmask F0 is bit0, F1 is bit1, …
-
static void update_active_functions(uint32_t new_function_bitmask, uint8_t clr_bit_ind, bool direction_change)
Update functions F0 - F31 when function command is received or in case of direction change.
This function updates the active functions based on the provided bitmask. It can also clear a specific bit and handle changes in direction.
- Parameters:
new_function_bitmask – New function bitmask representing the state of functions F0 - F31.
clr_bit_ind – Index of the bit to be cleared in the bitmask.
direction_change – Flag indicating if there is a change in direction.
-
static bool error_detection(size_t number_of_bytes, const uint8_t *byte_array)
Detect errors in the byte array using exor of all bytes.
Exor of all bytes is expected to be “0b00000000”.
- Parameters:
number_of_bytes – Number of bytes in the array.
byte_array – Pointer to the byte array.
- Returns:
true if no error is detected, false otherwise.
-
static bool is_long_address(size_t number_of_bytes, const uint8_t byte_array[])
Check if the address is a long address.
- Parameters:
number_of_bytes – Number of bytes in the array.
byte_array – Pointer to the byte array.
- Returns:
true if it is a long address, false otherwise.
-
static bool address_evaluation(size_t number_of_bytes, const uint8_t byte_array[])
Evaluate the address of the message.
- Parameters:
number_of_bytes – Number of bytes in the array.
byte_array – Pointer to the byte array.
- Returns:
true if the address is correct, false otherwise.
-
static void instruction_evaluation(size_t number_of_bytes, const uint8_t byte_array[])
Evaluate the instruction of the message.
- Parameters:
number_of_bytes – Number of bytes in the array.
byte_array – Pointer to the byte array.
-
static bool reset_message_check(size_t number_of_bytes, const uint8_t *const byte_array)
Check for reset message - When reset message is found, stop the motor and disable all functions.
- Returns:
Number of bytes if valid reset message bit-pattern is found. Otherwise -1 is returned.
-
static size_t detect_dcc_packet()
Verify DCC message.
Procedure:
Check for valid preamble using bitmasks
Return number of bytes if valid bit-pattern is found. Otherwise -1 is returned.
- Returns:
Number of bytes if valid bit-pattern is found. Otherwise return -1.
-
static void bits_to_dcc_packet_data(dcc_packet_t *packet)
Convert sequence of bits to byte array.
For each byte in the byte array, shift the input bit buffer by 9 bits and save the result in the byte array. Bytes are ordered as follows: Start of transmission -> byte_n(address byte) -> … -> byte_0(error detection byte) -> end of transmission
- Parameters:
packet – Pointer to dcc packet struct which contains data and length of packet.
-
static void evaluate_packet()
Main message evaluation function.
Procedure:
Check if input buffer contains a valid DCC message
Split data into array of bytes
Check for errors
Check for matching address
Check for reset message and set flag when reset message is received
-
static void track_signal_rise()
Track signal rising edge interrupt callback function.
Saves timer counter value using get_absolute_time(), then disables rising edge IRQ and enables falling edge IRQ.
-
static void track_signal_fall()
Track signal falling edge interrupt callback function.
Saves timer counter value using get_absolute_time(), then calculates time difference between rising and falling edge. When the time difference is greater than 87us, this bit is interpreted as a logical 0. Otherwise, it is interpreted as a logical 1. Refers to NMRA S-9.1 and RCN-210 standards Puts the latest bit value into 64 bit unsigned integer “buffer”, then checks if the buffer contains a valid DCC packet. When a valid packet is found, it is written into the ring buffer, and the write index gets incremented.
-
static void init_outputs()
Output initialization function.
Initializes all allowed GPIO outputs as outputs
Sets PWM functionality for GPIOs configured as PWM pins CV_112 - CV_115 (see
CV.hfor reference)
PWM frequency and clock divider are configured according to CV_116 - CV_171 (see
CV.h for reference)
-
static void cv_setup_check()
Checks various CV values in order to detect factory condition or pending calibrations.
Procedure:
Check for flash memory factory condition when found write default values specified in CV.h to flash memory
Check for existing ADC offset setup when not found write 7 to read-only CV_7 in order to trigger a ADC offset measurement/calibration
Check for base PWM configuration/calibration, run calibration when none is found
-
static void init_motor_pwm(uint8_t gpio)
Function initializes motor PWM for pin specified in CMakeLists.txt.
- Parameters:
gpio – GPIO pin
-
static void init_adc()
Initializes ADC.
Function initializes ADC, corresponding GPIO pins specified in CMakeLists.txt, also configures ADC FIFO.
Variables
-
const uint32_t clr_bit_arr[6] = {0b11111111111111111111111111100000, 0b11111111111111111111111000011111, 0b11111111111111111110000111111111, 0b11111111111000000001111111111111, 0b11100000000111111111111111111111, 0b00011111111111111111111111111111,}
Bitmasks used for clearing bits corresponding to function blocks.
[0] = F0-F4 [1] = F5-F8 [2] = F9-F12 [3] = F13-F20 [4] = F21-F28 [5] = F29-F31
-
struct dcc_packet_t
- #include <core0.h>
- Param data:
An array of bytes representing the DCC packet data.
- Param length:
The length of the DCC packet data.
-
struct dcc_ring_buffer_t
Public Members
-
dcc_packet_t packets[RING_BUFFER_PACKETS]
Array of DCC packets stored in the ring buffer.
-
size_t wr_idx
Index for the next write operation in the ring buffer.
-
size_t rd_idx
Index for the next read operation in the ring buffer.
-
dcc_packet_t packets[RING_BUFFER_PACKETS]
core1.h
Used exclusively by core1
Defines
-
BASE_PWM_ARR_LEN
The length of the base PWM array.
This macro defines the length of the base PWM ring buffer array.
Typedefs
-
typedef struct startup_parameters_t startup_parameters_t
Structure for startup controller parameters and variables.
This structure defines the parameters and variables used in the startup controller, including the latest PWM level, base pwm level ring buffer array and the respective ring buffer index.
-
typedef struct pid_parameters_t pid_parameters_t
Structure for PID controller parameters and variables.
This structure defines the parameters and variables used in the PID controller, including gains, error tracking, integrator limits, …
-
typedef struct controller_parameter_t controller_parameter_t
Structure for various controller parameters.
This structure defines the highest level general controller parameters, including General settings (setpoint, mode), startup controller settings struct, PID controller struct, and measurement data.
Enums
Functions
-
float measure(uint8_t total_iterations, uint8_t measurement_delay_us, uint8_t l_side_arr_cutoff, uint8_t r_side_arr_cutoff, direction_t direction)
Measures Back-EMF voltage (proportional to the rotational speed of the motor) on GPIO 28 and GPIO 29 respectively (depending on direction).
This function performs multiple ADC measurements, sorts the results using insertion sort, and calculates the average of the middle values, discarding a specified number of the lowest and highest values.
- Parameters:
total_iterations – The total number of ADC measurements to perform.
measurement_delay_us – The delay in microseconds between the switch-off of the motor PWM and the measurement.
l_side_arr_cutoff – The number of lowest ADC values to discard.
r_side_arr_cutoff – The number of highest ADC values to discard.
direction – The direction of the motor.
- Returns:
The average of the middle ADC values after discarding the specified number of lowest and highest values.
-
uint8_t get_speed_step_table_index_of_speed_step(uint8_t speed_step)
Get the speed step table index based on the speed step value.
- Parameters:
speed_step – The speed step value.
- Returns:
The index in the speed step table.
-
void speed_helper(controller_parameter_t *ctrl_par)
Repeating timer callback function called every x milliseconds to implement a time delay in acceleration or deceleration.
x is CV_175
- Parameters:
ctrl_par – Pointer to the controller parameter structure.
-
void adjust_pwm_level(uint16_t level)
Function to adjust the PWM level/duty cycle.
- Parameters:
level – The new PWM level.
-
float get_kp(const controller_parameter_t *ctrl_par)
Function to calculate and return the proportional gain based on the speed step.
This is done in order to make kp dependent on the speed step (gain scheduling).
- Parameters:
ctrl_par – Pointer to the controller parameter structure.
- Returns:
The proportional gain.
-
uint16_t get_initial_level(controller_parameter_t *ctrl_par)
Calculate and return the initial PWM level.
Calculation is done by taking the last 16 initial levels (when available) and calculating the average. The average is the multiplied by a factor of 2/3.
- Parameters:
ctrl_par – Pointer to the controller parameter structure.
- Returns:
The initial PWM level.
-
void controller_startup_mode(controller_parameter_t *ctrl_par)
Controller function for startup mode.
This is called inside the controller_general() function when the controller mode is set to STARTUP_MODE.
- Parameters:
ctrl_par – Pointer to the controller parameter structure.
-
void controller_pid_mode(controller_parameter_t *const ctrl_par)
Controller function for PID mode.
This is called inside the controller_general() function when the controller mode is set to PID_MODE.
- Parameters:
ctrl_par – Pointer to the controller parameter structure.
-
void controller_general(controller_parameter_t *ctrl_par)
General controller function called every x milliseconds.
- Parameters:
ctrl_par – Pointer to the controller parameter structure.
-
void init_controller(controller_parameter_t *ctrl_par)
Initialize controller variables, measurement parameters, and speed table.
- Parameters:
ctrl_par – Pointer to the controller parameter structure.
-
struct startup_parameters_t
-
struct pid_parameters_t
Public Members
-
float k_i
Integral gain
-
float k_d
Derivative gain
-
float t
Sampling time
-
float tau
Low-pass-filter time constant
-
float ci_0
(k_i*t)/2
-
float cd_0
(k_d*2)/(2*tau+t)
-
float cd_1
(2*tau-t)/(2*tau+t)
-
float int_lim_max
max limit for integrator
-
float int_lim_min
min limit for integrator
-
float max_output
max possible pwm output value
-
float k_p
Proportional gain
-
float e
Error
-
float e_prev
Previous Error
-
float i_prev
Previous Integral Value
-
float d_prev
Previous Derivative Value
-
float k_p_x_1_shift
Effectively defines where x1 is shifted from 0% of max setpoint to 100% of max setpoint
-
float k_p_x_1
x1
-
float k_p_x_2
x2
-
float k_p_y_0
y0 = KP @ x0
-
float k_p_y_1
y1 = KP @ x1
-
float k_p_y_2
y2 = KP @ x2
-
float k_p_m_1
slope from (x0, y0) to (x1, y1)
-
float k_p_m_2
slope from (x1, y1) to (x2, y2)
-
float k_i
-
struct controller_parameter_t
Public Members
-
controller_mode_t mode
Current controller mode
-
float feed_fwd
Current feed forward value set by startup controller
-
uint32_t setpoint
Current setpoint
-
uint16_t speed_table[127]
Array with setpoint values corresponding to every speed step
-
startup_parameters_t startup
Struct for startup controller specific variables
-
pid_parameters_t pid
Struct for PID controller specific variables
-
float measurement
Latest measurement value
-
float measurement_prev
Previous measurement value
-
float measurement_corrected
Corrected measurement value (measurement - adc_offset = measurement_corrected)
-
float adc_offset
ADC offset value
-
uint8_t msr_delay_in_us
Delay before V_EMF is measured
-
uint8_t msr_total_iterations
Amount of samples
-
uint8_t l_side_arr_cutoff
Discarded outlier samples (left side)
-
uint8_t r_side_arr_cutoff
Discarded outlier samples (right side)
-
controller_mode_t mode
CV.h
Defines
-
CV_ARRAY_SIZE
Variables
-
uint8_t CV_ARRAY_DEFAULT[CV_ARRAY_SIZE]