rusefi
/
bldc
mirror of https://github.com/rusefi/bldc.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

[Utils] split into math and system utilities

This commit is contained in:
Kenn Sebesta 2022-03-16 13:43:25 -04:00
parent e06da66343
commit 5ccde377f3
4 changed files with 197 additions and 130 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

120
utils.c → utils_math.c
View File

@ -17,7 +17,7 @@
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "utils.h"
#include "utils_math.h"
#include "hal.h"
#include "app.h"
#include <math.h>
@ -25,8 +25,9 @@
#include <stdlib.h>
// 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;

16
utils.h → utils_math.h
View File

@ -17,8 +17,8 @@
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef UTILS_H_
#define UTILS_H_
#ifndef UTILS_MATH_H_
#define UTILS_MATH_H_
#include <stdbool.h>
#include <stdint.h>
@ -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_ */

152
utils_sys.c Normal file
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
#include "utils_sys.h"
#include "hal.h"
#include "app.h"
#include <math.h>
#include <string.h>
#include <stdlib.h>
// 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
}

39
utils_sys.h Normal file
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
#ifndef UTILS_SYS_H_
#define UTILS_SYS_H_
#include <stdbool.h>
#include <stdint.h>
#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_ */