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; } } }