void imu_update(imu_t *imu) { uint32_t t = time_keeper_get_time_ticks(); imu->dt = time_keeper_ticks_to_seconds(t - imu->last_update); imu->last_update = t; imu_raw2oriented(imu); imu_oriented2scale(imu); }
float pid_controller_update(pid_controller_t* controller, float error) { uint32_t t = time_keeper_get_time_ticks(); controller->error = maths_soft_zone(error, controller->soft_zone_width); controller->dt = time_keeper_ticks_to_seconds(t - controller->last_update); controller->last_update = t; controller->output = controller->p_gain * controller->error + pid_controller_integrate( &controller->integrator, controller->error, controller->dt) + pid_controller_differentiate(&controller->differentiator, controller->error, controller->dt); if( controller->output < controller->clip_min ) { controller->output = controller->clip_min; } if( controller->output > controller->clip_max ) { controller->output=controller->clip_max; } return controller->output; }
double time_keeper_get_time() { // time in seconds since system start return time_keeper_ticks_to_seconds(time_keeper_get_time_ticks()); }