nils/N17BLDC
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

move vel and acc calc to comm task

Browse Source
This commit is contained in:
Nils Schulte 2025-07-19 22:08:45 +02:00
parent dc4cd48224
commit 67609d7b31
2 changed files with 24 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
fw/src/main.cpp
View File

@ -129,7 +129,7 @@ void write_dual_i2c(uint8_t addr, uint8_t reg_lsb, uint8_t val1, uint8_t val2) {
SemaphoreHandle_t mutex;
struct UdralServoState servo_state = {0};
void mutexed_state_sync(UdralServoState *const state) {
void mutexed_state_sync(UdralServoState *const state, const float hz) {
xSemaphoreTake(mutex, portMAX_DELAY);
servo_state.target_force = state->target_force;
servo_state.target_acceleration = state->target_acceleration;
@ -137,6 +137,19 @@ void mutexed_state_sync(UdralServoState *const state) {
servo_state.target_position = state->target_position;
servo_state.arming = state->arming;
servo_state.pid = state->pid;
float curr_angle = servo_state.curr_position;
static float curr_angle_last = curr_angle;
float curr_vel = (curr_angle - curr_angle_last) * hz;
curr_angle_last = curr_angle;
static float curr_vel_last = curr_vel;
float curr_accel =(curr_vel - curr_vel_last) * hz;
curr_vel_last = curr_vel;
servo_state.curr_acceleration = curr_accel;
servo_state.curr_velocity = curr_vel;
*state = servo_state;
xSemaphoreGive(mutex);
}
@ -273,7 +286,10 @@ void loop() {
#ifdef MOTOR
bool armed = false;
float curr_pcb_temp_kelvin = temp_pcb + 273.15f;
/* adc reading */
drv8323s.focdriver->voltage_power_supply = vdrive_read;
const float curr_pcb_temp_kelvin = temp_pcb + 273.15f;
xSemaphoreTake(mutex, portMAX_DELAY);
armed = servo_state.arming.armed;
float target = servo_state.target_force;
@ -282,25 +298,9 @@ void loop() {
servo_state.motor_temperature = NAN;
float curr_angle = encoder_calibrated.getAngle();
float curr_vel = encoder_calibrated.getVelocity();
unsigned long now = millis();
static unsigned long last_update = now;
unsigned long tdelta = now - last_update;
last_update = now;
float curr_accel =
tdelta ? (curr_vel - servo_state.curr_velocity) / tdelta : 0;
servo_state.curr_acceleration = curr_accel;
servo_state.curr_velocity = curr_vel;
servo_state.curr_position = curr_angle;
servo_state.curr_force = servo_state.target_force;
servo_state.curr_velocity = encoder.getAngle();
servo_state.curr_acceleration = encoder_absolute.getAngle();
servo_state.curr_force = abs_inc_sensor.getAngle();
servo_state.vcc_volts = drv8323s.focdriver->voltage_power_supply;
servo_state.current = NAN;
xSemaphoreGive(mutex);
@ -320,7 +320,6 @@ void loop() {
}
digitalWrite(LED_PIN, armed ? LOW : HIGH);
drv8323s.focdriver->voltage_power_supply = vdrive_read;
motor.loopFOC();
#else

8
fw/src/udral_servo.hpp
View File

@ -100,7 +100,7 @@ struct UdralServoState {
};
};
typedef void (*state_sync_f)(UdralServoState *const);
typedef void (*state_sync_f)(UdralServoState *const, const float hz);
struct UdralServoInternalState {
CanardMicrosecond started_at;
@ -1067,17 +1067,19 @@ int udral_loop(state_sync_f servo_state_sync_f)
// Now the node is initialized and we're ready to roll.
state.started_at = getMonotonicMicroseconds();
const CanardMicrosecond fast_loop_period = MEGA / 50;
const unsigned int hz = 50;
const CanardMicrosecond fast_loop_period = MEGA / hz;
CanardMicrosecond next_fast_iter_at = state.started_at + fast_loop_period;
CanardMicrosecond next_1_hz_iter_at = state.started_at + MEGA;
CanardMicrosecond next_01_hz_iter_at = state.started_at + MEGA * 10;
do
{
state.user_state_sync_f(&state.servo);
vTaskDelay(1);
// Run a trivial scheduler polling the loops that run the business logic.
CanardMicrosecond now_usec = getMonotonicMicroseconds();
if (now_usec >= next_fast_iter_at)
{
state.user_state_sync_f(&state.servo, hz);
next_fast_iter_at += fast_loop_period;
handleFastLoop(&state, now_usec);
}