void simulation_simulate_sonar(simulation_model_t *sim) { int16_t distance_cm = 0.5f - 100 * sim->local_position.pos[Z]; float distance_m = (float)distance_cm / 100.0f; float dt = 0.0f; float velocity = 0.0f; if ( distance_m > sim->sonar->min_distance && distance_m < sim->sonar->max_distance ) { dt = (time_keeper_get_micros() - sim->sonar->last_update)/1000000.0f; velocity = (distance_m - sim->sonar->current_distance)/dt; if (maths_f_abs(velocity) > 20.0f) { velocity = 0.0f; } sim->sonar->current_velocity = LPF_SONAR_VARIO*sim->sonar->current_velocity + (1.0f-LPF_SONAR_VARIO)*velocity; sim->sonar->current_distance = distance_m; sim->sonar->last_update = time_keeper_get_micros(); sim->sonar->healthy = true; sim->sonar->healthy_vel = true; } else { sim->sonar->healthy = false; sim->sonar->current_distance = 0.0f; sim->sonar->healthy_vel = false; sim->sonar->current_velocity = 0.0f; } }
void stabilisation_copter_send_outputs(stabilisation_copter_t* stabilisation_copter, const mavlink_stream_t* mavlink_stream, mavlink_message_t* msg) { aero_attitude_t attitude_yaw_inverse; quat_t q_rot,qtmp; attitude_yaw_inverse = coord_conventions_quat_to_aero(stabilisation_copter->ahrs->qe); attitude_yaw_inverse.rpy[0] = 0.0f; attitude_yaw_inverse.rpy[1] = 0.0f; attitude_yaw_inverse.rpy[2] = attitude_yaw_inverse.rpy[2]; q_rot = coord_conventions_quaternion_from_aero(attitude_yaw_inverse); qtmp=quaternions_create_from_vector(stabilisation_copter->stabiliser_stack.velocity_stabiliser.output.rpy); quat_t rpy_local; quaternions_rotate_vector(quaternions_inverse(q_rot), qtmp.v, rpy_local.v); mavlink_msg_debug_vect_pack( mavlink_stream->sysid, mavlink_stream->compid, msg, "OutVel", time_keeper_get_micros(), -rpy_local.v[X] * 1000, rpy_local.v[Y] * 1000, stabilisation_copter->stabiliser_stack.velocity_stabiliser.output.rpy[YAW] * 1000); mavlink_stream_send(mavlink_stream,msg); mavlink_msg_debug_vect_pack( mavlink_stream->sysid, mavlink_stream->compid, msg, "OutAtt", time_keeper_get_micros(), stabilisation_copter->stabiliser_stack.attitude_stabiliser.output.rpy[ROLL] * 1000, stabilisation_copter->stabiliser_stack.attitude_stabiliser.output.rpy[PITCH] * 1000, stabilisation_copter->stabiliser_stack.attitude_stabiliser.output.rpy[YAW] * 1000); mavlink_stream_send(mavlink_stream,msg); mavlink_msg_debug_vect_pack( mavlink_stream->sysid, mavlink_stream->compid, msg, "OutRate", time_keeper_get_micros(), stabilisation_copter->stabiliser_stack.rate_stabiliser.output.rpy[ROLL] * 1000, stabilisation_copter->stabiliser_stack.rate_stabiliser.output.rpy[PITCH] * 1000, stabilisation_copter->stabiliser_stack.rate_stabiliser.output.rpy[YAW] * 1000); mavlink_stream_send(mavlink_stream,msg); }
void scheduler_run_task_now(task_entry_t *te) { if ((te->run_mode == RUN_NEVER)) { te->run_mode = RUN_ONCE; } te->next_run = time_keeper_get_micros(); }
bool scheduler_add_task(scheduler_t* scheduler, uint32_t repeat_period, task_run_mode_t run_mode, task_timing_mode_t timing_mode, task_priority_t priority, task_function_t call_function, task_argument_t function_argument, uint32_t task_id) { bool task_successfully_added = false; task_set_t* ts = scheduler->task_set; // Check if the scheduler is not full if ( ts->task_count < ts->max_task_count ) { // Check if there is already a task with this ID bool id_is_unique = true; for (uint32_t i = 0; i < ts->task_count; ++i) { if ( ts->tasks[i].task_id == task_id ) { id_is_unique = false; break; } } // Add new task if ( id_is_unique == true ) { task_entry_t* new_task = &ts->tasks[ts->task_count]; new_task->call_function = call_function; new_task->function_argument = function_argument; new_task->task_id = task_id; new_task->run_mode = run_mode; new_task->timing_mode = timing_mode; new_task->priority = priority; new_task->repeat_period = repeat_period; new_task->next_run = time_keeper_get_micros(); new_task->execution_time = 0; new_task->delay_max = 0; new_task->delay_avg = 0; new_task->delay_var_squared = 0; ts->task_count += 1; task_successfully_added = true; } else { print_util_dbg_print("[SCHEDULER] Error: There is already a task with this ID\r\n"); task_successfully_added = false; } } else { print_util_dbg_print("[SCHEDULER] Error: Cannot add more task\r\n"); task_successfully_added = false; } return task_successfully_added; }
void stabilisation_send_command(control_command_t* controls, const mavlink_stream_t* mavlink_stream, mavlink_message_t* msg) { // Controls output mavlink_msg_debug_vect_pack( mavlink_stream->sysid, mavlink_stream->compid, msg, "Controls", time_keeper_get_micros(), controls->tvel[X], controls->tvel[Y], controls->tvel[Z]); }
void acoustic_telemetry_send (const audio_t* audio_data, const mavlink_stream_t* mavlink_stream, mavlink_message_t* msg) { //TODO: create a mavlink message mavlink_msg_debug_vect_pack( mavlink_stream->sysid, mavlink_stream->compid, msg, "Audio", time_keeper_get_micros(), (float)audio_data->azimuth, (float)audio_data->elevation, (float)audio_data->reliabe_data); }
void sonar_i2cxl_get_last_measure(sonar_i2cxl_t* sonar_i2cxl) { uint8_t buf[2]; uint16_t distance_cm = 0; float distance_m = 0.0f; float velocity = 0.0f; float dt = 0.0f; uint32_t time_us = time_keeper_get_micros(); twim_read(&AVR32_TWIM1, buf, 2, sonar_i2cxl->i2c_address, false); distance_cm = (buf[0] << 8) + buf[1]; distance_m = ((float)distance_cm) / 100.0f; if ( distance_m > sonar_i2cxl->data.min_distance && distance_m < sonar_i2cxl->data.max_distance ) { dt = ((float)time_us - sonar_i2cxl->data.last_update)/1000000.0f; if (sonar_i2cxl->data.healthy) { velocity = (distance_m - sonar_i2cxl->data.current_distance) / dt; //discard sonar velocity estimation if it seams too big if (maths_f_abs(velocity) > 20.0f) { velocity = 0.0f; } if (sonar_i2cxl->data.healthy_vel) { sonar_i2cxl->data.current_velocity = (1.0f-LPF_SONAR_VARIO)*sonar_i2cxl->data.current_velocity + LPF_SONAR_VARIO*velocity; } else { sonar_i2cxl->data.current_velocity = velocity; } sonar_i2cxl->data.healthy_vel = true; } sonar_i2cxl->data.current_distance = distance_m; sonar_i2cxl->data.last_update = time_us; sonar_i2cxl->data.healthy = true; } else { sonar_i2cxl->data.current_velocity = 0.0f; sonar_i2cxl->data.healthy = false; sonar_i2cxl->data.healthy_vel = false; } }
task_return_t imu_send_raw(imu_t* imu) { mavlink_message_t msg; mavlink_msg_raw_imu_pack( imu->mavlink_stream->sysid, imu->mavlink_stream->compid, &msg, time_keeper_get_micros(), imu->oriented_accelero.data[IMU_X], imu->oriented_accelero.data[IMU_Y], imu->oriented_accelero.data[IMU_Z], imu->oriented_gyro.data[IMU_X], imu->oriented_gyro.data[IMU_Y], imu->oriented_gyro.data[IMU_Z], imu->oriented_compass.data[IMU_X], imu->oriented_compass.data[IMU_Y], imu->oriented_compass.data[IMU_Z]); mavlink_stream_send(imu->mavlink_stream,&msg); return TASK_RUN_SUCCESS; }
void forces_from_servos_diag_quad(simulation_model_t* sim) { float motor_command[4]; float rotor_lifts[4], rotor_drags[4], rotor_inertia[4]; float ldb; quat_t wind_gf = {.s = 0, .v = {sim->vehicle_config.wind_x, sim->vehicle_config.wind_y, 0.0f}}; quat_t wind_bf = quaternions_global_to_local(sim->ahrs.qe, wind_gf); float sqr_lateral_airspeed = SQR(sim->vel_bf[0] + wind_bf.v[0]) + SQR(sim->vel_bf[1] + wind_bf.v[1]); float lateral_airspeed = sqrt(sqr_lateral_airspeed); float old_rotor_speed; for (int32_t i = 0; i < 4; i++) { motor_command[i] = (float)sim->mix->servos->servo[i].value - sim->vehicle_config.rotor_rpm_offset; if (motor_command[i] < 0.0f) { motor_command[i] = 0; } // temporarily save old rotor speeds old_rotor_speed = sim->rotorspeeds[i]; // estimate rotor speeds by low - pass filtering //sim->rotorspeeds[i] = (sim->vehicle_config.rotor_lpf) * sim->rotorspeeds[i] + (1.0f - sim->vehicle_config.rotor_lpf) * (motor_command[i] * sim->vehicle_config.rotor_rpm_gain); sim->rotorspeeds[i] = (motor_command[i] * sim->vehicle_config.rotor_rpm_gain); // calculate torque created by rotor inertia rotor_inertia[i] = (sim->rotorspeeds[i] - old_rotor_speed) / sim->dt * sim->vehicle_config.rotor_momentum; ldb = lift_drag_base(sim, sim->rotorspeeds[i], sqr_lateral_airspeed, -sim->vel_bf[Z]); //ldb = lift_drag_base(sim, sim->rotorspeeds[i], sqr_lateral_airspeed, 0.0f); rotor_lifts[i] = ldb * sim->vehicle_config.rotor_cl; rotor_drags[i] = ldb * sim->vehicle_config.rotor_cd; } float mpos_x = sim->vehicle_config.rotor_arm_length / 1.4142f; float mpos_y = sim->vehicle_config.rotor_arm_length / 1.4142f; // torque around x axis (roll) sim->torques_bf[ROLL] = ((rotor_lifts[sim->mix->motor_front_left] + rotor_lifts[sim->mix->motor_rear_left] ) - (rotor_lifts[sim->mix->motor_front_right] + rotor_lifts[sim->mix->motor_rear_right] )) * mpos_y;; // torque around y axis (pitch) sim->torques_bf[PITCH] = ((rotor_lifts[sim->mix->motor_front_left] + rotor_lifts[sim->mix->motor_front_right] ) - (rotor_lifts[sim->mix->motor_rear_left] + rotor_lifts[sim->mix->motor_rear_right] ))* mpos_x; sim->torques_bf[YAW] = (sim->mix->motor_front_left_dir * (10.0f * rotor_drags[sim->mix->motor_front_left] + rotor_inertia[sim->mix->motor_front_left]) + sim->mix->motor_front_right_dir * (10.0f * rotor_drags[sim->mix->motor_front_right] + rotor_inertia[sim->mix->motor_front_right]) + sim->mix->motor_rear_left_dir * (10.0f * rotor_drags[sim->mix->motor_rear_left] + rotor_inertia[sim->mix->motor_rear_left]) + sim->mix->motor_rear_right_dir * (10.0f * rotor_drags[sim->mix->motor_rear_right] + rotor_inertia[sim->mix->motor_rear_right] ))* sim->vehicle_config.rotor_diameter; sim->lin_forces_bf[X] = -(sim->vel_bf[X] - wind_bf.v[0]) * lateral_airspeed * sim->vehicle_config.vehicle_drag; sim->lin_forces_bf[Y] = -(sim->vel_bf[Y] - wind_bf.v[1]) * lateral_airspeed * sim->vehicle_config.vehicle_drag; sim->lin_forces_bf[Z] = -(rotor_lifts[sim->mix->motor_front_left]+ rotor_lifts[sim->mix->motor_front_right] +rotor_lifts[sim->mix->motor_rear_left] +rotor_lifts[sim->mix->motor_rear_right]); } //------------------------------------------------------------------------------ // PUBLIC FUNCTIONS IMPLEMENTATION //------------------------------------------------------------------------------ bool simulation_init(simulation_model_t* sim, const simulation_config_t* sim_config, ahrs_t* ahrs, imu_t* imu, position_estimation_t* pos_est, barometer_t* pressure, gps_t* gps, sonar_t* sonar, state_t* state, bool* waypoint_set, const servo_mix_quadcotper_diag_t* mix) { bool init_success = true; //Init dependencies sim->vehicle_config = *sim_config; sim->imu = imu; sim->pos_est = pos_est; sim->pressure = pressure; sim->gps = gps; sim->sonar = sonar; sim->state = state; sim->nav_plan_active = &state->nav_plan_active; sim->mix = mix; // set initial conditions to a given attitude_filter sim->estimated_attitude = ahrs; sim->ahrs = *ahrs; print_util_dbg_print("Attitude:"); print_util_dbg_print_num(sim->ahrs.qe.s*100,10); print_util_dbg_print(", "); print_util_dbg_print_num(sim->ahrs.qe.v[0]*100,10); print_util_dbg_print(", "); print_util_dbg_print_num(sim->ahrs.qe.v[1]*100,10); print_util_dbg_print(", "); print_util_dbg_print_num(sim->ahrs.qe.v[2]*100,10); print_util_dbg_print("\r\n"); for (int32_t i = 0; i < ROTORCOUNT; i++) { sim->rotorspeeds[i] = 0.0f; } sim->last_update = time_keeper_get_micros(); sim->dt = 0.01f; simulation_reset_simulation(sim); simulation_calib_set(sim); print_util_dbg_print("[HIL SIMULATION] initialised.\r\n"); return init_success; }
void qfilter_update(qfilter_t *qf) { static uint32_t convergence_update_count = 0; float omc[3], omc_mag[3] , tmp[3], snorm, norm, s_acc_norm, acc_norm, s_mag_norm, mag_norm; quat_t qed, qtmp1, up, up_bf; quat_t mag_global, mag_corrected_local; quat_t front_vec_global = { .s = 0.0f, .v = {1.0f, 0.0f, 0.0f} }; float kp = qf->kp; float kp_mag = qf->kp_mag; float ki = qf->ki; float ki_mag = qf->ki_mag; // Update time in us float now_us = time_keeper_get_micros(); // Delta t in seconds float dt = 1e-6 * (float)(now_us - qf->ahrs->last_update); // Write to ahrs structure qf->ahrs->dt = dt; qf->ahrs->last_update = now_us; // up_bf = qe^-1 *(0,0,0,-1) * qe up.s = 0; up.v[X] = UPVECTOR_X; up.v[Y] = UPVECTOR_Y; up.v[Z] = UPVECTOR_Z; up_bf = quaternions_global_to_local(qf->ahrs->qe, up); // calculate norm of acceleration vector s_acc_norm = qf->imu->scaled_accelero.data[X] * qf->imu->scaled_accelero.data[X] + qf->imu->scaled_accelero.data[Y] * qf->imu->scaled_accelero.data[Y] + qf->imu->scaled_accelero.data[Z] * qf->imu->scaled_accelero.data[Z]; if ( (s_acc_norm > 0.7f * 0.7f) && (s_acc_norm < 1.3f * 1.3f) ) { // approximate square root by running 2 iterations of newton method acc_norm = maths_fast_sqrt(s_acc_norm); tmp[X] = qf->imu->scaled_accelero.data[X] / acc_norm; tmp[Y] = qf->imu->scaled_accelero.data[Y] / acc_norm; tmp[Z] = qf->imu->scaled_accelero.data[Z] / acc_norm; // omc = a x up_bf.v CROSS(tmp, up_bf.v, omc); } else { omc[X] = 0; omc[Y] = 0; omc[Z] = 0; } // Heading computation // transfer qtmp1 = quaternions_create_from_vector(qf->imu->scaled_compass.data); mag_global = quaternions_local_to_global(qf->ahrs->qe, qtmp1); // calculate norm of compass vector //s_mag_norm = SQR(mag_global.v[X]) + SQR(mag_global.v[Y]) + SQR(mag_global.v[Z]); s_mag_norm = SQR(mag_global.v[X]) + SQR(mag_global.v[Y]); if ( (s_mag_norm > 0.004f * 0.004f) && (s_mag_norm < 1.8f * 1.8f) ) { mag_norm = maths_fast_sqrt(s_mag_norm); mag_global.v[X] /= mag_norm; mag_global.v[Y] /= mag_norm; mag_global.v[Z] = 0.0f; // set z component in global frame to 0 // transfer magneto vector back to body frame qf->ahrs->north_vec = quaternions_global_to_local(qf->ahrs->qe, front_vec_global); mag_corrected_local = quaternions_global_to_local(qf->ahrs->qe, mag_global); // omc = a x up_bf.v CROSS(mag_corrected_local.v, qf->ahrs->north_vec.v, omc_mag); } else { omc_mag[X] = 0; omc_mag[Y] = 0; omc_mag[Z] = 0; } // get error correction gains depending on mode switch (qf->ahrs->internal_state) { case AHRS_UNLEVELED: kp = qf->kp * 10.0f; kp_mag = qf->kp_mag * 10.0f; ki = 0.5f * qf->ki; ki_mag = 0.5f * qf->ki_mag; convergence_update_count += 1; if( convergence_update_count > 2000 ) { convergence_update_count = 0; qf->ahrs->internal_state = AHRS_CONVERGING; print_util_dbg_print("End of AHRS attitude initialization.\r\n"); } break; case AHRS_CONVERGING: kp = qf->kp; kp_mag = qf->kp_mag; ki = qf->ki * 3.0f; ki_mag = qf->ki_mag * 3.0f; convergence_update_count += 1; if( convergence_update_count > 2000 ) { convergence_update_count = 0; qf->ahrs->internal_state = AHRS_READY; print_util_dbg_print("End of AHRS leveling.\r\n"); } break; case AHRS_READY: kp = qf->kp; kp_mag = qf->kp_mag; ki = qf->ki; ki_mag = qf->ki_mag; break; } // apply error correction with appropriate gains for accelerometer and compass for (uint8_t i = 0; i < 3; i++) { qtmp1.v[i] = 0.5f * (qf->imu->scaled_gyro.data[i] + kp * omc[i] + kp_mag * omc_mag[i]); } qtmp1.s = 0; // apply step rotation with corrections qed = quaternions_multiply(qf->ahrs->qe,qtmp1); // TODO: correct this formulas! qf->ahrs->qe.s = qf->ahrs->qe.s + qed.s * dt; qf->ahrs->qe.v[X] += qed.v[X] * dt; qf->ahrs->qe.v[Y] += qed.v[Y] * dt; qf->ahrs->qe.v[Z] += qed.v[Z] * dt; snorm = qf->ahrs->qe.s * qf->ahrs->qe.s + qf->ahrs->qe.v[X] * qf->ahrs->qe.v[X] + qf->ahrs->qe.v[Y] * qf->ahrs->qe.v[Y] + qf->ahrs->qe.v[Z] * qf->ahrs->qe.v[Z]; if (snorm < 0.0001f) { norm = 0.01f; } else { // approximate square root by running 2 iterations of newton method norm = maths_fast_sqrt(snorm); } qf->ahrs->qe.s /= norm; qf->ahrs->qe.v[X] /= norm; qf->ahrs->qe.v[Y] /= norm; qf->ahrs->qe.v[Z] /= norm; // bias estimate update qf->imu->calib_gyro.bias[X] += - dt * ki * omc[X] / qf->imu->calib_gyro.scale_factor[X]; qf->imu->calib_gyro.bias[Y] += - dt * ki * omc[Y] / qf->imu->calib_gyro.scale_factor[Y]; qf->imu->calib_gyro.bias[Z] += - dt * ki * omc[Z] / qf->imu->calib_gyro.scale_factor[Z]; qf->imu->calib_gyro.bias[X] += - dt * ki_mag * omc_mag[X] / qf->imu->calib_compass.scale_factor[X]; qf->imu->calib_gyro.bias[Y] += - dt * ki_mag * omc_mag[Y] / qf->imu->calib_compass.scale_factor[Y]; qf->imu->calib_gyro.bias[Z] += - dt * ki_mag * omc_mag[Z] / qf->imu->calib_compass.scale_factor[Z]; // set up-vector (bodyframe) in attitude qf->ahrs->up_vec.v[X] = up_bf.v[X]; qf->ahrs->up_vec.v[Y] = up_bf.v[Y]; qf->ahrs->up_vec.v[Z] = up_bf.v[Z]; // Update linear acceleration qf->ahrs->linear_acc[X] = 9.81f * (qf->imu->scaled_accelero.data[X] - qf->ahrs->up_vec.v[X]) ; // TODO: review this line! qf->ahrs->linear_acc[Y] = 9.81f * (qf->imu->scaled_accelero.data[Y] - qf->ahrs->up_vec.v[Y]) ; // TODO: review this line! qf->ahrs->linear_acc[Z] = 9.81f * (qf->imu->scaled_accelero.data[Z] - qf->ahrs->up_vec.v[Z]) ; // TODO: review this line! //update angular_speed. qf->ahrs->angular_speed[X] = qf->imu->scaled_gyro.data[X]; qf->ahrs->angular_speed[Y] = qf->imu->scaled_gyro.data[Y]; qf->ahrs->angular_speed[Z] = qf->imu->scaled_gyro.data[Z]; }
void scheduler_suspend_task(task_entry_t *te, uint32_t delay) { te->next_run = time_keeper_get_micros() + delay; }
int32_t scheduler_update(scheduler_t* scheduler) { int32_t i; int32_t realtime_violation = 0; task_set_t* ts = scheduler->task_set; task_function_t call_task; task_argument_t function_argument; task_return_t treturn; // Iterate through registered tasks for (i = ts->current_schedule_slot; i < ts->task_count; i++) { uint32_t current_time = time_keeper_get_micros(); // If the task is active and has waited long enough... if ( (ts->tasks[i].run_mode != RUN_NEVER) && (current_time >= ts->tasks[i].next_run) ) { uint32_t delay = current_time - (ts->tasks[i].next_run); uint32_t task_start_time; task_start_time = time_keeper_get_micros(); // Get function pointer and function argument call_task = ts->tasks[i].call_function; function_argument = ts->tasks[i].function_argument; // Execute task treturn = call_task(function_argument); // Set the next execution time of the task switch (ts->tasks[i].timing_mode) { case PERIODIC_ABSOLUTE: // Do not take delays into account ts->tasks[i].next_run += ts->tasks[i].repeat_period; break; case PERIODIC_RELATIVE: // Take delays into account ts->tasks[i].next_run = time_keeper_get_micros() + ts->tasks[i].repeat_period; break; } // Set the task to inactive if it has to run only once if (ts->tasks[i].run_mode == RUN_ONCE) { ts->tasks[i].run_mode = RUN_NEVER; } // Check real time violations if (ts->tasks[i].next_run < current_time) { realtime_violation = -i; //realtime violation!! ts->tasks[i].rt_violations++; ts->tasks[i].next_run = current_time + ts->tasks[i].repeat_period; } // Compute real-time statistics ts->tasks[i].delay_avg = (7 * ts->tasks[i].delay_avg + delay) / 8; if (delay > ts->tasks[i].delay_max) { ts->tasks[i].delay_max = delay; } ts->tasks[i].delay_var_squared = (15 * ts->tasks[i].delay_var_squared + (delay - ts->tasks[i].delay_avg) * (delay - ts->tasks[i].delay_avg)) / 16; ts->tasks[i].execution_time = (7 * ts->tasks[i].execution_time + (time_keeper_get_micros() - task_start_time)) / 8; // Depending on shceduling strategy, select next task slot switch (scheduler->schedule_strategy) { case FIXED_PRIORITY: // Fixed priority scheme - scheduler will start over with tasks with the highest priority ts->current_schedule_slot = 0; break; case ROUND_ROBIN: // Round robin scheme - scheduler will pick up where it left. if (i >= ts->task_count) { ts->current_schedule_slot = 0; } break; default: break; } return realtime_violation; } } return realtime_violation; }
static void position_estimation_position_correction(position_estimator_t *pos_est) { global_position_t global_gps_position; local_coordinates_t local_coordinates; float dt = pos_est->ahrs->dt; // quat_t bias_correction = {.s = 0, .v = {0.0f, 0.0f, 1.0f}}; quat_t vel_correction = { .s = 0, .v = { 0.0f, 0.0f, 1.0f } }; float pos_error[3] = { 0.0f, 0.0f, 0.0f }; float baro_alt_error = 0.0f; float baro_vel_error = 0.0f; float baro_gain = 0.0f; float gps_gain = 0.0f; float gps_dt = 0.0f; float vel_error[3] = { 0.0f, 0.0f, 0.0f }; uint32_t t_inter_gps, t_inter_baro; int32_t i; if (pos_est->init_barometer) { // altimeter correction if ( pos_est->time_last_barometer_msg < pos_est->barometer->last_update ) { pos_est->last_alt = -(pos_est->barometer->altitude ) + pos_est->local_position.origin.altitude; pos_est->time_last_barometer_msg = pos_est->barometer->last_update; } t_inter_baro = (time_keeper_get_micros() - pos_est->barometer->last_update) / 1000.0f; baro_gain = 1.0f; //math_util_fmax(1.0f - t_inter_baro / 1000.0f, 0.0f); //pos_est->local_position.pos[2] += kp_alt_baro / ((float)(t_inter_baro / 2.5f + 1.0f)) * alt_error; baro_alt_error = pos_est->last_alt - pos_est->local_position.pos[2]; baro_vel_error = pos_est->barometer->vario_vz - pos_est->vel[2]; //vel_error[2] = 0.1f * pos_error[2]; //pos_est->vel[2] += kp_alt_baro_v * vel_error[2]; } else { bmp085_reset_origin_altitude(pos_est->barometer, pos_est->local_position.origin.altitude); pos_est->init_barometer = true; } if (pos_est->init_gps_position) { if ( (pos_est->time_last_gps_msg < pos_est->gps->time_last_msg) && (pos_est->gps->status == GPS_OK) ) { pos_est->time_last_gps_msg = pos_est->gps->time_last_msg; global_gps_position.longitude = pos_est->gps->longitude; global_gps_position.latitude = pos_est->gps->latitude; global_gps_position.altitude = pos_est->gps->altitude; global_gps_position.heading = 0.0f; local_coordinates = coord_conventions_global_to_local_position(global_gps_position,pos_est->local_position.origin); local_coordinates.timestamp_ms = pos_est->gps->time_last_msg; // compute GPS velocity estimate gps_dt = (local_coordinates.timestamp_ms - pos_est->last_gps_pos.timestamp_ms) / 1000.0f; if (gps_dt > 0.001f) { for (i = 0; i < 3; i++) { pos_est->last_vel[i] = (local_coordinates.pos[i] - pos_est->last_gps_pos.pos[i]) / gps_dt; } pos_est->last_gps_pos = local_coordinates; } else { print_util_dbg_print("GPS dt is too small!\r\n"); } } t_inter_gps = time_keeper_get_millis() - pos_est->gps->time_last_msg; //gps_gain = math_util_fmax(1.0f - t_inter_gps / 1000.0f, 0.0f); gps_gain = 1.0f; for (i = 0;i < 3;i++) { pos_error[i] = pos_est->last_gps_pos.pos[i] - pos_est->local_position.pos[i]; vel_error[i] = pos_est->last_vel[i] - pos_est->vel[i]; } } else { gps_position_init(pos_est); for (i = 0;i < 2;i++) { pos_error[i] = 0.0f; vel_error[i] = 0.0f; } gps_gain = 0.1f; } // Apply error correction to velocity and position estimates for (i = 0;i < 3;i++) { pos_est->local_position.pos[i] += pos_est->kp_pos_gps[i] * gps_gain * pos_error[i]* dt; } pos_est->local_position.pos[2] += pos_est->kp_alt_baro * baro_gain * baro_alt_error* dt; for (i = 0; i < 3; i++) { vel_correction.v[i] = vel_error[i]; } for (i = 0;i < 3;i++) { pos_est->vel[i] += pos_est->kp_vel_gps[i] * gps_gain * vel_correction.v[i]* dt; } pos_est->vel[2] += pos_est->kp_vel_baro * baro_gain * baro_vel_error* dt; } static void gps_position_init(position_estimator_t *pos_est) { int32_t i; if ( (pos_est->init_gps_position == false)&&(pos_est->gps->status == GPS_OK) ) { if ( pos_est->time_last_gps_msg < pos_est->gps->time_last_msg ) { pos_est->time_last_gps_msg = pos_est->gps->time_last_msg; pos_est->init_gps_position = true; pos_est->local_position.origin.longitude = pos_est->gps->longitude; pos_est->local_position.origin.latitude = pos_est->gps->latitude; pos_est->local_position.origin.altitude = pos_est->gps->altitude; pos_est->local_position.timestamp_ms = pos_est->gps->time_last_msg; pos_est->last_gps_pos = pos_est->local_position; pos_est->last_alt = 0; for(i = 0;i < 3;i++) { pos_est->last_vel[i] = 0.0f; pos_est->local_position.pos[i] = 0.0f; pos_est->vel[i] = 0.0f; } print_util_dbg_print("GPS position initialized!\r\n"); } } }
void simulation_update(simulation_model_t *sim) { int32_t i; quat_t qtmp1, qvel_bf, qed; const quat_t front = {.s = 0.0f, .v = {1.0f, 0.0f, 0.0f}}; const quat_t up = {.s = 0.0f, .v = {UPVECTOR_X, UPVECTOR_Y, UPVECTOR_Z}}; uint32_t now = time_keeper_get_micros(); sim->dt = (now - sim->last_update) / 1000000.0f; if (sim->dt > 0.1f) { sim->dt = 0.1f; } sim->last_update = now; // compute torques and forces based on servo commands forces_from_servos_diag_quad(sim); // integrate torques to get simulated gyro rates (with some damping) sim->rates_bf[0] = maths_clip((1.0f - 0.1f * sim->dt) * sim->rates_bf[0] + sim->dt * sim->torques_bf[0] / sim->vehicle_config.roll_pitch_momentum, 10.0f); sim->rates_bf[1] = maths_clip((1.0f - 0.1f * sim->dt) * sim->rates_bf[1] + sim->dt * sim->torques_bf[1] / sim->vehicle_config.roll_pitch_momentum, 10.0f); sim->rates_bf[2] = maths_clip((1.0f - 0.1f * sim->dt) * sim->rates_bf[2] + sim->dt * sim->torques_bf[2] / sim->vehicle_config.yaw_momentum, 10.0f); for (i = 0; i < 3; i++) { qtmp1.v[i] = 0.5f * sim->rates_bf[i]; } qtmp1.s = 0; // apply step rotation qed = quaternions_multiply(sim->ahrs.qe,qtmp1); // TODO: correct this formulas when using the right scales sim->ahrs.qe.s = sim->ahrs.qe.s + qed.s * sim->dt; sim->ahrs.qe.v[0] += qed.v[0] * sim->dt; sim->ahrs.qe.v[1] += qed.v[1] * sim->dt; sim->ahrs.qe.v[2] += qed.v[2] * sim->dt; sim->ahrs.qe = quaternions_normalise(sim->ahrs.qe); sim->ahrs.up_vec = quaternions_global_to_local(sim->ahrs.qe, up); sim->ahrs.north_vec = quaternions_global_to_local(sim->ahrs.qe, front); // velocity and position integration // check altitude - if it is lower than 0, clamp everything (this is in NED, assuming negative altitude) if (sim->local_position.pos[Z] >0) { sim->vel[Z] = 0.0f; sim->local_position.pos[Z] = 0.0f; // simulate "acceleration" caused by contact force with ground, compensating gravity for (i = 0; i < 3; i++) { sim->lin_forces_bf[i] = sim->ahrs.up_vec.v[i] * sim->vehicle_config.total_mass * sim->vehicle_config.sim_gravity; } // slow down... (will make velocity slightly inconsistent until next update cycle, but shouldn't matter much) for (i = 0; i < 3; i++) { sim->vel_bf[i] = 0.95f * sim->vel_bf[i]; } //upright sim->rates_bf[0] = sim->ahrs.up_vec.v[1]; sim->rates_bf[1] = - sim->ahrs.up_vec.v[0]; sim->rates_bf[2] = 0; } sim->ahrs.qe = quaternions_normalise(sim->ahrs.qe); sim->ahrs.up_vec = quaternions_global_to_local(sim->ahrs.qe, up); sim->ahrs.north_vec = quaternions_global_to_local(sim->ahrs.qe, front); for (i = 0; i < 3; i++) { qtmp1.v[i] = sim->vel[i]; } qtmp1.s = 0.0f; qvel_bf = quaternions_global_to_local(sim->ahrs.qe, qtmp1); float acc_bf[3]; for (i = 0; i < 3; i++) { sim->vel_bf[i] = qvel_bf.v[i]; // following the convention in the IMU, this is the acceleration due to force, as measured sim->ahrs.linear_acc[i] = sim->lin_forces_bf[i] / sim->vehicle_config.total_mass; // this is the "clean" acceleration without gravity acc_bf[i] = sim->ahrs.linear_acc[i] - sim->ahrs.up_vec.v[i] * sim->vehicle_config.sim_gravity; sim->vel_bf[i] = sim->vel_bf[i] + acc_bf[i] * sim->dt; } // calculate velocity in global frame // vel = qe *vel_bf * qe - 1 qvel_bf.s = 0.0f; qvel_bf.v[0] = sim->vel_bf[0]; qvel_bf.v[1] = sim->vel_bf[1]; qvel_bf.v[2] = sim->vel_bf[2]; qtmp1 = quaternions_local_to_global(sim->ahrs.qe, qvel_bf); sim->vel[0] = qtmp1.v[0]; sim->vel[1] = qtmp1.v[1]; sim->vel[2] = qtmp1.v[2]; for (i = 0; i < 3; i++) { sim->local_position.pos[i] = sim->local_position.pos[i] + sim->vel[i] * sim->dt; } // fill in simulated IMU values for (i = 0;i < 3; i++) { sim->imu->raw_gyro.data[sim->imu->calib_gyro.axis[i]] = (sim->rates_bf[i] * sim->calib_gyro.scale_factor[i] + sim->calib_gyro.bias[i]) * sim->calib_gyro.orientation[i]; sim->imu->raw_accelero.data[sim->imu->calib_accelero.axis[i]] = ((sim->lin_forces_bf[i] / sim->vehicle_config.total_mass / sim->vehicle_config.sim_gravity) * sim->calib_accelero.scale_factor[i] + sim->calib_accelero.bias[i]) * sim->calib_accelero.orientation[i]; sim->imu->raw_compass.data[sim->imu->calib_compass.axis[i]] = ((sim->ahrs.north_vec.v[i] ) * sim->calib_compass.scale_factor[i] + sim->calib_compass.bias[i])* sim->calib_compass.orientation[i]; } sim->local_position.heading = coord_conventions_get_yaw(sim->ahrs.qe); } void simulation_simulate_barometer(simulation_model_t *sim) { sim->pressure->altitude = sim->local_position.origin.altitude - sim->local_position.pos[Z]; sim->pressure->vario_vz = sim->vel[Z]; sim->pressure->last_update = time_keeper_get_millis(); sim->pressure->altitude_offset = 0; } void simulation_simulate_gps(simulation_model_t *sim) { global_position_t gpos = coord_conventions_local_to_global_position(sim->local_position); sim->gps->altitude = gpos.altitude; sim->gps->latitude = gpos.latitude; sim->gps->longitude = gpos.longitude; sim->gps->time_last_msg = time_keeper_get_millis(); sim->gps->time_last_posllh_msg = time_keeper_get_millis(); sim->gps->time_last_velned_msg = time_keeper_get_millis(); sim->gps->north_speed = sim->vel[X]; sim->gps->east_speed = sim->vel[Y]; sim->gps->vertical_speed = sim->vel[Z]; sim->gps->status = GPS_OK; sim->gps->healthy = true; }
static void position_estimation_position_correction(position_estimation_t *pos_est) { global_position_t global_gps_position; local_coordinates_t local_coordinates; float dt = pos_est->ahrs->dt; // quat_t bias_correction = {.s = 0, .v = {0.0f, 0.0f, 1.0f}}; quat_t vel_correction = { .s = 0, .v = { 0.0f, 0.0f, 1.0f } }; float pos_error[3] = { 0.0f, 0.0f, 0.0f }; float baro_alt_error = 0.0f; float baro_vel_error = 0.0f; float baro_gain = 0.0f; float gps_gain = 0.0f; float gps_dt = 0.0f; float vel_error[3] = { 0.0f, 0.0f, 0.0f }; uint32_t t_inter_gps, t_inter_baro; int32_t i; if (pos_est->init_barometer) { // altimeter correction if ( pos_est->time_last_barometer_msg < pos_est->barometer->last_update ) { pos_est->last_alt = -(pos_est->barometer->altitude ) + pos_est->local_position.origin.altitude; baro_alt_error = -(pos_est->barometer->altitude ) - pos_est->local_position.pos[2] + pos_est->local_position.origin.altitude; pos_est->time_last_barometer_msg = pos_est->barometer->last_update; } t_inter_baro = (time_keeper_get_micros() - pos_est->barometer->last_update) / 1000.0f; baro_gain = 1.0f; //math_util_fmax(1.0f - t_inter_baro / 1000.0f, 0.0f); //pos_est->local_position.pos[2] += kp_alt_baro / ((float)(t_inter_baro / 2.5f + 1.0f)) * alt_error; baro_alt_error = pos_est->last_alt - pos_est->local_position.pos[2]; baro_vel_error = pos_est->barometer->vario_vz - pos_est->vel[2]; //vel_error[2] = 0.1f * pos_error[2]; //pos_est->vel[2] += kp_alt_baro_v * vel_error[2]; } else { bmp085_reset_origin_altitude(pos_est->barometer, pos_est->local_position.origin.altitude); pos_est->init_barometer = true; } if (pos_est->init_gps_position) { if ( (pos_est->time_last_gps_msg < pos_est->gps->time_last_msg) && (pos_est->gps->status == GPS_OK) ) { pos_est->time_last_gps_msg = pos_est->gps->time_last_msg; global_gps_position.longitude = pos_est->gps->longitude; global_gps_position.latitude = pos_est->gps->latitude; global_gps_position.altitude = pos_est->gps->altitude; global_gps_position.heading = 0.0f; local_coordinates = coord_conventions_global_to_local_position(global_gps_position,pos_est->local_position.origin); local_coordinates.timestamp_ms = pos_est->gps->time_last_msg; // compute GPS velocity estimate gps_dt = (local_coordinates.timestamp_ms - pos_est->last_gps_pos.timestamp_ms) / 1000.0f; if (gps_dt > 0.001f) { for (i = 0; i < 3; i++) { pos_est->last_vel[i] = (local_coordinates.pos[i] - pos_est->last_gps_pos.pos[i]) / gps_dt; } pos_est->last_gps_pos = local_coordinates; } else { print_util_dbg_print("GPS dt is too small!\r\n"); } } t_inter_gps = time_keeper_get_millis() - pos_est->gps->time_last_msg; //gps_gain = math_util_fmax(1.0f - t_inter_gps / 1000.0f, 0.0f); gps_gain = 1.0f; for (i = 0;i < 3;i++) { pos_error[i] = pos_est->last_gps_pos.pos[i] - pos_est->local_position.pos[i]; vel_error[i] = pos_est->last_vel[i] - pos_est->vel[i]; } } else { gps_position_init(pos_est); for (i = 0;i < 2;i++) { pos_error[i] = 0.0f; vel_error[i] = 0.0f; } gps_gain = 0.1f; } // Apply error correction to velocity and position estimates for (i = 0;i < 3;i++) { pos_est->local_position.pos[i] += pos_est->kp_pos_gps[i] * gps_gain * pos_error[i]* dt; } pos_est->local_position.pos[2] += pos_est->kp_alt_baro * baro_gain * baro_alt_error* dt; for (i = 0; i < 3; i++) { vel_correction.v[i] = vel_error[i]; } for (i = 0;i < 3;i++) { pos_est->vel[i] += pos_est->kp_vel_gps[i] * gps_gain * vel_correction.v[i]* dt; } pos_est->vel[2] += pos_est->kp_vel_baro * baro_gain * baro_vel_error* dt; } static void gps_position_init(position_estimation_t *pos_est) { if ( (pos_est->init_gps_position == false)&&(pos_est->gps->status == GPS_OK) ) { if ( pos_est->time_last_gps_msg < pos_est->gps->time_last_msg ) { pos_est->time_last_gps_msg = pos_est->gps->time_last_msg; pos_est->init_gps_position = true; pos_est->local_position.origin.longitude = pos_est->gps->longitude; pos_est->local_position.origin.latitude = pos_est->gps->latitude; pos_est->local_position.origin.altitude = pos_est->gps->altitude; pos_est->local_position.timestamp_ms = pos_est->gps->time_last_msg; pos_est->last_gps_pos = pos_est->local_position; pos_est->last_alt = 0; for(int32_t i = 0;i < 3;i++) { pos_est->last_vel[i] = 0.0f; pos_est->local_position.pos[i] = 0.0f; pos_est->vel[i] = 0.0f; } print_util_dbg_print("GPS position initialized!\r\n"); } } } //------------------------------------------------------------------------------ // PUBLIC FUNCTIONS IMPLEMENTATION //------------------------------------------------------------------------------ void position_estimation_init(position_estimation_t* pos_est, const position_estimation_conf_t* config, state_t* state, barometer_t *barometer, const gps_t *gps, const ahrs_t *ahrs, const imu_t *imu) { //init dependencies pos_est->barometer = barometer; pos_est->gps = gps; pos_est->ahrs = ahrs; pos_est->imu = imu; pos_est->state = state; pos_est->nav_plan_active = &state->nav_plan_active; // default GPS home position pos_est->local_position.origin.longitude = config->origin.longitude; pos_est->local_position.origin.latitude = config->origin.latitude; pos_est->local_position.origin.altitude = config->origin.altitude; pos_est->local_position.pos[X] = 0; pos_est->local_position.pos[Y] = 0; pos_est->local_position.pos[Z] = 0; // reset position estimator pos_est->last_alt = 0; for(int32_t i = 0;i < 3;i++) { pos_est->last_vel[i] = 0.0f; pos_est->vel[i] = 0.0f; pos_est->vel_bf[i] = 0.0f; } pos_est->gravity = config->gravity; pos_est->init_gps_position = false; pos_est->init_barometer = false; pos_est->time_last_gps_msg = 0; pos_est->time_last_barometer_msg = 0; pos_est->kp_pos_gps[X] = 2.0f; pos_est->kp_pos_gps[Y] = 2.0f; pos_est->kp_pos_gps[Z] = 0.0f; pos_est->kp_vel_gps[X] = 1.0f; pos_est->kp_vel_gps[Y] = 1.0f; pos_est->kp_vel_gps[Z] = 0.5f; pos_est->kp_alt_baro = 2.0f; pos_est->kp_vel_baro = 1.0f; gps_position_init(pos_est); print_util_dbg_print("Position estimation initialized.\r\n"); }
void time_keeper_delay_micros(int32_t microseconds) { uint32_t now = time_keeper_get_micros(); while (time_keeper_get_micros() < now + microseconds); }
void time_keeper_delay_until(uint32_t until_time) { while (time_keeper_get_micros() < until_time); }