Skip to content

Commit

Permalink
make more variables const
Browse files Browse the repository at this point in the history
  • Loading branch information
@r3drock
r3drock committed Dec 31, 2023
1 parent 5ae65b3 commit 02e1fdf
Show file tree
Hide file tree
Showing 3 changed files with 71 additions and 74 deletions.
91 changes: 44 additions & 47 deletions core0.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,27 +13,27 @@ absolute_time_t falling_edge_time,rising_edge_time;


// Function returns average of values deviating less than twice the standard deviation from original average
float two_std_dev(const float arr[], uint32_t length){
float two_std_dev(const float arr[], const uint32_t length){
// Calculate arithmetic average
float sum = 0;
for (uint32_t i = 0; i < length; ++i) {
sum += arr[i];
}
float x_avg = sum / (float)length;
const float x_avg = sum / (float)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);
}
float variance = sum / (float)(length-1);
float std_dev = sqrtf(variance);
const float variance = sum / (float)(length-1);
const float std_dev = sqrtf(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) {
float diff = fabsf(arr[i]-x_avg);
const float diff = fabsf(arr[i]-x_avg);
if ( diff < std_dev*2.0f ){
sum += arr[i];
counter++;
Expand Down Expand Up @@ -74,10 +74,10 @@ void adc_offset_adjustment(uint32_t n){
adc_fifo_drain();

// Find overall average discarding outliers in measurement
float offset_avg_fwd = two_std_dev(offsets_fwd, n);
float offset_avg_rev = two_std_dev(offsets_rev, n);
float overall_avg = (offset_avg_fwd+offset_avg_rev)/2;
uint8_t offset = (uint8_t)roundf(overall_avg);
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);

LOG(1, "new adc offset CV[171] (%f): (uint8_t)%d\n", overall_avg, offset);

Expand All @@ -93,7 +93,7 @@ void adc_offset_adjustment(uint32_t n){
// Confirmation after writing or verifying a CV
// Works by creating a spike in current consumption, this is detected by the command station
void acknowledge() {
uint16_t max_lvl = _125M/(CV_ARRAY_FLASH[8]*100+10000);
const uint16_t max_lvl = _125M/(CV_ARRAY_FLASH[8]*100+10000);
pwm_set_gpio_level(MOTOR_FWD_PIN, max_lvl);
busy_wait_ms(3);
pwm_set_gpio_level(MOTOR_FWD_PIN, 0);
Expand All @@ -104,23 +104,23 @@ void acknowledge() {


// When bit matches sent byte -> acknowledge()
void verify_cv_bit(uint16_t cv_address,bool bit_val, uint8_t bit_pos) {
uint8_t mask = 0b00000001;
bool res = ( (CV_ARRAY_FLASH[cv_address] >> bit_pos) &mask ) == bit_val;
void verify_cv_bit(const uint16_t cv_address,bool bit_val, uint8_t bit_pos) {
const uint8_t mask = 0b00000001;
const bool res = ( (CV_ARRAY_FLASH[cv_address] >> bit_pos) &mask ) == bit_val;
if (res) {
acknowledge();
}
}


// When byte matches sent byte -> acknowledge()
void verify_cv_byte(uint16_t cv_address, uint8_t cv_data){
void verify_cv_byte(const uint16_t cv_address, const uint8_t cv_data){
if (CV_ARRAY_FLASH[cv_address] == cv_data) acknowledge();
}


// CV writing function first erases necessary amount of blocks in flash and then rewrites to flash
void regular_cv_write(uint16_t cv_index, uint8_t cv_data){
void regular_cv_write(const uint16_t cv_index, const uint8_t cv_data){
uint8_t CV_ARRAY_TEMP[CV_ARRAY_SIZE];
memcpy(CV_ARRAY_TEMP, CV_ARRAY_FLASH, sizeof(CV_ARRAY_TEMP));
CV_ARRAY_TEMP[cv_index] = cv_data;
Expand All @@ -131,14 +131,13 @@ void regular_cv_write(uint16_t cv_index, uint8_t cv_data){


// 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(uint16_t cv_index, uint8_t cv_data){
void write_cv_handler(const uint16_t cv_index, const uint8_t cv_data){
switch (cv_index) {
case 0: //CV_1
// CV_1 => Value = 0 is not allowed
if (cv_data == 0 || cv_data > 127)
break;
else
regular_cv_write(cv_index,cv_data);
regular_cv_write(cv_index,cv_data);
break;
case 6: //CV_7
// Read only (CV_7 - Version no.)
Expand All @@ -162,14 +161,12 @@ void write_cv_handler(uint16_t cv_index, uint8_t cv_data){
case 16: //CV_17
if ( (cv_data < 192 || cv_data > 231) || (CV_ARRAY_FLASH[17] == 0 && cv_data == 192) )
break;
else
regular_cv_write(cv_index,cv_data);
regular_cv_write(cv_index,cv_data);
break;
case 17: //CV_18
if(CV_ARRAY_FLASH[16] == 192 && cv_data == 0)
break;
else
regular_cv_write(cv_index,cv_data);
regular_cv_write(cv_index,cv_data);
break;
// CV_31 & CV_32 Index high byte isn't implemented and shall not be set
case 30: //CV_31
Expand All @@ -183,14 +180,14 @@ void write_cv_handler(uint16_t cv_index, uint8_t cv_data){


// CV Programming function resets core1 and evaluates the type of programming instruction (e.g. write byte)
void program_mode(uint8_t number_of_bytes, const uint8_t * const byte_array) {
void program_mode(const uint8_t number_of_bytes, const uint8_t * const byte_array) {
//First check for valid programming command ("address" 112-127)
if (byte_array[number_of_bytes - 1]<128 && byte_array[number_of_bytes - 1]>111) {
uint8_t instruction_type_mask = 0b00001100;
uint8_t instruction_type = instruction_type_mask & byte_array[number_of_bytes - 1];
uint8_t cv_address_ms_bits_mask = 0b00000011;
uint16_t cv_address_ms_bits = cv_address_ms_bits_mask & byte_array[number_of_bytes - 1];
uint16_t cv_address = byte_array[number_of_bytes - 2] + (cv_address_ms_bits << 8);
const uint8_t instruction_type_mask = 0b00001100;
const uint8_t instruction_type = instruction_type_mask & byte_array[number_of_bytes - 1];
const uint8_t cv_address_ms_bits_mask = 0b00000011;
const uint16_t cv_address_ms_bits = cv_address_ms_bits_mask & byte_array[number_of_bytes - 1];
const uint16_t cv_address = byte_array[number_of_bytes - 2] + (cv_address_ms_bits << 8);

// Before accessing flash, timers and interrupts need to be disabled and core1 needs to be shut down.
if(pid_control_timer.pool)
Expand All @@ -200,27 +197,27 @@ void program_mode(uint8_t number_of_bytes, const uint8_t * const byte_array) {

multicore_reset_core1();

uint32_t saved_interrupts = save_and_disable_interrupts();
const uint32_t saved_interrupts = save_and_disable_interrupts();

// Verify CV bit instruction
if (instruction_type == 0b000001000) {
uint8_t bit_pos_mask = 0b00000111;
uint8_t bit_pos = bit_pos_mask&byte_array[number_of_bytes - 3];
uint8_t bit_val_mask = 0b00000001;
uint8_t bit_val_uint = (byte_array[number_of_bytes - 3]>>3) & bit_val_mask;
bool bit_val = bit_val_uint;
const uint8_t bit_pos_mask = 0b00000111;
const uint8_t bit_pos = bit_pos_mask&byte_array[number_of_bytes - 3];
const uint8_t bit_val_mask = 0b00000001;
const uint8_t bit_val_uint = (byte_array[number_of_bytes - 3]>>3) & bit_val_mask;
const bool bit_val = bit_val_uint;
verify_cv_bit(cv_address, bit_val, bit_pos);
}

// Verify CV byte instruction
else if (instruction_type == 0b000000100) {
uint8_t cv_data = byte_array[number_of_bytes - 3];
const uint8_t cv_data = byte_array[number_of_bytes - 3];
verify_cv_byte(cv_address, cv_data);
}

// Write CV byte instruction
else if (instruction_type == 0b000001100) {
uint8_t cv_data = byte_array[number_of_bytes - 3];
const uint8_t cv_data = byte_array[number_of_bytes - 3];
write_cv_handler(cv_address, cv_data);
}

Expand All @@ -232,9 +229,9 @@ void program_mode(uint8_t number_of_bytes, const uint8_t * const byte_array) {


// Set outputs according to configuration (PWM on/off, PWM duty cycle/level, ...)
void set_outputs(uint32_t functions_to_set_bitmask) {
void set_outputs(const uint32_t functions_to_set_bitmask) {
// Get outputs with pwm enabled and preset outputs_to_set_PWM variable with resulting GPIO Bitmask
uint32_t PWM_enabled_outputs = get_32bit_CV(111);
const uint32_t PWM_enabled_outputs = get_32bit_CV(111);
uint32_t outputs_to_set_PWM = PWM_enabled_outputs;

// Get enabled output configuration corresponding to set functions and direction
Expand Down Expand Up @@ -340,8 +337,8 @@ bool address_evaluation(const uint8_t number_of_bytes,const uint8_t * const byte
{
// Short Address Package
// start of transmission -> address_byte_0 -> ... -> end of transmission
read_address = (byte_array[number_of_bytes - 1]);
return (CV_ARRAY_FLASH[0] == read_address);
read_address = byte_array[number_of_bytes - 1];
return CV_ARRAY_FLASH[0] == read_address;
}
}

Expand Down Expand Up @@ -408,14 +405,14 @@ void instruction_evaluation(const uint8_t number_of_bytes,const uint8_t * const


// Check for reset package - When reset package is found stop the motor and disable all functions
bool reset_package_check(uint8_t number_of_bytes,const uint8_t * const byte_array){
bool reset_package_check(const uint8_t number_of_bytes,const uint8_t * const byte_array){
if (byte_array[number_of_bytes-1] == 0b00000000 && byte_array[number_of_bytes-2] == 0b00000000){
uint32_t speed_step = 1;
const uint32_t speed_step = 1;
multicore_fifo_push_blocking(speed_step); // Emergency Stop
update_active_functions(0,0,0); // Disables all functions
return true;
}
else return false;
return false;
}


Expand Down Expand Up @@ -476,7 +473,7 @@ void evaluation(){


// DCC-Signal (GPIO 21) rising edge interrupt
void track_signal_rise(unsigned int gpio, long unsigned int events) {
void track_signal_rise(const unsigned int gpio, const long unsigned int events) {
// Save current timer value
rising_edge_time = get_absolute_time();
// Disable rising edge interrupt and enable falling edge interrupt
Expand All @@ -486,7 +483,7 @@ void track_signal_rise(unsigned int gpio, long unsigned int events) {


// DCC-Signal (GPIO 21) falling edge interrupt
void track_signal_fall(unsigned int gpio, long unsigned int events) {
void track_signal_fall( const unsigned int gpio, const long unsigned int events) {
// Save current timer value
falling_edge_time = get_absolute_time();
// Time difference is equal to the time the signal was in a logical high state
Expand Down Expand Up @@ -531,7 +528,7 @@ void init_outputs(){


// Measures "base" pwm duty cycle needed to overcome friction
uint16_t measure_base_pwm(bool direction, uint8_t iterations){
uint16_t measure_base_pwm(const bool direction, const uint8_t iterations){
uint gpio;
if (direction) gpio = MOTOR_FWD_PIN;
else gpio = MOTOR_REV_PIN;
Expand Down Expand Up @@ -633,7 +630,7 @@ bool get_direction(bool *direction_ptr){


// Motor PWM initialization
void init_motor_pwm(uint8_t gpio) {
void init_motor_pwm(const uint8_t gpio) {
const uint16_t wrap_counter = (_125M/(CV_ARRAY_FLASH[8]*100+10000)) - 1;
const uint32_t slice_num = pwm_gpio_to_slice_num(gpio);
pwm_set_clkdiv_int_frac(slice_num,CV_ARRAY_FLASH[173],0);
Expand Down
46 changes: 23 additions & 23 deletions core1.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,7 @@ struct repeating_timer pid_control_timer,speed_helper_timer;
void update_speed_dir(pid_params *pid, uint32_t speed_step_target){
pid->speed_step_target = speed_step_target;
// When CV_29 Bit 0 is set direction is reversed
bool reverse_dir = CV_ARRAY_FLASH[28] & 0b00000001;
const bool reverse_dir = CV_ARRAY_FLASH[28] & 0b00000001;
// Forward
if(speed_step_target > 127) {
pid->direction = true^reverse_dir;
Expand All @@ -34,12 +34,12 @@ void update_speed_dir(pid_params *pid, uint32_t speed_step_target){

// This function gets called every x milliseconds where x is CV_175.
// The purpose of this function is to implement a time delay in acceleration or deceleration
bool speed_helper(struct repeating_timer *t) {
bool speed_helper(struct repeating_timer *const t) {
// pid->setpoint only gets adjusted when the speed_helper_counter is equal to the accel_rate/decel_rate
// -> Time for 1 Speed Step := (speed_helper timer delay)*(accel_rate) or CV_175*CV_3 or CV_175*CV_4
pid_params *pid = (pid_params *)(t->user_data);
uint8_t accel_rate = CV_ARRAY_FLASH[2];
uint8_t decel_rate = CV_ARRAY_FLASH[3];
const uint8_t accel_rate = CV_ARRAY_FLASH[2];
const uint8_t decel_rate = CV_ARRAY_FLASH[3];
static bool prev_dir;
static uint8_t speed_helper_counter;
static uint8_t speed_table_index;
Expand Down Expand Up @@ -89,7 +89,7 @@ bool speed_helper(struct repeating_timer *t) {


// Helper function to adjust pwm level/duty cycle.
void adjust_pwm_level(uint16_t level, pid_params *pid) {
void adjust_pwm_level(const uint16_t level, const pid_params *pid) {
// Forward
if (pid->direction) {
pwm_set_gpio_level(MOTOR_REV_PIN, 0);
Expand All @@ -104,14 +104,14 @@ void adjust_pwm_level(uint16_t level, pid_params *pid) {


// Update feed-forward slope/gradient for both directions
void update_m(pid_params *pid){
void update_m(pid_params *const pid){
pid->m_fwd = (pid->y_2_fwd-pid->y_1_fwd)/(pid->x_2_fwd-pid->x_1_fwd);
pid->m_rev = (pid->y_2_rev-pid->y_1_rev)/(pid->x_2_rev-pid->x_1_rev);
}


// Update feedforward function value y_2 in both directions
void update_y(pid_params *pid,float i){
void update_y(pid_params *const pid, const float i){
// Forward
if(pid->direction) {
pid->y_2_fwd += i*pid->k_ff;
Expand All @@ -132,7 +132,7 @@ void update_y(pid_params *pid,float i){


// Returns feedforward value corresponding to current setpoint
float get_ff_val(pid_params *pid){
float get_ff_val(const pid_params *const pid){
float val;
// Forward
if(pid->direction){
Expand All @@ -147,7 +147,7 @@ float get_ff_val(pid_params *pid){


// Returns proportional gain value corresponding to current setpoint
float get_kp(pid_params *pid){
float get_kp(const pid_params *const pid){
float sp = (float)pid->setpoint;
if (sp < pid->k_p_x_1){
return pid->k_p_m_1 * sp + pid->k_p_y_0;
Expand All @@ -171,7 +171,7 @@ bool pid_control(struct repeating_timer *t){
}

// Get feed-forward and proportional gain values
float feed_fwd = get_ff_val(pid);
const float feed_fwd = get_ff_val(pid);
pid->k_p = get_kp(pid);

// Measure BEMF voltage and compute error
Expand All @@ -180,16 +180,16 @@ bool pid_control(struct repeating_timer *t){
pid->l_side_arr_cutoff,
pid->r_side_arr_cutoff,
pid->direction);
float e = (float)pid->setpoint - (pid->measurement - pid->adc_offset);
const float e = (float)pid->setpoint - (pid->measurement - pid->adc_offset);

// Proportional part, integral part and derivative part (including digital low-pass-filter with time constant tau)
float p = pid->k_p * e;
const float p = pid->k_p * e;
float i = pid->ci_0*(e + pid->e_prev) + pid->i_prev;
float d = pid->cd_0*(pid->measurement-pid->measurement_prev)+pid->cd_1*pid->d_prev;
const float d = pid->cd_0*(pid->measurement-pid->measurement_prev)+pid->cd_1*pid->d_prev;

// Check for limits on the integral part
if (i > pid->int_lim_max)i = pid->int_lim_max;
else if (i < pid->int_lim_min)i = pid->int_lim_min;
if (i > pid->int_lim_max) i = pid->int_lim_max;
else if (i < pid->int_lim_min) i = pid->int_lim_min;

// Sum feed forward + all controller terms (p+i+d). Limit Output from 0% to 100% duty cycle
float output = feed_fwd+p+i+d;
Expand Down Expand Up @@ -226,7 +226,7 @@ bool pid_control(struct repeating_timer *t){


// PID controller and measurement parameter, and speed_table initialization
void init_pid(pid_params *pid){
void init_pid(pid_params *const pid){
// Measurement variables initialization
pid->adc_offset = (float)CV_ARRAY_FLASH[171];
pid->msr_total_iterations = CV_ARRAY_FLASH[60];
Expand All @@ -236,15 +236,15 @@ void init_pid(pid_params *pid){

// Speed table initialization
// Calculate speed setpoint table according to V_min, V_max and V_mid CVs
double v_min = (double)CV_ARRAY_FLASH[1];
double v_mid = (double)CV_ARRAY_FLASH[5]*16;
double v_max = (double)CV_ARRAY_FLASH[4]*16;
const double v_min = (double)CV_ARRAY_FLASH[1];
const double v_mid = (double)CV_ARRAY_FLASH[5]*16;
const double v_max = (double)CV_ARRAY_FLASH[4]*16;

LOG(1, "speedtable min(%f) mid(%f) max(%f)\n", v_min, v_mid, v_max);

double delta_x = 63;
double m_1 = (v_mid-v_min)/delta_x;
double m_2 = (v_max-v_mid)/delta_x;
const double delta_x = 63;
const double m_1 = (v_mid-v_min)/delta_x;
const double m_2 = (v_max-v_mid)/delta_x;
pid->speed_table[0] = 0;
for (uint8_t i = 1; i < 64; ++i) {
pid->speed_table[i] = (uint16_t)lround(m_1*(i-1)+v_min);
Expand Down Expand Up @@ -319,7 +319,7 @@ void core1_entry() {
// Note: Pointer will be cast into uint32_t and then back into bool * on core0 (Both 32-Bit Datatype).
// This is done only to accommodate the correct data type associated with multicore_fifo_push_blocking().
multicore_fifo_push_blocking((uint32_t) &(pid->direction));
uint64_t packet_timeout_threshold_in_us = CV_ARRAY_FLASH[10]*1000000;
const uint64_t packet_timeout_threshold_in_us = CV_ARRAY_FLASH[10]*1000000;
absolute_time_t time_last_update = get_absolute_time();
// printf("core1 done\n");
while (1){
Expand Down
Loading

0 comments on commit 02e1fdf

Please sign in to comment.