Skip to content

Commit

Permalink
format
Browse files Browse the repository at this point in the history
  • Loading branch information
IhorNehrutsa authored and IhorNehrutsa committed Dec 25, 2023
1 parent 649c9da commit 8ec9436
Show file tree
Hide file tree
Showing 3 changed files with 68 additions and 70 deletions.
76 changes: 38 additions & 38 deletions ports/esp32/cmodules/invsqrt/invsqrt.c
100755 → 100644
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,39 +15,39 @@

float invSqrt(float x) {
#if USE_VERSION == 1
// Fast inverse square-root
// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
float halfx = 0.5f * x;
float y = x;
long i = *(long*)&y;
i = 0x5f3759df - (i>>1);
y = *(float*)&i;
return y * (1.5f - (halfx * y * y));
// Fast inverse square-root
// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
float halfx = 0.5f * x;
float y = x;
long i = *(long *)&y;
i = 0x5f3759df - (i >> 1);
y = *(float *)&i;
return y * (1.5f - (halfx * y * y));

#elif USE_VERSION == 2
// A Modification of the Fast Inverse SquareRoot Algorithm
// See: https://www.researchgate.net/publication/335235981_A_Modification_of_the_Fast_Inverse_Square_Root_Algorithm
float simhalfnumber = 0.500438180f * x;
int i = *(int*) &x;
i = 0x5F375A86 - (i>>1);
float y = *(float*) &i;
y = y * (1.50131454f - simhalfnumber * y * y);
return y * (1.50000086f - 0.999124984f * simhalfnumber * y * y);
// A Modification of the Fast Inverse SquareRoot Algorithm
// See: https://www.researchgate.net/publication/335235981_A_Modification_of_the_Fast_Inverse_Square_Root_Algorithm
float simhalfnumber = 0.500438180f * x;
int i = *(int *)&x;
i = 0x5F375A86 - (i >> 1);
float y = *(float *)&i;
y = y * (1.50131454f - simhalfnumber * y * y);
return y * (1.50000086f - 0.999124984f * simhalfnumber * y * y);
#elif USE_VERSION == 3
// Fast Inverse Square Root. Pizer's Weblog
// See: https://pizer.wordpress.com/2008/10/12/fast-inverse-square-root/
uint32_t i = 0x5F1F1412 - (*(uint32_t*)&x >> 1);
float tmp = *(float*)&i;
return tmp * (1.69000231f - 0.714158168f * x * tmp * tmp);
// Fast Inverse Square Root. Pizer's Weblog
// See: https://pizer.wordpress.com/2008/10/12/fast-inverse-square-root/
uint32_t i = 0x5F1F1412 - (*(uint32_t *)&x >> 1);
float tmp = *(float *)&i;
return tmp * (1.69000231f - 0.714158168f * x * tmp * tmp);
#else
return 1.0f/sqrt(x);
return 1.0f / sqrt(x);
#endif
}

// functions for micropython invsqrt module

