diff --git a/core0.c b/core0.c index 845bf56..37bb841 100644 --- a/core0.c +++ b/core0.c @@ -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++; @@ -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); @@ -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); @@ -104,9 +104,9 @@ 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(); } @@ -114,13 +114,13 @@ void verify_cv_bit(uint16_t cv_address,bool bit_val, uint8_t bit_pos) { // 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; @@ -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.) @@ -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 @@ -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) @@ -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); } @@ -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 @@ -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; } } @@ -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; } @@ -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 @@ -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 @@ -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; @@ -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); diff --git a/core1.c b/core1.c index 9a75822..74d9527 100644 --- a/core1.c +++ b/core1.c @@ -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; @@ -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; @@ -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); @@ -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; @@ -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){ @@ -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; @@ -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 @@ -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; @@ -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]; @@ -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); @@ -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){ diff --git a/core1.h b/core1.h index 9d473df..ea69aa2 100644 --- a/core1.h +++ b/core1.h @@ -62,10 +62,10 @@ typedef struct pid_params{ void update_speed_dir(pid_params *pid, uint32_t speed_step_target); bool speed_helper(struct repeating_timer *t); -void adjust_pwm_level(uint16_t level, pid_params *pid); +void adjust_pwm_level(uint16_t level, const pid_params *pid); void update_m(pid_params *pid); void update_y(pid_params *pid,float i); -float get_ff_val(pid_params *pid); -float get_kp(pid_params *pid); +float get_ff_val(const pid_params *pid); +float get_kp(const pid_params *pid); bool pid_control(struct repeating_timer *t); -void init_pid(pid_params *pid); \ No newline at end of file +void init_pid( pid_params * pid); \ No newline at end of file