From 8fb10e212ea94d4385b94bb6b821e2a0fd7380a0 Mon Sep 17 00:00:00 2001 From: gk6394s Date: Tue, 2 Jan 2024 12:56:25 +0100 Subject: [PATCH] Simplify get_speed_step_table_index_of_speed_step(); Removed unnecessary parameter from adjust_pwm_level function; Removed some redundant else statements and redundant "== true" from some expressions; Fixed reset_message_check() function; Added const to some function parameters and local variables; --- core0.c | 29 +++++++++++---------- core1.c | 80 ++++++++++++++++++++++++++------------------------------- core1.h | 2 +- 3 files changed, 53 insertions(+), 58 deletions(-) diff --git a/core0.c b/core0.c index a6dd6c6..792f328 100644 --- a/core0.c +++ b/core0.c @@ -45,7 +45,7 @@ float two_std_dev(const float arr[], const uint32_t length) { // Measures constant adc offset and programs the offset into flash -void adc_offset_adjustment(uint32_t n) { +void adc_offset_adjustment(const uint32_t n) { LOG(1, "adc_offset_adjustment"); float offsets_fwd[n]; @@ -105,7 +105,7 @@ void acknowledge() { // When bit matches sent byte -> acknowledge() -void verify_cv_bit(const uint16_t cv_address, bool bit_val, uint8_t bit_pos) { +void verify_cv_bit(const uint16_t cv_address, const bool bit_val, const uint8_t bit_pos) { const uint8_t mask = 0b00000001; const bool res = ((CV_ARRAY_FLASH[cv_address] >> bit_pos) & mask) == bit_val; if (res) { @@ -250,7 +250,7 @@ void set_outputs(const uint32_t functions_to_set_bitmask) { uint32_t temp_mask = 1; for (uint8_t i = 0; i < 32; i++) { if (temp_mask & functions_to_set_bitmask) { - bool direction = get_direction_of_speed_step(speed_step_target); + const bool direction = get_direction_of_speed_step(speed_step_target); outputs_to_set |= get_32bit_CV(i * 8 + 260 - 4 * direction); } temp_mask <<= 1; @@ -278,7 +278,7 @@ void set_outputs(const uint32_t functions_to_set_bitmask) { // Update functions F0 - F31 when function command is received or in case of direction change -void update_active_functions(uint32_t new_function_bitmask, uint8_t clr_bit_ind, bool direction_change) { +void update_active_functions(uint32_t new_function_bitmask, const uint8_t clr_bit_ind, const bool direction_change) { static uint32_t active_functions; static uint32_t prev_active_func; @@ -294,7 +294,7 @@ void update_active_functions(uint32_t new_function_bitmask, uint8_t clr_bit_ind, new_function_bitmask &= ~(clr_bit_arr[clr_bit_ind]); // Clear every bit except bits corresponding to group of functions active_functions &= clr_bit_arr[clr_bit_ind]; // Clear bits corresponding to group of functions active_functions |= new_function_bitmask; // Write bits corresponding to group of functions - uint32_t changes = active_functions ^ prev_active_func; // Checks for changes from previous state + const uint32_t changes = active_functions ^ prev_active_func; // Checks for changes from previous state if (changes) { set_outputs(active_functions); } @@ -363,7 +363,6 @@ bool address_evaluation(const uint8_t number_of_bytes, const uint8_t *const byte void instruction_evaluation(const uint8_t number_of_bytes, const uint8_t *const byte_array) { // start of transmission -> ... -> command_byte_n -> ... -> command_byte_0 -> ... -> end of transmission // The position of command bytes depend on whether the address is long or not - uint8_t command_byte_n; uint8_t command_byte_start_index; if (is_long_address(number_of_bytes, byte_array)) { command_byte_start_index = number_of_bytes - 3; @@ -371,14 +370,14 @@ void instruction_evaluation(const uint8_t number_of_bytes, const uint8_t *const else { command_byte_start_index = number_of_bytes - 2; } - command_byte_n = byte_array[command_byte_start_index]; + const uint8_t command_byte_n = byte_array[command_byte_start_index]; if (command_byte_n == 0b00111111) { // 0011-1111 (128 Speed Step Control) - 2 Byte length speed_step_target_prev = speed_step_target; speed_step_target = byte_array[command_byte_start_index - 1]; // Check for direction change - bool direction_changed = get_direction_of_speed_step(speed_step_target) != + const bool direction_changed = get_direction_of_speed_step(speed_step_target) != get_direction_of_speed_step(speed_step_target_prev); // In case of a direction change, functions need to be updated because functions depend on direction if (direction_changed) { @@ -423,12 +422,16 @@ void instruction_evaluation(const uint8_t number_of_bytes, const uint8_t *const // Check for reset message - When reset message is found, stop the motor and disable all functions. bool reset_message_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) { - const uint32_t speed_step = 1; - multicore_fifo_push_blocking(speed_step); // Emergency Stop + const bool direction = get_direction_of_speed_step(speed_step_target); + // Set previous speed step target to current speed step target + speed_step_target_prev = speed_step_target; + // Set speed step target to emergency stop speed step ( 1 for Reverse direction / 129 for forward direction) + // for details see get_direction_of_speed_step() doxygen comment in "shared.h" + speed_step_target = 1 + 128*direction; update_active_functions(0,0,0); // Disables all functions return true; } - else return false; + return false; } @@ -447,7 +450,7 @@ int8_t verify_dcc_message() { //start of transmission -> byte_n(address byte) -> ... -> byte_0(error detection byte) -> end of transmission -void bits_to_byte_array(int8_t number_of_bytes, uint8_t byte_array[]) { +void bits_to_byte_array(const int8_t number_of_bytes, uint8_t byte_array[]) { for (uint8_t i = 0; i < number_of_bytes; i++) { byte_array[i] = input_bit_buffer >> (i * 9 + 1); } @@ -565,7 +568,7 @@ uint16_t measure_base_pwm(const bool direction, const uint8_t iterations) { return 0; } } while ((measurement - (float) CV_ARRAY_FLASH[171]) < 5.0f); - LOG(3, "level_arr[%d] = %f\n", i, level); + LOG(3, "level_arr[%d] = %d\n", i, level); level_arr[i] = (float) level; busy_wait_ms(100); } diff --git a/core1.c b/core1.c index 2f9ea34..276d4b6 100644 --- a/core1.c +++ b/core1.c @@ -11,24 +11,14 @@ struct repeating_timer pid_control_timer, speed_helper_timer; // Get speed_step_table_index depending on speed_step uint8_t get_speed_step_table_index_of_speed_step(uint8_t speed_step) { - if (speed_step > 127) { - // Forward - if (speed_step == 128) { - return 0; - } - else { - return (speed_step - 129); - } - } - else { - // Reverse - if (speed_step == 0) { - return 0; - } - else { - return (speed_step - 1); - } + // Clear direction information in bit7 + speed_step = speed_step & 0b01111111; + // Check for Stop or Emergency Stop speed step + if (speed_step == 0 || speed_step == 1) { + return 0; } + // In any other case i.e. speed steps 1 to 126: + return speed_step-1; } @@ -37,16 +27,16 @@ uint8_t get_speed_step_table_index_of_speed_step(uint8_t speed_step) { 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); + pid_params *const pid = (pid_params *) (t->user_data); const uint8_t accel_rate = CV_ARRAY_FLASH[2]; const uint8_t decel_rate = CV_ARRAY_FLASH[3]; static uint8_t speed_helper_counter; static uint8_t speed_table_index; // Get speed_step_table_end_index corresponding to current speed_step_target - uint8_t speed_step_table_end_index = get_speed_step_table_index_of_speed_step(speed_step_target); - bool direction_changed = get_direction_of_speed_step(speed_step_target) != - get_direction_of_speed_step(speed_step_target_prev); + const uint8_t speed_step_table_end_index = get_speed_step_table_index_of_speed_step(speed_step_target); + const bool direction_changed = get_direction_of_speed_step(speed_step_target) != + get_direction_of_speed_step(speed_step_target_prev); if (pid->speed_step_target == 129 || pid->speed_step_target == 1) { // Emergency Stop @@ -56,32 +46,28 @@ bool speed_helper(struct repeating_timer *const t) { } else if (speed_step_table_end_index > speed_table_index && speed_helper_counter == accel_rate) { // Acceleration - if (!accel_rate) speed_table_index = speed_step_table_end_index; // directly accelerate to end target speed + if (!accel_rate) speed_table_index = speed_step_table_end_index; // directly accelerate to end target speed else speed_table_index++; pid->setpoint = pid->speed_table[speed_table_index]; speed_helper_counter = 0; } - else if (speed_step_table_end_index < speed_table_index && speed_helper_counter == decel_rate) { // Deceleration - if (!decel_rate) speed_table_index = speed_step_table_end_index; // directly decelerate to end target speed + if (!decel_rate) speed_table_index = speed_step_table_end_index; // directly decelerate to end target speed else speed_table_index--; pid->setpoint = pid->speed_table[speed_table_index]; speed_helper_counter = 0; } - else if (direction_changed) { // Change of direction while decelerating -> jump to most recent speed_step without delay (other direction) speed_table_index = speed_step_table_end_index; pid->setpoint = pid->speed_table[speed_table_index]; speed_helper_counter = 0; } - else if (speed_step_table_end_index != speed_table_index) { // Acceleration/Deceleration "scheduled" speed_helper_counter++; } - else { // Target reached -> Reset Counter speed_helper_counter = 0; @@ -93,8 +79,8 @@ bool speed_helper(struct repeating_timer *const t) { // Helper function to adjust pwm level/duty cycle. -void adjust_pwm_level(const uint16_t level, const pid_params *pid) { - if (get_direction_of_speed_step(speed_step_target) == true) { +void adjust_pwm_level(const uint16_t level) { + if (get_direction_of_speed_step(speed_step_target)) { // Forward pwm_set_gpio_level(MOTOR_REV_PIN, 0); pwm_set_gpio_level(MOTOR_FWD_PIN, level); @@ -116,7 +102,7 @@ void update_m(pid_params *const pid) { // Update feedforward function value y_2 in both directions void update_y(pid_params *const pid, const float i) { - if (get_direction_of_speed_step(speed_step_target) == true) { + if (get_direction_of_speed_step(speed_step_target)) { // Forward pid->y_2_fwd += i * pid->k_ff; // Limit y_2_fwd @@ -137,7 +123,7 @@ void update_y(pid_params *const pid, const float i) { // Returns feedforward value corresponding to current setpoint float get_ff_val(const pid_params *const pid) { float val; - if (get_direction_of_speed_step(speed_step_target) == true) { + if (get_direction_of_speed_step(speed_step_target)) { // Forward val = pid->m_fwd * ((float) pid->setpoint - pid->x_1_fwd) + pid->y_1_fwd; } @@ -151,24 +137,22 @@ float get_ff_val(const pid_params *const pid) { // Returns proportional gain value corresponding to current setpoint float get_kp(const pid_params *const pid) { - float sp = (float) pid->setpoint; + const float sp = (float) pid->setpoint; if (sp < pid->k_p_x_1) { return pid->k_p_m_1 * sp + pid->k_p_y_0; } - else { - return pid->k_p_m_2 * (sp - pid->k_p_x_1) + pid->k_p_y_1; - } + return pid->k_p_m_2 * (sp - pid->k_p_x_1) + pid->k_p_y_1; } // PID controller function gets called every x milliseconds where x is CV_49 aka sampling time (pid->t) -bool pid_control(struct repeating_timer *t) { +bool pid_control(struct repeating_timer *const t) { // Cast (void *) to (pid_params *) - pid_params *pid = (pid_params *) (t->user_data); + pid_params *const pid = (pid_params *) (t->user_data); // Acceleration from standstill or change in direction -> reset previous derivative, error and integral parts - bool direction_changed = get_direction_of_speed_step(speed_step_target) != - get_direction_of_speed_step(speed_step_target_prev); + const bool direction_changed = get_direction_of_speed_step(speed_step_target) != + get_direction_of_speed_step(speed_step_target_prev); if ((!pid->setpoint_prev && !pid->setpoint) || direction_changed) { pid->d_prev = 0.0f; pid->i_prev = 0.0f; @@ -195,13 +179,21 @@ bool pid_control(struct repeating_timer *t) { 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; - if (output < 0 || !pid->setpoint) output = 0.0f; - else if (output > pid->max_output) output = pid->max_output; + if (output < 0 || !pid->setpoint) { + output = 0.0f; + } + else if (output > pid->max_output) { + output = pid->max_output; + } if (LOGLEVEL >= 1) { static int counter = 0; @@ -213,7 +205,7 @@ bool pid_control(struct repeating_timer *t) { } // Set PWM Level and discard results in adc fifo - adjust_pwm_level((uint16_t) roundf(output), pid); + adjust_pwm_level((uint16_t) roundf(output)); adc_fifo_drain(); // Check for High or Low Integrator values -> Update x,y values of feedforward function diff --git a/core1.h b/core1.h index 557cd11..e0b3b1c 100644 --- a/core1.h +++ b/core1.h @@ -59,7 +59,7 @@ typedef struct pid_params{ uint8_t get_speed_step_table_index_of_speed_step(uint8_t speed_step); bool speed_helper(struct repeating_timer *t); -void adjust_pwm_level(uint16_t level, const pid_params *pid); +void adjust_pwm_level(uint16_t level); void update_m(pid_params *pid); void update_y(pid_params *pid,float i); float get_ff_val(const pid_params *pid);