make more variables const

This commit is contained in:
r3drock committed 2023-12-31 22:12:07 +01:00
1 parent 5ae65b301d
commit 02e1fdf156
3 files changed
+71 -74

No files matched your search

+44 -47
View File
@@ -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 // 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 // Calculate arithmetic average
float sum = 0; float sum = 0;
for (uint32_t i = 0; i < length; ++i) { for (uint32_t i = 0; i < length; ++i) {
sum += arr[i]; sum += arr[i];
} }
float x_avg = sum / (float)length; const float x_avg = sum / (float)length;
// Calculate sample variance and sampled standard deviation // Calculate sample variance and sampled standard deviation
sum = 0; sum = 0;
for (uint32_t i = 0; i < length; ++i) { for (uint32_t i = 0; i < length; ++i) {
sum += (arr[i]-x_avg)*(arr[i]-x_avg); sum += (arr[i]-x_avg)*(arr[i]-x_avg);
} }
float variance = sum / (float)(length-1); const float variance = sum / (float)(length-1);
float std_dev = sqrtf(variance); const float std_dev = sqrtf(variance);
// Sum every element that is within two times the sampled standard deviation and compute new average // Sum every element that is within two times the sampled standard deviation and compute new average
sum = 0; sum = 0;
uint32_t counter = 0; uint32_t counter = 0;
for (uint32_t i = 0; i < length; ++i) { 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 ){ if ( diff < std_dev*2.0f ){
sum += arr[i]; sum += arr[i];
counter++; counter++;
@@ -74,10 +74,10 @@ void adc_offset_adjustment(uint32_t n){
adc_fifo_drain(); adc_fifo_drain();
// Find overall average discarding outliers in measurement // Find overall average discarding outliers in measurement
float offset_avg_fwd = two_std_dev(offsets_fwd, n); const float offset_avg_fwd = two_std_dev(offsets_fwd, n);
float offset_avg_rev = two_std_dev(offsets_rev, n); const float offset_avg_rev = two_std_dev(offsets_rev, n);
float overall_avg = (offset_avg_fwd+offset_avg_rev)/2; const float overall_avg = (offset_avg_fwd+offset_avg_rev)/2;
uint8_t offset = (uint8_t)roundf(overall_avg); const uint8_t offset = (uint8_t)roundf(overall_avg);
LOG(1, "new adc offset CV[171] (%f): (uint8_t)%d\n", overall_avg, offset); 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 // Confirmation after writing or verifying a CV
// Works by creating a spike in current consumption, this is detected by the command station // Works by creating a spike in current consumption, this is detected by the command station
void acknowledge() { 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); pwm_set_gpio_level(MOTOR_FWD_PIN, max_lvl);
busy_wait_ms(3); busy_wait_ms(3);
pwm_set_gpio_level(MOTOR_FWD_PIN, 0); pwm_set_gpio_level(MOTOR_FWD_PIN, 0);
@@ -104,9 +104,9 @@ void acknowledge() {
// When bit matches sent byte -> acknowledge() // When bit matches sent byte -> acknowledge()
void verify_cv_bit(uint16_t cv_address,bool bit_val, uint8_t bit_pos) { void verify_cv_bit(const uint16_t cv_address,bool bit_val, uint8_t bit_pos) {
uint8_t mask = 0b00000001; const uint8_t mask = 0b00000001;
bool res = ( (CV_ARRAY_FLASH[cv_address] >> bit_pos) &mask ) == bit_val; const bool res = ( (CV_ARRAY_FLASH[cv_address] >> bit_pos) &mask ) == bit_val;
if (res) { if (res) {
acknowledge(); 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() // 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(); 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 // 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]; uint8_t CV_ARRAY_TEMP[CV_ARRAY_SIZE];
memcpy(CV_ARRAY_TEMP, CV_ARRAY_FLASH, sizeof(CV_ARRAY_TEMP)); memcpy(CV_ARRAY_TEMP, CV_ARRAY_FLASH, sizeof(CV_ARRAY_TEMP));
CV_ARRAY_TEMP[cv_index] = cv_data; 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 // 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) { switch (cv_index) {
case 0: //CV_1 case 0: //CV_1
// CV_1 => Value = 0 is not allowed // CV_1 => Value = 0 is not allowed
if (cv_data == 0 || cv_data > 127) if (cv_data == 0 || cv_data > 127)
break; break;
else regular_cv_write(cv_index,cv_data);
regular_cv_write(cv_index,cv_data);
break; break;
case 6: //CV_7 case 6: //CV_7
// Read only (CV_7 - Version no.) // 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 case 16: //CV_17
if ( (cv_data < 192 || cv_data > 231) || (CV_ARRAY_FLASH[17] == 0 && cv_data == 192) ) if ( (cv_data < 192 || cv_data > 231) || (CV_ARRAY_FLASH[17] == 0 && cv_data == 192) )
break; break;
else regular_cv_write(cv_index,cv_data);
regular_cv_write(cv_index,cv_data);
break; break;
case 17: //CV_18 case 17: //CV_18
if(CV_ARRAY_FLASH[16] == 192 && cv_data == 0) if(CV_ARRAY_FLASH[16] == 192 && cv_data == 0)
break; break;
else regular_cv_write(cv_index,cv_data);
regular_cv_write(cv_index,cv_data);
break; break;
// CV_31 & CV_32 Index high byte isn't implemented and shall not be set // CV_31 & CV_32 Index high byte isn't implemented and shall not be set
case 30: //CV_31 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) // 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) //First check for valid programming command ("address" 112-127)
if (byte_array[number_of_bytes - 1]<128 && byte_array[number_of_bytes - 1]>111) { if (byte_array[number_of_bytes - 1]<128 && byte_array[number_of_bytes - 1]>111) {
uint8_t instruction_type_mask = 0b00001100; const uint8_t instruction_type_mask = 0b00001100;
uint8_t instruction_type = instruction_type_mask & byte_array[number_of_bytes - 1]; const uint8_t instruction_type = instruction_type_mask & byte_array[number_of_bytes - 1];
uint8_t cv_address_ms_bits_mask = 0b00000011; const 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]; const 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 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. // Before accessing flash, timers and interrupts need to be disabled and core1 needs to be shut down.
if(pid_control_timer.pool) 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(); multicore_reset_core1();
uint32_t saved_interrupts = save_and_disable_interrupts(); const uint32_t saved_interrupts = save_and_disable_interrupts();
// Verify CV bit instruction // Verify CV bit instruction
if (instruction_type == 0b000001000) { if (instruction_type == 0b000001000) {
uint8_t bit_pos_mask = 0b00000111; const uint8_t bit_pos_mask = 0b00000111;
uint8_t bit_pos = bit_pos_mask&byte_array[number_of_bytes - 3]; const uint8_t bit_pos = bit_pos_mask&byte_array[number_of_bytes - 3];
uint8_t bit_val_mask = 0b00000001; const uint8_t bit_val_mask = 0b00000001;
uint8_t bit_val_uint = (byte_array[number_of_bytes - 3]>>3) & bit_val_mask; const uint8_t bit_val_uint = (byte_array[number_of_bytes - 3]>>3) & bit_val_mask;
bool bit_val = bit_val_uint; const bool bit_val = bit_val_uint;
verify_cv_bit(cv_address, bit_val, bit_pos); verify_cv_bit(cv_address, bit_val, bit_pos);
} }
// Verify CV byte instruction // Verify CV byte instruction
else if (instruction_type == 0b000000100) { 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); verify_cv_byte(cv_address, cv_data);
} }
// Write CV byte instruction // Write CV byte instruction
else if (instruction_type == 0b000001100) { 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); 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, ...) // 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 // 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; uint32_t outputs_to_set_PWM = PWM_enabled_outputs;
// Get enabled output configuration corresponding to set functions and direction // 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 // Short Address Package
// start of transmission -> address_byte_0 -> ... -> end of transmission // start of transmission -> address_byte_0 -> ... -> end of transmission
read_address = (byte_array[number_of_bytes - 1]); read_address = byte_array[number_of_bytes - 1];
return (CV_ARRAY_FLASH[0] == read_address); 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 // 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){ 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 multicore_fifo_push_blocking(speed_step); // Emergency Stop
update_active_functions(0,0,0); // Disables all functions update_active_functions(0,0,0); // Disables all functions
return true; return true;
} }
else return false; return false;
} }
@@ -476,7 +473,7 @@ void evaluation(){
// DCC-Signal (GPIO 21) rising edge interrupt // 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 // Save current timer value
rising_edge_time = get_absolute_time(); rising_edge_time = get_absolute_time();
// Disable rising edge interrupt and enable falling edge interrupt // 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 // 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 // Save current timer value
falling_edge_time = get_absolute_time(); falling_edge_time = get_absolute_time();
// Time difference is equal to the time the signal was in a logical high state // 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 // 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; uint gpio;
if (direction) gpio = MOTOR_FWD_PIN; if (direction) gpio = MOTOR_FWD_PIN;
else gpio = MOTOR_REV_PIN; else gpio = MOTOR_REV_PIN;
@@ -633,7 +630,7 @@ bool get_direction(bool *direction_ptr){
// Motor PWM initialization // 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 uint16_t wrap_counter = (_125M/(CV_ARRAY_FLASH[8]*100+10000)) - 1;
const uint32_t slice_num = pwm_gpio_to_slice_num(gpio); const uint32_t slice_num = pwm_gpio_to_slice_num(gpio);
pwm_set_clkdiv_int_frac(slice_num,CV_ARRAY_FLASH[173],0); pwm_set_clkdiv_int_frac(slice_num,CV_ARRAY_FLASH[173],0);
+23 -23
View File
@@ -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){ void update_speed_dir(pid_params *pid, uint32_t speed_step_target){
pid->speed_step_target = speed_step_target; pid->speed_step_target = speed_step_target;
// When CV_29 Bit 0 is set direction is reversed // 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 // Forward
if(speed_step_target > 127) { if(speed_step_target > 127) {
pid->direction = true^reverse_dir; 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. // 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 // 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 // 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 // -> 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 *pid = (pid_params *)(t->user_data);
uint8_t accel_rate = CV_ARRAY_FLASH[2]; const uint8_t accel_rate = CV_ARRAY_FLASH[2];
uint8_t decel_rate = CV_ARRAY_FLASH[3]; const uint8_t decel_rate = CV_ARRAY_FLASH[3];
static bool prev_dir; static bool prev_dir;
static uint8_t speed_helper_counter; static uint8_t speed_helper_counter;
static uint8_t speed_table_index; 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. // 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 // Forward
if (pid->direction) { if (pid->direction) {
pwm_set_gpio_level(MOTOR_REV_PIN, 0); 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 // 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_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); 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 // 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 // Forward
if(pid->direction) { if(pid->direction) {
pid->y_2_fwd += i*pid->k_ff; 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 // 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; float val;
// Forward // Forward
if(pid->direction){ if(pid->direction){
@@ -147,7 +147,7 @@ float get_ff_val(pid_params *pid){
// Returns proportional gain value corresponding to current setpoint // 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; float sp = (float)pid->setpoint;
if (sp < pid->k_p_x_1){ if (sp < pid->k_p_x_1){
return pid->k_p_m_1 * sp + pid->k_p_y_0; 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 // 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); pid->k_p = get_kp(pid);
// Measure BEMF voltage and compute error // Measure BEMF voltage and compute error
@@ -180,16 +180,16 @@ bool pid_control(struct repeating_timer *t){
pid->l_side_arr_cutoff, pid->l_side_arr_cutoff,
pid->r_side_arr_cutoff, pid->r_side_arr_cutoff,
pid->direction); 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) // 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 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 // Check for limits on the integral part
if (i > pid->int_lim_max)i = pid->int_lim_max; if (i > pid->int_lim_max) i = pid->int_lim_max;
else if (i < pid->int_lim_min)i = pid->int_lim_min; 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 // Sum feed forward + all controller terms (p+i+d). Limit Output from 0% to 100% duty cycle
float output = feed_fwd+p+i+d; 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 // 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 // Measurement variables initialization
pid->adc_offset = (float)CV_ARRAY_FLASH[171]; pid->adc_offset = (float)CV_ARRAY_FLASH[171];
pid->msr_total_iterations = CV_ARRAY_FLASH[60]; pid->msr_total_iterations = CV_ARRAY_FLASH[60];
@@ -236,15 +236,15 @@ void init_pid(pid_params *pid){
// Speed table initialization // Speed table initialization
// Calculate speed setpoint table according to V_min, V_max and V_mid CVs // Calculate speed setpoint table according to V_min, V_max and V_mid CVs
double v_min = (double)CV_ARRAY_FLASH[1]; const double v_min = (double)CV_ARRAY_FLASH[1];
double v_mid = (double)CV_ARRAY_FLASH[5]*16; const double v_mid = (double)CV_ARRAY_FLASH[5]*16;
double v_max = (double)CV_ARRAY_FLASH[4]*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); LOG(1, "speedtable min(%f) mid(%f) max(%f)\n", v_min, v_mid, v_max);
double delta_x = 63; const double delta_x = 63;
double m_1 = (v_mid-v_min)/delta_x; const double m_1 = (v_mid-v_min)/delta_x;
double m_2 = (v_max-v_mid)/delta_x; const double m_2 = (v_max-v_mid)/delta_x;
pid->speed_table[0] = 0; pid->speed_table[0] = 0;
for (uint8_t i = 1; i < 64; ++i) { for (uint8_t i = 1; i < 64; ++i) {
pid->speed_table[i] = (uint16_t)lround(m_1*(i-1)+v_min); 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). // 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(). // This is done only to accommodate the correct data type associated with multicore_fifo_push_blocking().
multicore_fifo_push_blocking((uint32_t) &(pid->direction)); 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(); absolute_time_t time_last_update = get_absolute_time();
// printf("core1 done\n"); // printf("core1 done\n");
while (1){ while (1){
+4 -4
View File
@@ -62,10 +62,10 @@ typedef struct pid_params{
void update_speed_dir(pid_params *pid, uint32_t speed_step_target); void update_speed_dir(pid_params *pid, uint32_t speed_step_target);
bool speed_helper(struct repeating_timer *t); 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_m(pid_params *pid);
void update_y(pid_params *pid,float i); void update_y(pid_params *pid,float i);
float get_ff_val(pid_params *pid); float get_ff_val(const pid_params *pid);
float get_kp(pid_params *pid); float get_kp(const pid_params *pid);
bool pid_control(struct repeating_timer *t); bool pid_control(struct repeating_timer *t);
void init_pid(pid_params *pid); void init_pid( pid_params * pid);