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);
}
void remote_update(remote_t* remote)
{
uint32_t now = time_keeper_get_time_ticks() ;
float raw;
if ( remote->sat->new_data_available == true )
{
// Check signal quality
if ( remote->sat->dt < 100000)
{
// ok
remote->signal_quality = SIGNAL_GOOD;
}
else if ( remote->sat->dt < 1500000 )
{
// warning
remote->signal_quality = SIGNAL_BAD;
}
// Retrieve and scale channels
for (uint8_t i = 0; i < REMOTE_CHANNEL_COUNT; ++i)
{
raw = remote->sat->channels[i];
if ( raw < remote->deadzone && raw > -remote->deadzone )
{
remote->channels[i] = 0.0f;
}
else
{
remote->channels[i] = raw * remote->scale * remote->channel_inv[i] - remote->trims[i];
}
}
// Indicate that data was handled
remote->sat->new_data_available = false;
}
else
{
// Check for signal loss
if ( ( now - remote->sat->last_update ) > 1500000 )
{
// CRITICAL: Set all channels to failsafe
remote->channels[CHANNEL_THROTTLE] = -1.0f;
for (uint8_t i = 1; i < REMOTE_CHANNEL_COUNT; i++)
{
remote->channels[i] = 0.0f;
}
remote->signal_quality = SIGNAL_LOST;
}
}
}
void imu_init (imu_t *imu, const mavlink_stream_t* mavlink_stream)
{
imu->mavlink_stream = mavlink_stream;
//imu_calibrate_Gyros(imu);
//init gyro
imu->calib_gyro.scale_factor[X] = 1.0f / RAW_GYRO_X_SCALE;
imu->calib_gyro.scale_factor[Y] = 1.0f / RAW_GYRO_Y_SCALE;
imu->calib_gyro.scale_factor[Z] = 1.0f / RAW_GYRO_Z_SCALE;
imu->calib_gyro.bias[X] = GYRO_BIAIS_X;
imu->calib_gyro.bias[Y] = GYRO_BIAIS_Y;
imu->calib_gyro.bias[Z] = GYRO_BIAIS_Z;
imu->calib_gyro.orientation[X] = GYRO_ORIENTATION_X;
imu->calib_gyro.orientation[Y] = GYRO_ORIENTATION_Y;
imu->calib_gyro.orientation[Z] = GYRO_ORIENTATION_Z;
imu->calib_gyro.axis[X] = GYRO_AXIS_X;
imu->calib_gyro.axis[Y] = GYRO_AXIS_Y;
imu->calib_gyro.axis[Z] = GYRO_AXIS_Z;
//init accelero
imu->calib_accelero.scale_factor[X] = 1.0f / RAW_ACC_X_SCALE;
imu->calib_accelero.scale_factor[Y] = 1.0f / RAW_ACC_Y_SCALE;
imu->calib_accelero.scale_factor[Z] = 1.0f / RAW_ACC_Z_SCALE;
imu->calib_accelero.bias[X] = ACC_BIAIS_X;
imu->calib_accelero.bias[Y] = ACC_BIAIS_Y;
imu->calib_accelero.bias[Z] = ACC_BIAIS_Z;
imu->calib_accelero.orientation[X] = ACC_ORIENTATION_X;
imu->calib_accelero.orientation[Y] = ACC_ORIENTATION_Y;
imu->calib_accelero.orientation[Z] = ACC_ORIENTATION_Z;
imu->calib_accelero.axis[X] = ACC_AXIS_X;
imu->calib_accelero.axis[Y] = ACC_AXIS_Y;
imu->calib_accelero.axis[Z] = ACC_AXIS_Z;
//init compass
imu->calib_compass.scale_factor[X] = 1.0f / RAW_MAG_X_SCALE;
imu->calib_compass.scale_factor[Y] = 1.0f / RAW_MAG_Y_SCALE;
imu->calib_compass.scale_factor[Z] = 1.0f / RAW_MAG_Z_SCALE;
imu->calib_compass.bias[X] = MAG_BIAIS_X;
imu->calib_compass.bias[Y] = MAG_BIAIS_Y;
imu->calib_compass.bias[Z] = MAG_BIAIS_Z;
imu->calib_compass.orientation[X] = MAG_ORIENTATION_X;
imu->calib_compass.orientation[Y] = MAG_ORIENTATION_Y;
imu->calib_compass.orientation[Z] = MAG_ORIENTATION_Z;
imu->calib_compass.axis[X] = MAG_AXIS_X;
imu->calib_compass.axis[Y] = MAG_AXIS_Y;
imu->calib_compass.axis[Z] = MAG_AXIS_Z;
imu->last_update = time_keeper_get_time_ticks();
imu->dt = 0.004;
}
void pid_controller_init(pid_controller_t* controller, const pid_controller_conf_t* config)
{
uint32_t t = time_keeper_get_time_ticks();
controller->p_gain = config->p_gain;
controller->clip_min = config->clip_min;
controller->clip_max = config->clip_max;
controller->integrator = config->integrator;
controller->differentiator = config->differentiator;
controller->soft_zone_width = config->soft_zone_width;
controller->output = 0.0f;
controller->error = 0.0f;
controller->last_update = t;
controller->dt = 0.0f;
}
void pid_controller_init_pass_through(pid_controller_t* controller)
{
uint32_t t = time_keeper_get_time_ticks();
controller->dt = 0.0f;
controller->last_update = t;
controller->p_gain = 1.0f;
controller->clip_min = -10000.0f;
controller->clip_max = 10000.0f;
controller->error = 0.0f;
controller->output = 0.0f;
controller->soft_zone_width = 0.0f;
controller->differentiator.previous = 0.0f;
pid_controller_init_differenciator( &(controller->differentiator),
0.0f, 0.0f);
pid_controller_init_integrator( &(controller->integrator),
0.0f, 0.0f, 0.0f);
}
float pid_controller_update_dt(pid_controller_t* controller, float error, float dt)
{
controller->error = error;
controller->dt = dt;
controller->last_update = time_keeper_get_time_ticks();
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;
}
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;
}
void imu_init (imu_t *imu, state_t* state, const mavlink_stream_t* mavlink_stream, mavlink_communication_t* mavlink_communication)
{
imu->mavlink_stream = mavlink_stream;
imu->state = state;
//imu_calibrate_Gyros(imu);
//init gyro
imu->calib_gyro.scale_factor[X] = 1.0f / RAW_GYRO_X_SCALE;
imu->calib_gyro.scale_factor[Y] = 1.0f / RAW_GYRO_Y_SCALE;
imu->calib_gyro.scale_factor[Z] = 1.0f / RAW_GYRO_Z_SCALE;
imu->calib_gyro.bias[X] = GYRO_BIAIS_X;
imu->calib_gyro.bias[Y] = GYRO_BIAIS_Y;
imu->calib_gyro.bias[Z] = GYRO_BIAIS_Z;
imu->calib_gyro.orientation[X] = GYRO_ORIENTATION_X;
imu->calib_gyro.orientation[Y] = GYRO_ORIENTATION_Y;
imu->calib_gyro.orientation[Z] = GYRO_ORIENTATION_Z;
imu->calib_gyro.axis[X] = GYRO_AXIS_X;
imu->calib_gyro.axis[Y] = GYRO_AXIS_Y;
imu->calib_gyro.axis[Z] = GYRO_AXIS_Z;
imu->calib_gyro.max_oriented_values[X] = -10000.0f;
imu->calib_gyro.max_oriented_values[Y] = -10000.0f;
imu->calib_gyro.max_oriented_values[Z] = -10000.0f;
imu->calib_gyro.min_oriented_values[X] = 10000.0f;
imu->calib_gyro.min_oriented_values[Y] = 10000.0f;
imu->calib_gyro.min_oriented_values[Z] = 10000.0f;
imu->calib_gyro.calibration = false;
//init accelero
imu->calib_accelero.scale_factor[X] = 1.0f / RAW_ACC_X_SCALE;
imu->calib_accelero.scale_factor[Y] = 1.0f / RAW_ACC_Y_SCALE;
imu->calib_accelero.scale_factor[Z] = 1.0f / RAW_ACC_Z_SCALE;
imu->calib_accelero.bias[X] = ACC_BIAIS_X;
imu->calib_accelero.bias[Y] = ACC_BIAIS_Y;
imu->calib_accelero.bias[Z] = ACC_BIAIS_Z;
imu->calib_accelero.orientation[X] = ACC_ORIENTATION_X;
imu->calib_accelero.orientation[Y] = ACC_ORIENTATION_Y;
imu->calib_accelero.orientation[Z] = ACC_ORIENTATION_Z;
imu->calib_accelero.axis[X] = ACC_AXIS_X;
imu->calib_accelero.axis[Y] = ACC_AXIS_Y;
imu->calib_accelero.axis[Z] = ACC_AXIS_Z;
imu->calib_accelero.max_oriented_values[X] = -10000.0f;
imu->calib_accelero.max_oriented_values[Y] = -10000.0f;
imu->calib_accelero.max_oriented_values[Z] = -10000.0f;
imu->calib_accelero.min_oriented_values[X] = 10000.0f;
imu->calib_accelero.min_oriented_values[Y] = 10000.0f;
imu->calib_accelero.min_oriented_values[Z] = 10000.0f;
imu->calib_accelero.calibration = false;
//init compass
imu->calib_compass.scale_factor[X] = 1.0f / RAW_MAG_X_SCALE;
imu->calib_compass.scale_factor[Y] = 1.0f / RAW_MAG_Y_SCALE;
imu->calib_compass.scale_factor[Z] = 1.0f / RAW_MAG_Z_SCALE;
imu->calib_compass.bias[X] = MAG_BIAIS_X;
imu->calib_compass.bias[Y] = MAG_BIAIS_Y;
imu->calib_compass.bias[Z] = MAG_BIAIS_Z;
imu->calib_compass.orientation[X] = MAG_ORIENTATION_X;
imu->calib_compass.orientation[Y] = MAG_ORIENTATION_Y;
imu->calib_compass.orientation[Z] = MAG_ORIENTATION_Z;
imu->calib_compass.axis[X] = MAG_AXIS_X;
imu->calib_compass.axis[Y] = MAG_AXIS_Y;
imu->calib_compass.axis[Z] = MAG_AXIS_Z;
imu->calib_compass.max_oriented_values[X] = -10000.0f;
imu->calib_compass.max_oriented_values[Y] = -10000.0f;
imu->calib_compass.max_oriented_values[Z] = -10000.0f;
imu->calib_compass.min_oriented_values[X] = 10000.0f;
imu->calib_compass.min_oriented_values[Y] = 10000.0f;
imu->calib_compass.min_oriented_values[Z] = 10000.0f;
imu->calib_compass.calibration = false;
// Add callbacks for waypoint handler commands requests
mavlink_message_handler_cmd_callback_t callbackcmd;
callbackcmd.command_id = MAV_CMD_PREFLIGHT_CALIBRATION; // 241
callbackcmd.sysid_filter = MAVLINK_BASE_STATION_ID;
callbackcmd.compid_filter = MAV_COMP_ID_ALL;
callbackcmd.compid_target = MAV_COMP_ID_ALL; // 0
callbackcmd.function = (mavlink_cmd_callback_function_t) &imu_start_calibration;
callbackcmd.module_struct = imu;
mavlink_message_handler_add_cmd_callback(&mavlink_communication->message_handler, &callbackcmd);
imu->last_update = time_keeper_get_time_ticks();
imu->dt = 0.004;
print_util_dbg_print("[IMU] Initialisation\r\n");
}
fuzzy_controller_t fuzzy_control_init(float Kp, float Ki, float Kd, float normI, float normO, float limO)
{
// generate the initial table
float error_centers[] = {-1, -0.5, 0, 0.5, 1}; //TODO
float d_error_centers[] = {-1, -0.5, 0, 0.5, 1};
float dd_error_centers[] = { -1, -0.5, 0, 0.5, 1};
float*** table_PID = (float***) malloc(N_MF*sizeof(float**));
for (int i = 0; i < N_MF; i++)
{
table_PID[i] = (float**) malloc(N_MF*sizeof(float*));
}
for (int i = 0; i<N_MF; i++) {
for (int j = 0; j<N_MF; j++) {
table_PID[i][j] = (float*) malloc(N_MF*sizeof(float));
}
}
generateTablePID(error_centers, d_error_centers, dd_error_centers, Kp, Kd, Ki, table_PID); // -> n x n x n matrix
// create here the membership functions
triangularMF_t** MF;
MF = (triangularMF_t**) malloc(N_MF*sizeof(triangularMF_t*));
for (int i = 0; i < N_MF; i++)
{
MF[i] = (triangularMF_t*) malloc(N_MF*sizeof(triangularMF_t));
}
// declare the fuzzy controller
fuzzy_controller_t fuzzy_controller;
fuzzy_controller = (fuzzy_controller_t) {
.Kp = Kp,
.Ki = Ki,
.Kd = Kd,
.MF = MF,
.rule_table_PID = table_PID,
.output = 0,
.error = 0,
.last_error = 0,
.last_update = 0,
.dt = 0,
.last_output = 0,
.dt=1,
.norm_error_input = normI,
.norm_error_output = normO,
.lim_output = limO
};
// MF1
// error
MF[0][0].C = -1;
MF[0][0].L = -1.5;
MF[0][0].R = -0.5;
MF[0][0].ling_var = NL;
// d_error
MF[1][0].C = -1;
MF[1][0].L = -1.5;
MF[1][0].R = -0.5;
MF[1][0].ling_var = NL;
// dd_error
MF[2][0].C = -1;
MF[2][0].L = -1.5;
MF[2][0].R = -0.;
MF[2][0].ling_var = NL;
// MF2
// error
MF[0][1].C = -0.5;
MF[0][1].L = -1;
MF[0][1].R = 0;
MF[0][1].ling_var = NS;
// d_error
MF[1][1].C = -0.5;
MF[1][1].L = -1;
MF[1][1].R = 0;
MF[1][1].ling_var = NS;
// dd_1rror
MF[2][1].C = -0.5;
MF[2][1].L = -1;
MF[2][1].R = 0;
MF[2][1].ling_var = NS;
// MF3
// error
MF[0][2].C = 0;
MF[0][2].L = -0.5;
MF[0][2].R = 0.5;
MF[0][2].ling_var = ZZ;
// d_error
MF[1][2].C = 0;
MF[1][2].L = -0.5;
MF[1][2].R = 0.5;
MF[1][2].ling_var = ZZ;
// dd_1rror
MF[2][2].C = 0;
MF[2][2].L = -0.5;
MF[2][2].R = 0.5;
MF[2][2].ling_var = ZZ;
// MF4
// error
MF[0][3].C = 0.5;
MF[0][3].L = 0;
MF[0][3].R = 1;
MF[0][3].ling_var = PS;
// d_error
MF[1][3].C = 0.5;
MF[1][3].L = 0;
MF[1][3].R = 1;
MF[1][3].ling_var = PS;
// dd_error
MF[2][3].C = 0.5;
MF[2][3].L = 0;
MF[2][3].R = 1;
MF[2][3].ling_var = PS;
// MF5
// error
MF[0][4].C = 1;
MF[0][4].L = 0.5;
MF[0][4].R = 1.5;
MF[0][4].ling_var = PL;
// d_error
MF[1][4].C = 1;
MF[1][4].L = 0.5;
MF[1][4].R = 1.5;
MF[1][4].ling_var = PL;
// dd_error
MF[2][4].C = 1;
MF[2][4].L = 0.5;
MF[2][4].R = 1.5;
MF[2][4].ling_var = PL;
return fuzzy_controller;
}
float fuzzy_control_update(fuzzy_controller_t* controller, float error, float dt)
{
controller->last_update=time_keeper_get_time_ticks();
controller->dt = dt;
/* DONE: calculate d_error and dd_error */
float d_error = error - controller->last_error;
float dd_error = d_error - controller->last_d_error;
controller->last_error = error;
controller->last_d_error = d_error;
error = error/err_norm;
d_error = d_error/d_err_norm;
if(d_error > d_err_norm)
d_error = d_err_norm;
if(d_error < -d_err_norm)
d_error = -d_err_norm;
// calculate the membership degree
float error_centers[N_MF];
float d_error_centers[N_MF];
float dd_error_centers[N_MF];
calculateMFdegree(controller->MF[0], error, error_centers);
calculateMFdegree(controller->MF[1], d_error, d_error_centers);
calculateMFdegree(controller->MF[2], dd_error, dd_error_centers);
// recalculate rule table
float peaks[] = { -1,-0.5, 0, 0.5, 1};
generateTablePID(peaks, peaks, peaks, controller->Kp, controller->Kd/dt, controller->Ki*dt, controller->rule_table_PID);
// activate the triggered rules
float sum_output = 0;
float sum_activated = 0;
for (int i = 0; i<N_MF; i++) {
for (int j = 0; j<N_MF; j++) {
for (int k = 0; k<N_MF; k++) {
sum_output += controller->rule_table_PID[i][j][k]*fminf(fminf(error_centers[i],d_error_centers[j]),dd_error_centers[k]); // i = 0, j = 0, IF error_NL AND derror_NL THEN
sum_activated += fminf(fminf(error_centers[i],d_error_centers[j]),dd_error_centers[k]);
}
}
}
/*
// activate the triggered rules
float sum_output = 0;
float sum_activated = 0;
for (int i = 0; i<N_MF; i++) {
for (int j = 0; j<N_MF; j++) {
for (int k=0; k<N_MF; k++) {
sum_output += controller->rule_table_PID[i][j][k]*fminf(fminf(error_centers[i],d_error_centers[j]),dd_error_centers[k]);
sum_activated += fminf(fminf(error_centers[i],d_error_centers[j]),dd_error_centers[k]);
}
}
}*/
// calculate output delta_output = sum(triggered_singleton * mem_degree) / sum(mem_degree)
float delta_output = sum_output/sum_activated;
// return output = output_prev + delta_output
float output = delta_output + controller->last_output;
// scale normalized output
output *= controller->norm_error_output;
// limit on the output
if (output > controller->lim_output)
output = controller->lim_output;
else if (output < -controller->lim_output)
output = -controller->lim_output;
// save the last normalized output
controller->last_output = output/controller->norm_error_output;
return output;
}
uint32_t time_keeper_get_micros()
{
// microseconds since system start. Will run over after an hour.
return time_keeper_get_time_ticks() * (1000000 / TK_AST_FREQUENCY);
}
uint32_t time_keeper_get_millis()
{
//milliseconds since system start
return time_keeper_get_time_ticks() / 1000; /// (TK_AST_FREQUENCY / 1000);
}
double time_keeper_get_time()
{
// time in seconds since system start
return time_keeper_ticks_to_seconds(time_keeper_get_time_ticks());
}
static void process_data(void)
{
int32_t ch;
volatile int16_t value;
if (sample_counter>=number_of_samples)
{
if (continuous_mode)
{
sample_counter=0;
oversampling_counter=0;
} else
{
adcifa_disable_interrupt(adcifa, ADC_INT_SEOS0);
//adcifa_disable_interrupt(adcifa, ADC_INT_SEOS1);
adcifa_stop_itimer(adcifa);
}
}
else
{
if ((adcifa->sr&ADC_INT_SEOS0) ==0)
{}
//|| ((adcifa->sr&ADC_INT_SEOS1) ==0) ) {}
else
{
adc_int_period=(time_keeper_get_time_ticks() - last_adc_int_time);
last_adc_int_time=time_keeper_get_time_ticks();
if (sample_counter>=0)
{
if (oversampling_counter<=0)
{
for (ch=0; ch<sequencer_item_count; ch++)
{
value=adcifa->resx[ch];
internal_buffer[ch]= value ;
}
}
else
{
for (ch=0; ch<sequencer_item_count; ch++)
{
value=adcifa->resx[ch];
internal_buffer[ch]+= value ;
//adci_buffer[ch][even_odd][sample_counter]+=value;
}
}
}
else
{
sample_counter++;
return;
}
//if (function_generator!= NULL) {
// dac_dma_set_value((*function_generator)(sample_counter));
//}
oversampling_counter++;
if (oversampling_counter >= oversampling)
{
oversampling_counter=0;
for (ch=0; ch<channel_count; ch++)
{
int16_t *buffer = adci_buffer[ch];
buffer[sample_counter] = internal_buffer[ch] / oversampling_divider;
}
sample_counter++;
}
//dac_dma_set_value(even_odd * 400);
///< acknowledge processing finished
adcifa->scr=ADC_INT_SEOS0 | ADC_INT_SEOS1;
}
}
}
