void simulation_fake_gps_fix(simulation_model_t* sim, uint32_t timestamp_ms)
{
local_coordinates_t fake_pos;
fake_pos.pos[X] = 10.0f;
fake_pos.pos[Y] = 10.0f;
fake_pos.pos[Z] = 0.0f;
fake_pos.origin.latitude = sim->vehicle_config.home_coordinates[0];
fake_pos.origin.longitude = sim->vehicle_config.home_coordinates[1];
fake_pos.origin.altitude = sim->vehicle_config.home_coordinates[2];
fake_pos.timestamp_ms = timestamp_ms;
global_position_t gpos = coord_conventions_local_to_global_position(fake_pos);
sim->gps->latitude = gpos.latitude;
sim->gps->longitude = gpos.longitude;
sim->gps->altitude = gpos.altitude;
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 = 0.0f;
sim->gps->east_speed = 0.0f;
sim->gps->vertical_speed = 0.0f;
sim->gps->status = GPS_OK;
sim->gps->healthy = true;
}
static mav_result_t position_estimation_set_new_home_position(position_estimation_t *pos_est, mavlink_command_long_t* packet)
{
mav_result_t result;
if(pos_est->state->mav_mode.ARMED == ARMED_OFF)
{
if (packet->param1 == 1)
{
// Set new home position to actual position
print_util_dbg_print("Set new home location to actual position.\r\n");
pos_est->local_position.origin = coord_conventions_local_to_global_position(pos_est->local_position);
print_util_dbg_print("New Home location: (");
print_util_dbg_print_num(pos_est->local_position.origin.latitude * 10000000.0f,10);
print_util_dbg_print(", ");
print_util_dbg_print_num(pos_est->local_position.origin.longitude * 10000000.0f,10);
print_util_dbg_print(", ");
print_util_dbg_print_num(pos_est->local_position.origin.altitude * 1000.0f,10);
print_util_dbg_print(")\r\n");
}
else
{
// Set new home position from msg
print_util_dbg_print("[POSITION ESTIMATION] Set new home location. \r\n");
pos_est->local_position.origin.latitude = packet->param5;
pos_est->local_position.origin.longitude = packet->param6;
pos_est->local_position.origin.altitude = packet->param7;
print_util_dbg_print("New Home location: (");
print_util_dbg_print_num(pos_est->local_position.origin.latitude * 10000000.0f,10);
print_util_dbg_print(", ");
print_util_dbg_print_num(pos_est->local_position.origin.longitude * 10000000.0f,10);
print_util_dbg_print(", ");
print_util_dbg_print_num(pos_est->local_position.origin.altitude * 1000.0f,10);
print_util_dbg_print(")\r\n");
}
pos_est->fence_set = false;
position_estimation_set_new_fence_origin(pos_est);
*pos_est->nav_plan_active = false;
result = MAV_RESULT_ACCEPTED;
}
else
{
result = MAV_RESULT_TEMPORARILY_REJECTED;
}
return result;
}
void position_estimation_telemetry_send_global_position(const position_estimation_t* pos_est, const mavlink_stream_t* mavlink_stream, mavlink_message_t* msg)
{
// send integrated position (for now there is no GPS error correction...!!!)
global_position_t gpos = coord_conventions_local_to_global_position(pos_est->local_position);
//mavlink_msg_global_position_int_send(mavlink_channel_t chan, uint32_t time_boot_ms, int32_t lat, int32_t lon, int32_t alt, int32_t relative_alt, int16_t vx, int16_t vy, int16_t vz, uint16_t hdg)
mavlink_msg_global_position_int_pack( mavlink_stream->sysid,
mavlink_stream->compid,
msg,
time_keeper_get_millis(),
gpos.latitude * 10000000,
gpos.longitude * 10000000,
gpos.altitude * 1000.0f,
-pos_est->local_position.pos[2] * 1000,
pos_est->vel[0] * 100.0f,
pos_est->vel[1] * 100.0f,
pos_est->vel[2] * 100.0f,
pos_est->local_position.heading);
}
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;
}
global_position_t Gps_mocap::position_gf(void) const
{
return coord_conventions_local_to_global_position(local_position_);
}