//
//1 / sqrt(x)
// 1 / sqrt(x)
//
STATIC mp_obj_t invsqrt_invSqrt(mp_obj_t x_obj) {
mp_float_t x = mp_obj_get_float(x_obj);
Expand All @@ -62,18 +62,18 @@ MP_DEFINE_CONST_FUN_OBJ_1(invsqrt_invSqrt_obj, invsqrt_invSqrt);

#if USE_VERSION == 1
//
//Fast inverse square-root
//http://en.wikipedia.org/wiki/Fast_inverse_square_root
// Fast inverse square-root
// http://en.wikipedia.org/wiki/Fast_inverse_square_root
//
STATIC mp_obj_t invsqrt_invSqrt1(mp_obj_t x_obj) {
mp_float_t x = mp_obj_get_float(x_obj);
mp_float_t ret_val;

float halfx = 0.5f * x;
float y = x;
long i = *(long*)&y;
i = 0x5f3759df - (i>>1);
y = *(float*)&i;
long i = *(long *)&y;
i = 0x5f3759df - (i >> 1);
y = *(float *)&i;
ret_val = y * (1.5f - (halfx * y * y));

return mp_obj_new_float(ret_val);
Expand All @@ -83,17 +83,17 @@ MP_DEFINE_CONST_FUN_OBJ_1(invsqrt_invSqrt1_obj, invsqrt_invSqrt1);

#if USE_VERSION == 2
//
//A Modification of the Fast Inverse SquareRoot Algorithm
//https://www.researchgate.net/publication/335235981_A_Modification_of_the_Fast_Inverse_Square_Root_Algorithm
// A Modification of the Fast Inverse SquareRoot Algorithm
// https://www.researchgate.net/publication/335235981_A_Modification_of_the_Fast_Inverse_Square_Root_Algorithm
//
STATIC mp_obj_t invsqrt_invSqrt2(mp_obj_t x_obj) {
mp_float_t x = mp_obj_get_float(x_obj);
mp_float_t ret_val;

float simhalfnumber = 0.500438180f * x;
int i = *(int*) &x;
i = 0x5F375A86 - (i>>1);
float y = *(float*) &i;
int i = *(int *)&x;
i = 0x5F375A86 - (i >> 1);
float y = *(float *)&i;
y = y * (1.50131454f - simhalfnumber * y * y);
ret_val = y * (1.50000086f - 0.999124984f * simhalfnumber * y * y);

Expand All @@ -104,15 +104,15 @@ MP_DEFINE_CONST_FUN_OBJ_1(invsqrt_invSqrt2_obj, invsqrt_invSqrt2);

#if USE_VERSION == 3
//
//Fast Inverse Square Root. Pizer's Weblog
//https://pizer.wordpress.com/2008/10/12/fast-inverse-square-root/
// Fast Inverse Square Root. Pizer's Weblog
// https://pizer.wordpress.com/2008/10/12/fast-inverse-square-root/
//
STATIC mp_obj_t invsqrt_invSqrt3(mp_obj_t x_obj) {
mp_float_t x = mp_obj_get_float(x_obj);
mp_float_t ret_val;

uint32_t i = 0x5F1F1412 - (*(uint32_t*)&x >> 1);
float tmp = *(float*)&i;
uint32_t i = 0x5F1F1412 - (*(uint32_t *)&x >> 1);
float tmp = *(float *)&i;
ret_val = tmp * (1.69000231f - 0.714158168f * x * tmp * tmp);

return mp_obj_new_float(ret_val);
Expand All @@ -137,7 +137,7 @@ STATIC const mp_rom_map_elem_t invsqrt_module_globals_table[] = {
STATIC MP_DEFINE_CONST_DICT(invsqrt_module_globals, invsqrt_module_globals_table);
const mp_obj_module_t invsqrt_user_cmodule = {
.base = {&mp_type_module},
.globals = (mp_obj_dict_t*)&invsqrt_module_globals,
.globals = (mp_obj_dict_t *)&invsqrt_module_globals,
};

MP_REGISTER_MODULE(MP_QSTR_invsqrt, invsqrt_user_cmodule);
58 changes: 28 additions & 30 deletions ports/esp32/cmodules/mahony/mahony.c
100755 → 100644
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,35 +36,35 @@ Pizer's Weblog
#include "py/runtime.h"
#include "py/builtin.h"

//---------------------------------------------------------------------------------------------------
// ---------------------------------------------------------------------------------------------------
// Header files

#include <math.h>
#include "../invsqrt/invsqrt.h"

//---------------------------------------------------------------------------------------------------
// ---------------------------------------------------------------------------------------------------
// Definitions

//#define sampleFreq 512.0f // sample frequency in Hz
// #define sampleFreq 512.0f // sample frequency in Hz
#define twoKpDef (2.0f * 0.5f) // 2 * proportional gain
#define twoKiDef (2.0f * 0.0f) // 2 * integral gain

//---------------------------------------------------------------------------------------------------
// ---------------------------------------------------------------------------------------------------
// Variable definitions

static volatile mp_float_t twoKp = twoKpDef; // 2 * proportional gain (Kp)
static volatile mp_float_t twoKi = twoKiDef; // 2 * integral gain (Ki)
static volatile mp_float_t q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; // quaternion of sensor frame relative to auxiliary frame
static volatile mp_float_t integralFBx = 0.0f, integralFBy = 0.0f, integralFBz = 0.0f; // integral error terms scaled by Ki

//---------------------------------------------------------------------------------------------------
// ---------------------------------------------------------------------------------------------------
// Function declarations
STATIC mp_obj_t mahony_MahonyAHRSupdateIMU(size_t n_args, const mp_obj_t *args);

//---------------------------------------------------------------------------------------------------
// ---------------------------------------------------------------------------------------------------
// Attitude and heading reference system (AHRS) algorithm update
//
//def MahonyAHRSupdate(gx : float, gy : float, gz : float, ax : float, ay : float, az : float, mx : float, my : float, mz : float, delta_t_us:int) -> None:
// def MahonyAHRSupdate(gx : float, gy : float, gz : float, ax : float, ay : float, az : float, mx : float, my : float, mz : float, delta_t_us:int) -> None:
//
STATIC mp_obj_t mahony_MahonyAHRSupdate(size_t n_args, const mp_obj_t *args) {
mp_float_t gx = mp_obj_get_float(args[0]);
Expand All @@ -88,12 +88,12 @@ STATIC mp_obj_t mahony_MahonyAHRSupdate(size_t n_args, const mp_obj_t *args) {
mp_float_t qa, qb, qc;

// Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
if ((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
return mahony_MahonyAHRSupdateIMU(6, args);
}

// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {

// Normalise accelerometer measurement
recipNorm = invSqrt(ax * ax + ay * ay + az * az);
Expand Down Expand Up @@ -139,15 +139,14 @@ STATIC mp_obj_t mahony_MahonyAHRSupdate(size_t n_args, const mp_obj_t *args) {
halfez = (ax * halfvy - ay * halfvx) + (mx * halfwy - my * halfwx);

// Compute and apply integral feedback if enabled
if(twoKi > 0.0f) {
if (twoKi > 0.0f) {
integralFBx += twoKi * halfex * delta_t_s; // integral error scaled by Ki
integralFBy += twoKi * halfey * delta_t_s;
integralFBz += twoKi * halfez * delta_t_s;
gx += integralFBx; // apply integral feedback
gy += integralFBy;
gz += integralFBz;
}
else {
} else {
integralFBx = 0.0f; // prevent integral windup
integralFBy = 0.0f;
integralFBz = 0.0f;
Expand Down Expand Up @@ -191,10 +190,10 @@ STATIC mp_obj_t mahony_MahonyAHRSupdate(size_t n_args, const mp_obj_t *args) {
}
MP_DEFINE_CONST_FUN_OBJ_VAR_BETWEEN(mahony_MahonyAHRSupdate_obj, 10, 10, mahony_MahonyAHRSupdate);

//---------------------------------------------------------------------------------------------------
// ---------------------------------------------------------------------------------------------------
// Inertial measurement unit (IMU) algorithm update
//
//def MahonyAHRSupdateIMU(gx : float, gy : float, gz : float, ax : float, ay : float, az : float, delta_t_us:int) -> None:
// def MahonyAHRSupdateIMU(gx : float, gy : float, gz : float, ax : float, ay : float, az : float, delta_t_us:int) -> None:
//
STATIC mp_obj_t mahony_MahonyAHRSupdateIMU(size_t n_args, const mp_obj_t *args) {
mp_float_t gx = mp_obj_get_float(args[0]);
Expand All @@ -213,7 +212,7 @@ STATIC mp_obj_t mahony_MahonyAHRSupdateIMU(size_t n_args, const mp_obj_t *args)
mp_float_t qa, qb, qc;

// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {

// Normalise accelerometer measurement
recipNorm = invSqrt(ax * ax + ay * ay + az * az);
Expand All @@ -232,15 +231,14 @@ STATIC mp_obj_t mahony_MahonyAHRSupdateIMU(size_t n_args, const mp_obj_t *args)
halfez = (ax * halfvy - ay * halfvx);

// Compute and apply integral feedback if enabled
if(twoKi > 0.0f) {
if (twoKi > 0.0f) {
integralFBx += twoKi * halfex * delta_t_s; // integral error scaled by Ki
integralFBy += twoKi * halfey * delta_t_s;
integralFBz += twoKi * halfez * delta_t_s;
gx += integralFBx; // apply integral feedback
gy += integralFBy;
gz += integralFBz;
}
else {
} else {
integralFBx = 0.0f; // prevent integral windup
integralFBy = 0.0f;
integralFBz = 0.0f;
Expand Down Expand Up @@ -287,9 +285,9 @@ MP_DEFINE_CONST_FUN_OBJ_VAR_BETWEEN(mahony_MahonyAHRSupdateIMU_obj, 7, 7, mahony
// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
#define ROLL mp_float_t roll = atan2(q0 * q1 + q2 * q3, 0.5f - q1 * q1 - q2 * q2) * 57.295779513f; // degrees
#define PITCH mp_float_t pitch = asin(2.0f * (q0 * q2 - q3 * q1)) * 57.295779513f; // degrees
#define YAW mp_float_t yaw = atan2(q0 * q3 + q1 * q2, 0.5f - q2 * q2 - q3 * q3) * 57.295779513f; // degrees
#define YAW mp_float_t yaw = atan2(q0 * q3 + q1 * q2, 0.5f - q2 * q2 - q3 * q3) * 57.295779513f; // degrees
//
//def angles()
// def angles()
//
STATIC mp_obj_t mahony_angles() {
// roll(X), pitch(Y), yaw(Z)
Expand Down Expand Up @@ -377,9 +375,9 @@ STATIC mp_obj_t angles_to_quaternion(mp_obj_t _roll, mp_obj_t _pitch, mp_obj_t _
}
STATIC MP_DEFINE_CONST_FUN_OBJ_3(angles_to_quaternion_obj, angles_to_quaternion);

//---------------------------------------------------------------------------------------------------
// ---------------------------------------------------------------------------------------------------
//
//def get_twoKp() -> float:
// def get_twoKp() -> float:
//
STATIC mp_obj_t mahony_get_twoKp() {
mp_float_t ret_val;
Expand All @@ -391,7 +389,7 @@ STATIC mp_obj_t mahony_get_twoKp() {
MP_DEFINE_CONST_FUN_OBJ_0(mahony_get_twoKp_obj, mahony_get_twoKp);

//
//def get_twoKi() -> float:
// def get_twoKi() -> float:
//
STATIC mp_obj_t mahony_get_twoKi() {
mp_float_t ret_val;
Expand All @@ -403,7 +401,7 @@ STATIC mp_obj_t mahony_get_twoKi() {
MP_DEFINE_CONST_FUN_OBJ_0(mahony_get_twoKi_obj, mahony_get_twoKi);

//
//def set_twoKi(twoKi : float) -> None:
// def set_twoKi(twoKi : float) -> None:
//
STATIC mp_obj_t mahony_set_twoKi(mp_obj_t twoKi_obj) {
mp_float_t _twoKi = mp_obj_get_float(twoKi_obj);
Expand All @@ -415,7 +413,7 @@ STATIC mp_obj_t mahony_set_twoKi(mp_obj_t twoKi_obj) {
MP_DEFINE_CONST_FUN_OBJ_1(mahony_set_twoKi_obj, mahony_set_twoKi);

//
//def set_twoKp(twoKp : float) -> None:
// def set_twoKp(twoKp : float) -> None:
//
STATIC mp_obj_t mahony_set_twoKp(mp_obj_t twoKp_obj) {
mp_float_t _twoKp = mp_obj_get_float(twoKp_obj);
Expand All @@ -430,10 +428,10 @@ STATIC const mp_rom_map_elem_t mahony_module_globals_table[] = {
{ MP_ROM_QSTR(MP_QSTR___name__), MP_ROM_QSTR(MP_QSTR_mahony) },
{ MP_ROM_QSTR(MP_QSTR_MahonyAHRSupdate), MP_ROM_PTR(&mahony_MahonyAHRSupdate_obj) },
{ MP_ROM_QSTR(MP_QSTR_MahonyAHRSupdateIMU), MP_ROM_PTR(&mahony_MahonyAHRSupdateIMU_obj) },
{ MP_ROM_QSTR(MP_QSTR_Mahony_angles), MP_ROM_PTR(&mahony_angles_obj) },
{ MP_ROM_QSTR(MP_QSTR_Mahony_roll), MP_ROM_PTR(&mahony_roll_obj) },
{ MP_ROM_QSTR(MP_QSTR_Mahony_pitch), MP_ROM_PTR(&mahony_pitch_obj) },
{ MP_ROM_QSTR(MP_QSTR_Mahony_yaw), MP_ROM_PTR(&mahony_yaw_obj) },
{ MP_ROM_QSTR(MP_QSTR_Mahony_angles), MP_ROM_PTR(&mahony_angles_obj) },
{ MP_ROM_QSTR(MP_QSTR_Mahony_roll), MP_ROM_PTR(&mahony_roll_obj) },
{ MP_ROM_QSTR(MP_QSTR_Mahony_pitch), MP_ROM_PTR(&mahony_pitch_obj) },
{ MP_ROM_QSTR(MP_QSTR_Mahony_yaw), MP_ROM_PTR(&mahony_yaw_obj) },
{ MP_ROM_QSTR(MP_QSTR_get_twoKp), MP_ROM_PTR(&mahony_get_twoKp_obj) },
{ MP_ROM_QSTR(MP_QSTR_get_twoKi), MP_ROM_PTR(&mahony_get_twoKi_obj) },
{ MP_ROM_QSTR(MP_QSTR_set_twoKi), MP_ROM_PTR(&mahony_set_twoKi_obj) },
Expand All @@ -445,7 +443,7 @@ STATIC const mp_rom_map_elem_t mahony_module_globals_table[] = {
STATIC MP_DEFINE_CONST_DICT(mahony_module_globals, mahony_module_globals_table);
const mp_obj_module_t mahony_user_cmodule = {
.base = {&mp_type_module},
.globals = (mp_obj_dict_t*)&mahony_module_globals,
.globals = (mp_obj_dict_t *)&mahony_module_globals,
};

MP_REGISTER_MODULE(MP_QSTR_mahony, mahony_user_cmodule);
4 changes: 2 additions & 2 deletions ports/esp32/machine_encoder.c
100755 → 100644
View file
Original file line number Diff line number Diff line change
Expand Up @@ -108,13 +108,13 @@ STATIC void IRAM_ATTR pcnt_intr_handler(void *arg) {
}
if (status_unit & PCNT_EVT_ZERO) {
if (self->counter == 0) {
//self->status |= PCNT_EVT_ZERO;
// self->status |= PCNT_EVT_ZERO;
mp_sched_schedule(self->handler_zero, MP_OBJ_FROM_PTR(self));
mp_hal_wake_main_task_from_isr();
}
}
}
//PCNT.int_clr.val = BIT(id); // clear the interrupt
// PCNT.int_clr.val = BIT(id); // clear the interrupt
}
}
}
Expand Down

0 comments on commit 8ec9436

Please sign in to comment.