Exemplo n.º 1
0
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);
}
Exemplo n.º 3
0
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();
}
Exemplo n.º 4
0
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]);
}
Exemplo n.º 6
0
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);
}
Exemplo n.º 7
0
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;
	}
}
Exemplo n.º 8
0
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;
}
Exemplo n.º 9
0
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;
}
Exemplo n.º 10
0
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];
}
Exemplo n.º 11
0
void scheduler_suspend_task(task_entry_t *te, uint32_t delay) 
{
	te->next_run = time_keeper_get_micros() + delay;
}
Exemplo n.º 12
0
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");
		}
	}
}
Exemplo n.º 14
0
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");
}
Exemplo n.º 16
0
void time_keeper_delay_micros(int32_t microseconds)
{
	uint32_t now = time_keeper_get_micros();
	while (time_keeper_get_micros() < now + microseconds);
}
Exemplo n.º 17
0
void time_keeper_delay_until(uint32_t until_time)
{
	while (time_keeper_get_micros() < until_time);
}