diff --git a/utils.c b/utils_math.c similarity index 86% rename from utils.c rename to utils_math.c index 2142b2e9..03b38063 100644 --- a/utils.c +++ b/utils_math.c @@ -17,7 +17,7 @@ along with this program. If not, see . */ -#include "utils.h" +#include "utils_math.h" #include "hal.h" #include "app.h" #include @@ -25,8 +25,9 @@ #include // Private variables -static volatile int sys_lock_cnt = 0; + +// Functions void utils_step_towards(float *value, float goal, float step) { if (*value < goal) { if ((*value + step) < goal) { @@ -598,33 +599,7 @@ float utils_throttle_curve(float val, float curve_acc, float curve_brake, int mo return ret; } -/** - * A system locking function with a counter. For every lock, a corresponding unlock must - * exist to unlock the system. That means, if lock is called five times, unlock has to - * be called five times as well. Note that chSysLock and chSysLockFromIsr are the same - * for this port. - */ -void utils_sys_lock_cnt(void) { - if (!sys_lock_cnt) { - chSysLock(); - } - sys_lock_cnt++; -} -/** - * A system unlocking function with a counter. For every lock, a corresponding unlock must - * exist to unlock the system. That means, if lock is called five times, unlock has to - * be called five times as well. Note that chSysUnlock and chSysUnlockFromIsr are the same - * for this port. - */ -void utils_sys_unlock_cnt(void) { - if (sys_lock_cnt) { - sys_lock_cnt--; - if (!sys_lock_cnt) { - chSysUnlock(); - } - } -} uint32_t utils_crc32c(uint8_t *data, uint32_t len) { uint32_t crc = 0xFFFFFFFF; @@ -742,59 +717,6 @@ void utils_fft8_bin2(float *real_in, float *real, float *imag) { *imag /= 8.0; } -/** - * Get ID of second motor. - * - * @return - * id for second motor. -1 if this hardware only has one motor. - */ -uint8_t utils_second_motor_id(void) { -#ifdef HW_HAS_DUAL_MOTORS - uint8_t id_next = app_get_configuration()->controller_id + 1; - if (id_next == 255) { - id_next = 0; - } - return id_next; -#else - return 0; -#endif -} - -/** - * Read hall sensors - * - * @param is_second_motor - * Use hall sensor port for second motor on dual motor hardware. - * - * @param samples - * The number of extra samples to read and filter over. If this - * is 0, only one sample will be used. - * - * @return - * The state of the three hall sensors. - */ -int utils_read_hall(bool is_second_motor, int samples) { - samples = 1 + 2 * samples; - - int h1 = 0, h2 = 0, h3 = 0; - int tres = samples / 2; - - if (is_second_motor) { - while (samples--) { - h1 += READ_HALL1_2(); - h2 += READ_HALL2_2(); - h3 += READ_HALL3_2(); - } - } else { - while (samples--) { - h1 += READ_HALL1(); - h2 += READ_HALL2(); - h3 += READ_HALL3(); - } - } - - return (h1 > tres) | ((h2 > tres) << 1) | ((h3 > tres) << 2); -} // A mapping of a samsung 30q cell for % remaining capacity vs. voltage from // 4.2 to 3.2, note that the you lose 15% of the 3Ah rated capacity in this range @@ -826,43 +748,7 @@ uint16_t utils_median_filter_uint16_run(uint16_t *buffer, return buffer_sorted[filter_len / 2]; } -const char* utils_hw_type_to_string(HW_TYPE hw) { - switch (hw) { - case HW_TYPE_VESC: return "HW_TYPE_VESC"; break; - case HW_TYPE_VESC_BMS: return "HW_TYPE_VESC_BMS"; break; - case HW_TYPE_CUSTOM_MODULE: return "HW_TYPE_CUSTOM_MODULE"; break; - default: return "FAULT_HARDWARE"; break; - } -} -/** - * Check the minimum stack tp had left by counting the remaining fill characters. - */ -int utils_check_min_stack_left(thread_t *tp) { - uint32_t *p = (uint32_t *)tp->p_stklimit; - - int free = 0; - while (free < 8192) { - if (*p++ != 0x55555555) { - break; - } - free += sizeof(uint32_t); - } - - return free; -} - -/* - * Check how much stack the current thread has left now. - */ -int utils_stack_left_now(void) { -#ifndef UNIT_TEST - struct port_intctx *r13 = (struct port_intctx *)__get_PSP(); - return ((stkalign_t *)(r13 - 1) - chThdGetSelfX()->p_stklimit) * sizeof(stkalign_t); -#else - return -1; -#endif // UNIT_TEST -} void utils_rotate_vector3(float *input, float *rotation, float *output, bool reverse) { float s1, c1, s2, c2, s3, c3; diff --git a/utils.h b/utils_math.h similarity index 91% rename from utils.h rename to utils_math.h index adb2c458..b0b85b92 100644 --- a/utils.h +++ b/utils_math.h @@ -17,8 +17,8 @@ along with this program. If not, see . */ -#ifndef UTILS_H_ -#define UTILS_H_ +#ifndef UTILS_MATH_H_ +#define UTILS_MATH_H_ #include #include @@ -48,8 +48,6 @@ float utils_min_abs(float va, float vb); float utils_max_abs(float va, float vb); void utils_byte_to_binary(int x, char *b); float utils_throttle_curve(float val, float curve_acc, float curve_brake, int mode); -void utils_sys_lock_cnt(void); -void utils_sys_unlock_cnt(void); uint32_t utils_crc32c(uint8_t *data, uint32_t len); void utils_fft32_bin0(float *real_in, float *real, float *imag); void utils_fft32_bin1(float *real_in, float *real, float *imag); @@ -60,14 +58,9 @@ void utils_fft16_bin2(float *real_in, float *real, float *imag); void utils_fft8_bin0(float *real_in, float *real, float *imag); void utils_fft8_bin1(float *real_in, float *real, float *imag); void utils_fft8_bin2(float *real_in, float *real, float *imag); -uint8_t utils_second_motor_id(void); -int utils_read_hall(bool is_second_motor, int samples); float utils_batt_liion_norm_v_to_capacity(float norm_v); uint16_t utils_median_filter_uint16_run(uint16_t *buffer, unsigned int *buffer_index, unsigned int filter_len, uint16_t sample); -const char* utils_hw_type_to_string(HW_TYPE hw); -int utils_check_min_stack_left(thread_t *th); -int utils_stack_left_now(void); void utils_rotate_vector3(float *input, float *rotation, float *output, bool reverse); // Return the sign of the argument. -1.0 if negative, 1.0 if zero or positive. @@ -81,9 +74,6 @@ void utils_rotate_vector3(float *input, float *rotation, float *output, bool rev #define NORM2_f(x,y) (sqrtf(SQ(x) + SQ(y))) -// Return the age of a timestamp in seconds -#define UTILS_AGE_S(x) ((float)chVTTimeElapsedSinceX(x) / (float)CH_CFG_ST_FREQUENCY) - // nan and infinity check for floats #define UTILS_IS_INF(x) ((x) == (1.0 / 0.0) || (x) == (-1.0 / 0.0)) #define UTILS_IS_NAN(x) ((x) != (x)) @@ -141,4 +131,4 @@ extern const float utils_tab_sin_32_2[]; extern const float utils_tab_cos_32_1[]; extern const float utils_tab_cos_32_2[]; -#endif /* UTILS_H_ */ +#endif /* UTILS_MATH_H_ */ diff --git a/utils_sys.c b/utils_sys.c new file mode 100644 index 00000000..718272c3 --- /dev/null +++ b/utils_sys.c @@ -0,0 +1,152 @@ +/* + Copyright 2016 - 2019 Benjamin Vedder benjamin@vedder.se + + This file is part of the VESC firmware. + + The VESC firmware is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + The VESC firmware is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +#include "utils_sys.h" +#include "hal.h" +#include "app.h" +#include +#include +#include + +// Private variables +static volatile int sys_lock_cnt = 0; + + + +/** + * A system locking function with a counter. For every lock, a corresponding unlock must + * exist to unlock the system. That means, if lock is called five times, unlock has to + * be called five times as well. Note that chSysLock and chSysLockFromIsr are the same + * for this port. + */ +void utils_sys_lock_cnt(void) { + if (!sys_lock_cnt) { + chSysLock(); + } + sys_lock_cnt++; +} + +/** + * A system unlocking function with a counter. For every lock, a corresponding unlock must + * exist to unlock the system. That means, if lock is called five times, unlock has to + * be called five times as well. Note that chSysUnlock and chSysUnlockFromIsr are the same + * for this port. + */ +void utils_sys_unlock_cnt(void) { + if (sys_lock_cnt) { + sys_lock_cnt--; + if (!sys_lock_cnt) { + chSysUnlock(); + } + } +} + + +/** + * Get ID of second motor. + * + * @return + * id for second motor. -1 if this hardware only has one motor. + */ +uint8_t utils_second_motor_id(void) { +#ifdef HW_HAS_DUAL_MOTORS + uint8_t id_next = app_get_configuration()->controller_id + 1; + if (id_next == 255) { + id_next = 0; + } + return id_next; +#else + return 0; +#endif +} + +/** + * Read hall sensors + * + * @param is_second_motor + * Use hall sensor port for second motor on dual motor hardware. + * + * @param samples + * The number of extra samples to read and filter over. If this + * is 0, only one sample will be used. + * + * @return + * The state of the three hall sensors. + */ +int utils_read_hall(bool is_second_motor, int samples) { + samples = 1 + 2 * samples; + + int h1 = 0, h2 = 0, h3 = 0; + int tres = samples / 2; + + if (is_second_motor) { + while (samples--) { + h1 += READ_HALL1_2(); + h2 += READ_HALL2_2(); + h3 += READ_HALL3_2(); + } + } else { + while (samples--) { + h1 += READ_HALL1(); + h2 += READ_HALL2(); + h3 += READ_HALL3(); + } + } + + return (h1 > tres) | ((h2 > tres) << 1) | ((h3 > tres) << 2); +} + + +const char* utils_hw_type_to_string(HW_TYPE hw) { + switch (hw) { + case HW_TYPE_VESC: return "HW_TYPE_VESC"; break; + case HW_TYPE_VESC_BMS: return "HW_TYPE_VESC_BMS"; break; + case HW_TYPE_CUSTOM_MODULE: return "HW_TYPE_CUSTOM_MODULE"; break; + default: return "FAULT_HARDWARE"; break; + } +} + +/** + * Check the minimum stack tp had left by counting the remaining fill characters. + */ +int utils_check_min_stack_left(thread_t *tp) { + uint32_t *p = (uint32_t *)tp->p_stklimit; + + int free = 0; + while (free < 8192) { + if (*p++ != 0x55555555) { + break; + } + free += sizeof(uint32_t); + } + + return free; +} + +/* + * Check how much stack the current thread has left now. + */ +int utils_stack_left_now(void) { +#ifndef UNIT_TEST + struct port_intctx *r13 = (struct port_intctx *)__get_PSP(); + return ((stkalign_t *)(r13 - 1) - chThdGetSelfX()->p_stklimit) * sizeof(stkalign_t); +#else + return -1; +#endif // UNIT_TEST +} diff --git a/utils_sys.h b/utils_sys.h new file mode 100644 index 00000000..cc817a5e --- /dev/null +++ b/utils_sys.h @@ -0,0 +1,39 @@ +/* + Copyright 2016 - 2019 Benjamin Vedder benjamin@vedder.se + + This file is part of the VESC firmware. + + The VESC firmware is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + The VESC firmware is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +#ifndef UTILS_SYS_H_ +#define UTILS_SYS_H_ + +#include +#include +#include "datatypes.h" + +void utils_sys_lock_cnt(void); +void utils_sys_unlock_cnt(void); +uint8_t utils_second_motor_id(void); +int utils_read_hall(bool is_second_motor, int samples); +const char* utils_hw_type_to_string(HW_TYPE hw); +int utils_check_min_stack_left(thread_t *th); +int utils_stack_left_now(void); + + +// Return the age of a timestamp in seconds +#define UTILS_AGE_S(x) ((float)chVTTimeElapsedSinceX(x) / (float)CH_CFG_ST_FREQUENCY) + +#endif /* UTILS_SYS_H_ */