mirror of https://github.com/rusefi/bldc.git
Lisp updates, imu refactoring and rate check
This commit is contained in:
parent
9893c058a3
commit
18a683fc7d
24
commands.c
24
commands.c
|
@ -61,7 +61,7 @@
|
||||||
#include <stdio.h>
|
#include <stdio.h>
|
||||||
|
|
||||||
// Settings
|
// Settings
|
||||||
#define PRINT_BUFFER_SIZE 255
|
#define PRINT_BUFFER_SIZE 400
|
||||||
|
|
||||||
// Threads
|
// Threads
|
||||||
static THD_FUNCTION(blocking_thread, arg);
|
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);
|
chMtxLock(&print_mutex);
|
||||||
|
|
||||||
va_list arg;
|
va_list arg;
|
||||||
|
@ -1627,15 +1627,18 @@ void commands_printf(const char* format, ...) {
|
||||||
len = vsnprintf(print_buffer + 1, (PRINT_BUFFER_SIZE - 1), format, arg);
|
len = vsnprintf(print_buffer + 1, (PRINT_BUFFER_SIZE - 1), format, arg);
|
||||||
va_end (arg);
|
va_end (arg);
|
||||||
|
|
||||||
|
int len_to_print = (len < (PRINT_BUFFER_SIZE - 1)) ? len + 1 : PRINT_BUFFER_SIZE;
|
||||||
|
|
||||||
if(len > 0) {
|
if(len > 0) {
|
||||||
commands_send_packet_last_blocking((unsigned char*)print_buffer,
|
commands_send_packet_last_blocking((unsigned char*)print_buffer, len_to_print);
|
||||||
(len < (PRINT_BUFFER_SIZE - 1)) ? len + 1 : PRINT_BUFFER_SIZE);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
chMtxUnlock(&print_mutex);
|
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);
|
chMtxLock(&print_mutex);
|
||||||
|
|
||||||
va_list arg;
|
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);
|
len = vsnprintf(print_buffer + 1, (PRINT_BUFFER_SIZE - 1), format, arg);
|
||||||
va_end (arg);
|
va_end (arg);
|
||||||
|
|
||||||
|
int len_to_print = (len < (PRINT_BUFFER_SIZE - 1)) ? len + 1 : PRINT_BUFFER_SIZE;
|
||||||
|
|
||||||
if(len > 0) {
|
if(len > 0) {
|
||||||
commands_send_packet_last_blocking((unsigned char*)print_buffer,
|
if (print_buffer[len_to_print - 1] == '\n') {
|
||||||
(len < (PRINT_BUFFER_SIZE - 1)) ? len + 1 : PRINT_BUFFER_SIZE);
|
len_to_print--;
|
||||||
|
}
|
||||||
|
|
||||||
|
commands_send_packet_last_blocking((unsigned char*)print_buffer, len_to_print);
|
||||||
}
|
}
|
||||||
|
|
||||||
chMtxUnlock(&print_mutex);
|
chMtxUnlock(&print_mutex);
|
||||||
|
|
||||||
|
return len_to_print - 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
void commands_send_rotor_pos(float rotor_pos) {
|
void commands_send_rotor_pos(float rotor_pos) {
|
||||||
|
|
|
@ -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_send_packet_last_blocking(unsigned char *data, unsigned int len);
|
||||||
void commands_process_packet(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(*reply_func)(unsigned char *data, unsigned int len));
|
||||||
void commands_printf(const char* format, ...);
|
int commands_printf(const char* format, ...);
|
||||||
void commands_printf_lisp(const char* format, ...);
|
int commands_printf_lisp(const char* format, ...);
|
||||||
void commands_send_rotor_pos(float rotor_pos);
|
void commands_send_rotor_pos(float rotor_pos);
|
||||||
void commands_send_experiment_samples(float *samples, int len);
|
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);
|
void commands_fwd_can_frame(int len, unsigned char *data, uint32_t id, bool is_extended);
|
||||||
|
|
|
@ -24,7 +24,7 @@
|
||||||
#define FW_VERSION_MAJOR 6
|
#define FW_VERSION_MAJOR 6
|
||||||
#define FW_VERSION_MINOR 00
|
#define FW_VERSION_MINOR 00
|
||||||
// Set to 0 for building a release and iterate during beta test builds
|
// 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"
|
#include "datatypes.h"
|
||||||
|
|
||||||
|
|
|
@ -18,6 +18,7 @@
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#include "bmi160_wrapper.h"
|
#include "bmi160_wrapper.h"
|
||||||
|
#include "utils.h"
|
||||||
|
|
||||||
#include <stdio.h>
|
#include <stdio.h>
|
||||||
#include <string.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) {
|
void bmi160_wrapper_init(BMI_STATE *s, stkalign_t *work_area, size_t work_area_size) {
|
||||||
s->read_callback = 0;
|
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)) {
|
if (reset_init_bmi(s)) {
|
||||||
s->should_stop = false;
|
s->should_stop = false;
|
||||||
chThdCreateStatic(work_area, work_area_size, NORMALPRIO, bmi_thread, s);
|
chThdCreateStatic(work_area, work_area_size, NORMALPRIO, bmi_thread, s);
|
||||||
|
|
60
imu/imu.c
60
imu/imu.c
|
@ -48,7 +48,6 @@ static bool imu_ready;
|
||||||
|
|
||||||
// Private functions
|
// Private functions
|
||||||
static void imu_read_callback(float *accel, float *gyro, float *mag);
|
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_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_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);
|
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_stop();
|
||||||
imu_reset_orientation();
|
imu_reset_orientation();
|
||||||
|
|
||||||
mpu9150_set_rate_hz(set->sample_rate_hz);
|
mpu9150_set_rate_hz(MIN(set->sample_rate_hz, 1000));
|
||||||
m_icm20948_state.rate_hz = set->sample_rate_hz;
|
m_icm20948_state.rate_hz = MIN(set->sample_rate_hz, 1000);
|
||||||
m_bmi_state.rate_hz = set->sample_rate_hz;
|
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) {
|
if (set->type == IMU_TYPE_INTERNAL) {
|
||||||
#ifdef MPU9X50_SDA_GPIO
|
#ifdef MPU9X50_SDA_GPIO
|
||||||
|
@ -243,13 +242,13 @@ void imu_get_mag(float *mag) {
|
||||||
memcpy(mag, m_mag, sizeof(m_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];
|
float rpy[3];
|
||||||
imu_get_rpy(rpy);
|
imu_get_rpy(rpy);
|
||||||
|
|
||||||
const float ax = m_accel[0];
|
const float ax = input[0];
|
||||||
const float ay = m_accel[1];
|
const float ay = input[1];
|
||||||
const float az = m_accel[2];
|
const float az = input[2];
|
||||||
|
|
||||||
const float sr = sinf(rpy[0]);
|
const float sr = sinf(rpy[0]);
|
||||||
const float cr = -cosf(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_ax = ax * cp + ay * sp * sr + az * sp * cr;
|
||||||
float c_ay = ay * cr - az * sr;
|
float c_ay = ay * cr - az * sr;
|
||||||
float c_az = -ax * sp + ay * cp * sr + az * cp * cr;
|
float c_az = -ax * sp + ay * cp * sr + az * cp * cr;
|
||||||
|
|
||||||
float c_ax2 = cy * c_ax + sy * c_ay;
|
float c_ax2 = cy * c_ax + sy * c_ay;
|
||||||
float c_ay2 = sy * c_ax - cy * c_ay;
|
float c_ay2 = sy * c_ax - cy * c_ay;
|
||||||
|
|
||||||
accel[0] = c_ax2;
|
output[0] = c_ax2;
|
||||||
accel[1] = c_ay2;
|
output[1] = c_ay2;
|
||||||
accel[2] = c_az;
|
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) {
|
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);
|
m_settings.rot_roll = -RAD2DEG_f(roll_sample);
|
||||||
|
|
||||||
// Rotate gyro offsets to match new IMU orientation
|
// Rotate gyro offsets to match new IMU orientation
|
||||||
float rotation1[3] = {m_settings.rot_roll, m_settings.rot_pitch, m_settings.rot_yaw};
|
float rotation1[3] = {DEG2RAD_f(m_settings.rot_roll), DEG2RAD_f(m_settings.rot_pitch), DEG2RAD_f(m_settings.rot_yaw)};
|
||||||
rotate(original_gyro_offsets, rotation1, m_settings.gyro_offsets);
|
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)
|
// Reset AHRS and wait 1.5 seconds (for AHRS to settle now that pitch is calibrated)
|
||||||
ahrs_init_attitude_info(&m_att);
|
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);
|
m_settings.rot_pitch = RAD2DEG_f(pitch_sample);
|
||||||
|
|
||||||
// Rotate imu offsets to match
|
// Rotate imu offsets to match
|
||||||
float rotation2[3] = {m_settings.rot_roll, m_settings.rot_pitch, m_settings.rot_yaw};
|
float rotation2[3] = {DEG2RAD_f(m_settings.rot_roll), DEG2RAD_f(m_settings.rot_pitch), DEG2RAD_f(m_settings.rot_yaw)};
|
||||||
rotate(original_gyro_offsets, rotation2, m_settings.gyro_offsets);
|
utils_rotate_vector3(original_gyro_offsets, rotation2, m_settings.gyro_offsets, false);
|
||||||
|
|
||||||
// Set yaw rotations to match user input
|
// Set yaw rotations to match user input
|
||||||
m_settings.rot_yaw = yaw;
|
m_settings.rot_yaw = yaw;
|
||||||
|
|
||||||
// Rotate gyro offsets to match new IMU orientation
|
// Rotate gyro offsets to match new IMU orientation
|
||||||
float rotation3[3] = {m_settings.rot_roll, m_settings.rot_pitch, m_settings.rot_yaw};
|
float rotation3[3] = {DEG2RAD_f(m_settings.rot_roll), DEG2RAD_f(m_settings.rot_pitch), DEG2RAD_f(m_settings.rot_yaw)};
|
||||||
rotate(original_gyro_offsets, rotation3, m_settings.gyro_offsets);
|
utils_rotate_vector3(original_gyro_offsets, rotation3, m_settings.gyro_offsets, false);
|
||||||
|
|
||||||
// Note to future person interested in calibration:
|
// 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
|
// 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) {
|
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;
|
m_i2c_bb.has_error = 0;
|
||||||
|
|
||||||
|
|
|
@ -50,7 +50,9 @@ void imu_get_rpy(float *rpy);
|
||||||
void imu_get_accel(float *accel);
|
void imu_get_accel(float *accel);
|
||||||
void imu_get_gyro(float *gyro);
|
void imu_get_gyro(float *gyro);
|
||||||
void imu_get_mag(float *mag);
|
void imu_get_mag(float *mag);
|
||||||
|
void imu_derotate(float *input, float *output);
|
||||||
void imu_get_accel_derotated(float *accel);
|
void imu_get_accel_derotated(float *accel);
|
||||||
|
void imu_get_gyro_derotated(float *gyro);
|
||||||
void imu_get_quaternions(float *q);
|
void imu_get_quaternions(float *q);
|
||||||
void imu_get_calibration(float yaw, float * imu_cal);
|
void imu_get_calibration(float yaw, float * imu_cal);
|
||||||
|
|
||||||
|
|
|
@ -201,6 +201,7 @@ static bool start_lisp(bool print) {
|
||||||
|
|
||||||
lbm_set_timestamp_us_callback(timestamp_callback);
|
lbm_set_timestamp_us_callback(timestamp_callback);
|
||||||
lbm_set_usleep_callback(sleep_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);
|
chThdCreateStatic(eval_thread_wa, sizeof(eval_thread_wa), NORMALPRIO, eval_thread, NULL);
|
||||||
|
|
||||||
lisp_thd_running = true;
|
lisp_thd_running = true;
|
||||||
|
|
|
@ -62,6 +62,15 @@ static const char* functions[] = {
|
||||||
"(let ((len (lambda (l xs)"
|
"(let ((len (lambda (l xs)"
|
||||||
"(if (= xs nil) l (len (+ l 1) (cdr xs))))))"
|
"(if (= xs nil) l (len (+ l 1) (cdr xs))))))"
|
||||||
"(len 0 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[] = {
|
static const char* macros[] = {
|
||||||
|
|
|
@ -355,6 +355,34 @@ static lbm_value ext_get_imu_mag(lbm_value *args, lbm_uint argn) {
|
||||||
return imu_data;
|
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) {
|
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)) {
|
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);
|
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])));
|
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
|
// Bit operations
|
||||||
|
|
||||||
/*
|
/*
|
||||||
|
@ -1318,6 +1374,31 @@ static lbm_value ext_i2c_restore(lbm_value *args, lbm_uint argn) {
|
||||||
return lbm_enc_i(1);
|
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) {
|
void lispif_load_vesc_extensions(void) {
|
||||||
lbm_add_symbol_const("signal-can-sid", &sym_signal_can_sid);
|
lbm_add_symbol_const("signal-can-sid", &sym_signal_can_sid);
|
||||||
lbm_add_symbol_const("signal-can-eid", &sym_signal_can_eid);
|
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-acc", ext_get_imu_acc);
|
||||||
lbm_add_extension("get-imu-gyro", ext_get_imu_gyro);
|
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-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);
|
lbm_add_extension("send-data", ext_send_data);
|
||||||
|
|
||||||
// Motor set commands
|
// Motor set commands
|
||||||
|
@ -1406,6 +1489,8 @@ void lispif_load_vesc_extensions(void) {
|
||||||
lbm_add_extension("sqrt", ext_sqrt);
|
lbm_add_extension("sqrt", ext_sqrt);
|
||||||
lbm_add_extension("log", ext_log);
|
lbm_add_extension("log", ext_log);
|
||||||
lbm_add_extension("log10", ext_log10);
|
lbm_add_extension("log10", ext_log10);
|
||||||
|
lbm_add_extension("deg2rad", ext_deg2rad);
|
||||||
|
lbm_add_extension("rad2deg", ext_rad2deg);
|
||||||
|
|
||||||
// Bit operations
|
// Bit operations
|
||||||
lbm_add_extension("bits-enc-int", ext_bits_enc_int);
|
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-tx-rx", ext_i2c_tx_rx);
|
||||||
lbm_add_extension("i2c-restore", ext_i2c_restore);
|
lbm_add_extension("i2c-restore", ext_i2c_restore);
|
||||||
|
|
||||||
|
// Vectors
|
||||||
|
lbm_add_extension("vec3-rot", ext_vec3_rot);
|
||||||
|
|
||||||
// Array extensions
|
// Array extensions
|
||||||
lbm_array_extensions_init();
|
lbm_array_extensions_init();
|
||||||
}
|
}
|
||||||
|
|
42
utils.c
42
utils.c
|
@ -862,6 +862,48 @@ int utils_stack_left_now(void) {
|
||||||
return ((stkalign_t *)(r13 - 1) - chThdGetSelfX()->p_stklimit) * sizeof(stkalign_t);
|
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[] = {
|
const float utils_tab_sin_32_1[] = {
|
||||||
0.000000, 0.195090, 0.382683, 0.555570, 0.707107, 0.831470, 0.923880, 0.980785,
|
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,
|
1.000000, 0.980785, 0.923880, 0.831470, 0.707107, 0.555570, 0.382683, 0.195090,
|
||||||
|
|
9
utils.h
9
utils.h
|
@ -68,6 +68,7 @@ uint16_t utils_median_filter_uint16_run(uint16_t *buffer,
|
||||||
const char* utils_hw_type_to_string(HW_TYPE hw);
|
const char* utils_hw_type_to_string(HW_TYPE hw);
|
||||||
int utils_check_min_stack_left(thread_t *th);
|
int utils_check_min_stack_left(thread_t *th);
|
||||||
int utils_stack_left_now(void);
|
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.
|
// 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)
|
#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 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)))
|
#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.
|
* A simple low pass filter.
|
||||||
|
|
Loading…
Reference in New Issue