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

Lisp updates, imu refactoring and rate check

This commit is contained in:
Benjamin Vedder 2022-02-28 14:29:59 +01:00
parent 9893c058a3
commit 18a683fc7d
11 changed files with 201 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
commands.c
View File

@ -61,7 +61,7 @@
#include <stdio.h>
// Settings
#define PRINT_BUFFER_SIZE 255
#define PRINT_BUFFER_SIZE 400
// Threads
static THD_FUNCTION(blocking_thread, arg);
@ -1616,7 +1616,7 @@ void commands_process_packet(unsigned char *data, unsigned int len,
}
}
void commands_printf(const char* format, ...) {
int commands_printf(const char* format, ...) {
chMtxLock(&print_mutex);
va_list arg;
@ -1627,15 +1627,18 @@ void commands_printf(const char* format, ...) {
len = vsnprintf(print_buffer + 1, (PRINT_BUFFER_SIZE - 1), format, arg);
va_end (arg);
int len_to_print = (len < (PRINT_BUFFER_SIZE - 1)) ? len + 1 : PRINT_BUFFER_SIZE;
if(len > 0) {
commands_send_packet_last_blocking((unsigned char*)print_buffer,
(len < (PRINT_BUFFER_SIZE - 1)) ? len + 1 : PRINT_BUFFER_SIZE);
commands_send_packet_last_blocking((unsigned char*)print_buffer, len_to_print);
}
chMtxUnlock(&print_mutex);
return len_to_print - 1;
}
void commands_printf_lisp(const char* format, ...) {
int commands_printf_lisp(const char* format, ...) {
chMtxLock(&print_mutex);
va_list arg;
@ -1646,12 +1649,19 @@ void commands_printf_lisp(const char* format, ...) {
len = vsnprintf(print_buffer + 1, (PRINT_BUFFER_SIZE - 1), format, arg);
va_end (arg);
int len_to_print = (len < (PRINT_BUFFER_SIZE - 1)) ? len + 1 : PRINT_BUFFER_SIZE;
if(len > 0) {
commands_send_packet_last_blocking((unsigned char*)print_buffer,
(len < (PRINT_BUFFER_SIZE - 1)) ? len + 1 : PRINT_BUFFER_SIZE);
if (print_buffer[len_to_print - 1] == '\n') {
len_to_print--;
}
commands_send_packet_last_blocking((unsigned char*)print_buffer, len_to_print);
}
chMtxUnlock(&print_mutex);
return len_to_print - 1;
}
void commands_send_rotor_pos(float rotor_pos) {

4
commands.h
View File

@ -35,8 +35,8 @@ void commands_send_packet_nrf(unsigned char *data, unsigned int len);
void commands_send_packet_last_blocking(unsigned char *data, unsigned int len);
void commands_process_packet(unsigned char *data, unsigned int len,
void(*reply_func)(unsigned char *data, unsigned int len));
void commands_printf(const char* format, ...);
void commands_printf_lisp(const char* format, ...);
int commands_printf(const char* format, ...);
int commands_printf_lisp(const char* format, ...);
void commands_send_rotor_pos(float rotor_pos);
void commands_send_experiment_samples(float *samples, int len);
void commands_fwd_can_frame(int len, unsigned char *data, uint32_t id, bool is_extended);

2
conf_general.h
View File

@ -24,7 +24,7 @@
#define FW_VERSION_MAJOR 6
#define FW_VERSION_MINOR 00
// Set to 0 for building a release and iterate during beta test builds
#define FW_TEST_VERSION_NUMBER 10
#define FW_TEST_VERSION_NUMBER 11
#include "datatypes.h"

7
imu/bmi160_wrapper.c
View File

@ -18,6 +18,7 @@
*/
#include "bmi160_wrapper.h"
#include "utils.h"
#include <stdio.h>
#include <string.h>
@ -32,6 +33,12 @@ void user_delay_ms(uint32_t ms);
void bmi160_wrapper_init(BMI_STATE *s, stkalign_t *work_area, size_t work_area_size) {
s->read_callback = 0;
if (s->sensor.interface == BMI160_SPI_INTF) {
s->rate_hz = MIN(s->rate_hz, 5000);
} else {
s->rate_hz = MIN(s->rate_hz, 1000);
}
if (reset_init_bmi(s)) {
s->should_stop = false;
chThdCreateStatic(work_area, work_area_size, NORMALPRIO, bmi_thread, s);

60
imu/imu.c
View File

@ -48,7 +48,6 @@ static bool imu_ready;
// Private functions
static void imu_read_callback(float *accel, float *gyro, float *mag);
static void rotate(float *input, float *rotation, float *output);
static int8_t user_i2c_read(uint8_t dev_addr, uint8_t reg_addr, uint8_t *data, uint16_t len);
static int8_t user_i2c_write(uint8_t dev_addr, uint8_t reg_addr, uint8_t *data, uint16_t len);
static int8_t user_spi_read(uint8_t dev_id, uint8_t reg_addr, uint8_t *data, uint16_t len);
@ -60,10 +59,10 @@ void imu_init(imu_config *set) {
imu_stop();
imu_reset_orientation();
mpu9150_set_rate_hz(set->sample_rate_hz);
m_icm20948_state.rate_hz = set->sample_rate_hz;
mpu9150_set_rate_hz(MIN(set->sample_rate_hz, 1000));
m_icm20948_state.rate_hz = MIN(set->sample_rate_hz, 1000);
m_bmi_state.rate_hz = set->sample_rate_hz;
lsm6ds3_set_rate_hz(set->sample_rate_hz);
lsm6ds3_set_rate_hz(MIN(set->sample_rate_hz, 1000));
if (set->type == IMU_TYPE_INTERNAL) {
#ifdef MPU9X50_SDA_GPIO
@ -243,13 +242,13 @@ void imu_get_mag(float *mag) {
memcpy(mag, m_mag, sizeof(m_mag));
}
void imu_get_accel_derotated(float *accel) {
void imu_derotate(float *input, float *output) {
float rpy[3];
imu_get_rpy(rpy);
const float ax = m_accel[0];
const float ay = m_accel[1];
const float az = m_accel[2];
const float ax = input[0];
const float ay = input[1];
const float az = input[2];
const float sr = sinf(rpy[0]);
const float cr = -cosf(rpy[0]);
@ -261,12 +260,21 @@ void imu_get_accel_derotated(float *accel) {
float c_ax = ax * cp + ay * sp * sr + az * sp * cr;
float c_ay = ay * cr - az * sr;
float c_az = -ax * sp + ay * cp * sr + az * cp * cr;
float c_ax2 = cy * c_ax + sy * c_ay;
float c_ay2 = sy * c_ax - cy * c_ay;
accel[0] = c_ax2;
accel[1] = c_ay2;
accel[2] = c_az;
output[0] = c_ax2;
output[1] = c_ay2;
output[2] = c_az;
}
void imu_get_accel_derotated(float *accel) {
imu_derotate(m_accel, accel);
}
void imu_get_gyro_derotated(float *gyro) {
imu_derotate(m_gyro, gyro);
}
void imu_get_quaternions(float *q) {
@ -337,8 +345,8 @@ void imu_get_calibration(float yaw, float *imu_cal) {
m_settings.rot_roll = -RAD2DEG_f(roll_sample);
// Rotate gyro offsets to match new IMU orientation
float rotation1[3] = {m_settings.rot_roll, m_settings.rot_pitch, m_settings.rot_yaw};
rotate(original_gyro_offsets, rotation1, m_settings.gyro_offsets);
float rotation1[3] = {DEG2RAD_f(m_settings.rot_roll), DEG2RAD_f(m_settings.rot_pitch), DEG2RAD_f(m_settings.rot_yaw)};
utils_rotate_vector3(original_gyro_offsets, rotation1, m_settings.gyro_offsets, false);
// Reset AHRS and wait 1.5 seconds (for AHRS to settle now that pitch is calibrated)
ahrs_init_attitude_info(&m_att);
@ -356,15 +364,15 @@ void imu_get_calibration(float yaw, float *imu_cal) {
m_settings.rot_pitch = RAD2DEG_f(pitch_sample);
// Rotate imu offsets to match
float rotation2[3] = {m_settings.rot_roll, m_settings.rot_pitch, m_settings.rot_yaw};
rotate(original_gyro_offsets, rotation2, m_settings.gyro_offsets);
float rotation2[3] = {DEG2RAD_f(m_settings.rot_roll), DEG2RAD_f(m_settings.rot_pitch), DEG2RAD_f(m_settings.rot_yaw)};
utils_rotate_vector3(original_gyro_offsets, rotation2, m_settings.gyro_offsets, false);
// Set yaw rotations to match user input
m_settings.rot_yaw = yaw;
// Rotate gyro offsets to match new IMU orientation
float rotation3[3] = {m_settings.rot_roll, m_settings.rot_pitch, m_settings.rot_yaw};
rotate(original_gyro_offsets, rotation3, m_settings.gyro_offsets);
float rotation3[3] = {DEG2RAD_f(m_settings.rot_roll), DEG2RAD_f(m_settings.rot_pitch), DEG2RAD_f(m_settings.rot_yaw)};
utils_rotate_vector3(original_gyro_offsets, rotation3, m_settings.gyro_offsets, false);
// Note to future person interested in calibration:
// This is where accel calibration should go, because at this point the values should be 0,0,1
@ -506,24 +514,6 @@ static void imu_read_callback(float *accel, float *gyro, float *mag) {
}
}
void rotate(float *input, float *rotation, float *output) {
// Rotate imu offsets to match
float s1 = sinf(DEG2RAD_f(rotation[2]));
float c1 = cosf(DEG2RAD_f(rotation[2]));
float s2 = sinf(DEG2RAD_f(rotation[1]));
float c2 = cosf(DEG2RAD_f(rotation[1]));
float s3 = sinf(DEG2RAD_f(rotation[0]));
float c3 = cosf(DEG2RAD_f(rotation[0]));
float m11 = c1 * c2; float m12 = c1 * s2 * s3 - c3 * s1; float m13 = s1 * s3 + c1 * c3 * s2;
float m21 = c2 * s1; float m22 = c1 * c3 + s1 * s2 * s3; float m23 = c3 * s1 * s2 - c1 * s3;
float m31 = -s2; float m32 = c2 * s3; float m33 = c2 * c3;
output[0] = input[0] * m11 + input[1] * m12 + input[2] * m13;
output[1] = input[0] * m21 + input[1] * m22 + input[2] * m23;
output[2] = input[0] * m31 + input[1] * m32 + input[2] * m33;
}
static int8_t user_i2c_read(uint8_t dev_addr, uint8_t reg_addr, uint8_t *data, uint16_t len) {
m_i2c_bb.has_error = 0;

2
imu/imu.h
View File

@ -50,7 +50,9 @@ void imu_get_rpy(float *rpy);
void imu_get_accel(float *accel);
void imu_get_gyro(float *gyro);
void imu_get_mag(float *mag);
void imu_derotate(float *input, float *output);
void imu_get_accel_derotated(float *accel);
void imu_get_gyro_derotated(float *gyro);
void imu_get_quaternions(float *q);
void imu_get_calibration(float yaw, float * imu_cal);

1
lispBM/lispif.c
View File

@ -201,6 +201,7 @@ static bool start_lisp(bool print) {
lbm_set_timestamp_us_callback(timestamp_callback);
lbm_set_usleep_callback(sleep_callback);
lbm_set_printf_callback(commands_printf_lisp);
chThdCreateStatic(eval_thread_wa, sizeof(eval_thread_wa), NORMALPRIO, eval_thread, NULL);
lisp_thd_running = true;

9
lispBM/lispif_vesc_dynamic_loader.c
View File

@ -62,6 +62,15 @@ static const char* functions[] = {
"(let ((len (lambda (l xs)"
"(if (= xs nil) l (len (+ l 1) (cdr xs))))))"
"(len 0 xs)))",
"(defun apply (fun lst) (eval `(,fun ,@lst)))",
"(defun zipwith (f x y)"
"(let ((map-rec (lambda (f res xs ys)"
"(if (= xs nil)"
"(reverse res)"
"(map-rec f (cons (f (car xs) (car ys)) res) (cdr xs) (cdr ys))))))"
"(map-rec f nil x y)))",
};
static const char* macros[] = {

88
lispBM/lispif_vesc_extensions.c
View File

@ -355,6 +355,34 @@ static lbm_value ext_get_imu_mag(lbm_value *args, lbm_uint argn) {
return imu_data;
}
static lbm_value ext_get_imu_acc_derot(lbm_value *args, lbm_uint argn) {
(void)args; (void)argn;
float acc[3];
imu_get_accel_derotated(acc);
lbm_value imu_data = lbm_enc_sym(SYM_NIL);
imu_data = lbm_cons(lbm_enc_F(acc[2]), imu_data);
imu_data = lbm_cons(lbm_enc_F(acc[1]), imu_data);
imu_data = lbm_cons(lbm_enc_F(acc[0]), imu_data);
return imu_data;
}
static lbm_value ext_get_imu_gyro_derot(lbm_value *args, lbm_uint argn) {
(void)args; (void)argn;
float gyro[3];
imu_get_gyro_derotated(gyro);
lbm_value imu_data = lbm_enc_sym(SYM_NIL);
imu_data = lbm_cons(lbm_enc_F(gyro[2]), imu_data);
imu_data = lbm_cons(lbm_enc_F(gyro[1]), imu_data);
imu_data = lbm_cons(lbm_enc_F(gyro[0]), imu_data);
return imu_data;
}
static lbm_value ext_send_data(lbm_value *args, lbm_uint argn) {
if (argn != 1 || (lbm_type_of(args[0]) != LBM_PTR_TYPE_CONS && lbm_type_of(args[0]) != LBM_PTR_TYPE_ARRAY)) {
return lbm_enc_sym(SYM_EERROR);
@ -830,6 +858,34 @@ static lbm_value ext_log10(lbm_value *args, lbm_uint argn) {
return lbm_enc_F(log10f(lbm_dec_as_f(args[0])));
}
static lbm_value ext_deg2rad(lbm_value *args, lbm_uint argn) {
CHECK_NUMBER_ALL();
if (argn == 1) {
return lbm_enc_F(DEG2RAD_f(lbm_dec_as_f(args[0])));
} else {
lbm_value out_list = lbm_enc_sym(SYM_NIL);
for (int i = (argn - 1);i >= 0;i--) {
out_list = lbm_cons(lbm_enc_F(DEG2RAD_f(lbm_dec_as_f(args[i]))), out_list);
}
return out_list;
}
}
static lbm_value ext_rad2deg(lbm_value *args, lbm_uint argn) {
CHECK_NUMBER_ALL();
if (argn == 1) {
return lbm_enc_F(RAD2DEG_f(lbm_dec_as_f(args[0])));
} else {
lbm_value out_list = lbm_enc_sym(SYM_NIL);
for (int i = (argn - 1);i >= 0;i--) {
out_list = lbm_cons(lbm_enc_F(RAD2DEG_f(lbm_dec_as_f(args[i]))), out_list);
}
return out_list;
}
}
// Bit operations
/*
@ -1318,6 +1374,31 @@ static lbm_value ext_i2c_restore(lbm_value *args, lbm_uint argn) {
return lbm_enc_i(1);
}
static lbm_value ext_vec3_rot(lbm_value *args, lbm_uint argn) {
CHECK_NUMBER_ALL();
if (argn != 6 && argn != 7) {
return lbm_enc_sym(SYM_EERROR);
}
float input[] = {lbm_dec_as_f(args[0]), lbm_dec_as_f(args[1]), lbm_dec_as_f(args[2])};
float rotation[] = {lbm_dec_as_f(args[3]), lbm_dec_as_f(args[4]), lbm_dec_as_f(args[5])};
float output[3];
bool reverse = false;
if (argn == 7) {
reverse = lbm_dec_as_i(args[6]);
}
utils_rotate_vector3(input, rotation, output, reverse);
lbm_value out_list = lbm_enc_sym(SYM_NIL);
out_list = lbm_cons(lbm_enc_F(output[2]), out_list);
out_list = lbm_cons(lbm_enc_F(output[1]), out_list);
out_list = lbm_cons(lbm_enc_F(output[0]), out_list);
return out_list;
}
void lispif_load_vesc_extensions(void) {
lbm_add_symbol_const("signal-can-sid", &sym_signal_can_sid);
lbm_add_symbol_const("signal-can-eid", &sym_signal_can_eid);
@ -1344,6 +1425,8 @@ void lispif_load_vesc_extensions(void) {
lbm_add_extension("get-imu-acc", ext_get_imu_acc);
lbm_add_extension("get-imu-gyro", ext_get_imu_gyro);
lbm_add_extension("get-imu-mag", ext_get_imu_mag);
lbm_add_extension("get-imu-acc-derot", ext_get_imu_acc_derot);
lbm_add_extension("get-imu-gyro-derot", ext_get_imu_gyro_derot);
lbm_add_extension("send-data", ext_send_data);
// Motor set commands
@ -1406,6 +1489,8 @@ void lispif_load_vesc_extensions(void) {
lbm_add_extension("sqrt", ext_sqrt);
lbm_add_extension("log", ext_log);
lbm_add_extension("log10", ext_log10);
lbm_add_extension("deg2rad", ext_deg2rad);
lbm_add_extension("rad2deg", ext_rad2deg);
// Bit operations
lbm_add_extension("bits-enc-int", ext_bits_enc_int);
@ -1432,6 +1517,9 @@ void lispif_load_vesc_extensions(void) {
lbm_add_extension("i2c-tx-rx", ext_i2c_tx_rx);
lbm_add_extension("i2c-restore", ext_i2c_restore);
// Vectors
lbm_add_extension("vec3-rot", ext_vec3_rot);
// Array extensions
lbm_array_extensions_init();
}

42
utils.c
View File

@ -862,6 +862,48 @@ int utils_stack_left_now(void) {
return ((stkalign_t *)(r13 - 1) - chThdGetSelfX()->p_stklimit) * sizeof(stkalign_t);
}
void utils_rotate_vector3(float *input, float *rotation, float *output, bool reverse) {
float s1, c1, s2, c2, s3, c3;
if (rotation[2] != 0.0) {
s1 = sinf(rotation[2]);
c1 = cosf(rotation[2]);
} else {
s1 = 0.0;
c1 = 1.0;
}
if (rotation[1] != 0.0) {
s2 = sinf(rotation[1]);
c2 = cosf(rotation[1]);
} else {
s2 = 0.0;
c2 = 1.0;
}
if (rotation[0] != 0.0) {
s3 = sinf(rotation[0]);
c3 = cosf(rotation[0]);
} else {
s3 = 0.0;
c3 = 1.0;
}
float m11 = c1 * c2; float m12 = c1 * s2 * s3 - c3 * s1; float m13 = s1 * s3 + c1 * c3 * s2;
float m21 = c2 * s1; float m22 = c1 * c3 + s1 * s2 * s3; float m23 = c3 * s1 * s2 - c1 * s3;
float m31 = -s2; float m32 = c2 * s3; float m33 = c2 * c3;
if (reverse) {
output[0] = input[0] * m11 + input[1] * m21 + input[2] * m31;
output[1] = input[0] * m12 + input[1] * m22 + input[2] * m32;
output[2] = input[0] * m13 + input[1] * m23 + input[2] * m33;
} else {
output[0] = input[0] * m11 + input[1] * m12 + input[2] * m13;
output[1] = input[0] * m21 + input[1] * m22 + input[2] * m23;
output[2] = input[0] * m31 + input[1] * m32 + input[2] * m33;
}
}
const float utils_tab_sin_32_1[] = {
0.000000, 0.195090, 0.382683, 0.555570, 0.707107, 0.831470, 0.923880, 0.980785,
1.000000, 0.980785, 0.923880, 0.831470, 0.707107, 0.555570, 0.382683, 0.195090,

9
utils.h
View File

@ -68,6 +68,7 @@ uint16_t utils_median_filter_uint16_run(uint16_t *buffer,
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.
#define SIGN(x) (((x) < 0.0) ? -1.0 : 1.0)
@ -89,8 +90,12 @@ int utils_stack_left_now(void);
#define RPM2RADPS_f(rpm) ((rpm) * (float)((2.0 * M_PI) / 60.0))
#define RADPS2RPM_f(rad_per_sec) ((rad_per_sec) * (float)(60.0 / (2.0 * M_PI)))
#ifndef MIN
#define MIN(a,b) (((a)<(b))?(a):(b))
#endif
#ifndef MAX
#define MAX(a,b) (((a)>(b))?(a):(b))
#endif
/**
* A simple low pass filter.