static Mavlink_waypoint_handler::waypoint_struct_t convert_waypoint_to_local_ned(const Mavlink_waypoint_handler::waypoint_struct_t* waypoint_in, const global_position_t* origin)
{
global_position_t waypoint_global;
local_position_t waypoint_local;
// Init new waypoint
Mavlink_waypoint_handler::waypoint_struct_t waypoint = *waypoint_in;
waypoint.command = waypoint_in->command;
waypoint.param1 = waypoint_in->param1;
waypoint.param2 = waypoint_in->param2;
waypoint.param3 = waypoint_in->param3;
waypoint.param4 = waypoint_in->param4;
switch (waypoint_in->frame)
{
case MAV_FRAME_GLOBAL: /* Global coordinate frame, WGS84 coordinate system. First value / x: latitude, second value / y: longitude, third value / z: positive altitude over mean sea level (MSL) | */
waypoint_global.latitude = waypoint_in->x;
waypoint_global.longitude = waypoint_in->y;
waypoint_global.altitude = waypoint_in->z;
waypoint_local = coord_conventions_global_to_local_position(waypoint_global, *origin);
waypoint.frame = MAV_FRAME_LOCAL_NED;
waypoint.x = waypoint_local.pos[X];
waypoint.y = waypoint_local.pos[Y];
waypoint.z = waypoint_local.pos[Z];
break;
case MAV_FRAME_LOCAL_NED: /* Local coordinate frame, Z-up (x: north, y: east, z: down). | */
case MAV_FRAME_MISSION: /* NOT a coordinate frame, indicates a mission command. | */
case MAV_FRAME_GLOBAL_RELATIVE_ALT: /* Global coordinate frame, WGS84 coordinate system, relative altitude over ground with respect to the home position. First value / x: latitude, second value / y: longitude, third value / z: positive altitude with 0 being at the altitude of the home location. | */
case MAV_FRAME_LOCAL_ENU: /* Local coordinate frame, Z-down (x: east, y: north, z: up) | */
case MAV_FRAME_LOCAL_OFFSET_NED: /* Offset to the current local frame. Anything expressed in this frame should be added to the current local frame position. | */
case MAV_FRAME_BODY_NED: /* Setpoint in body NED frame. This makes sense if all position control is externalized - e.g. useful to command 2 m/s^2 acceleration to the right. | */
case MAV_FRAME_BODY_OFFSET_NED: /* Offset in body NED frame. This makes sense if adding setpoints to the current flight path, to avoid an obstacle - e.g. useful to command 2 m/s^2 acceleration to the east. | */
case MAV_FRAME_GLOBAL_TERRAIN_ALT: /* Global coordinate frame with above terrain level altitude. WGS84 coordinate system, relative altitude over terrain with respect to the waypoint coordinate. First value / x: latitude in degrees, second value / y: longitude in degrees, third value / z: positive altitude in meters with 0 being at ground level in terrain model. | */
case MAV_FRAME_ENUM_END:
// do not use this waypoint
waypoint.command = 0;
break;
}
return waypoint;
}
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 neighbors_selection_read_message_from_neighbors(neighbors_t *neighbors, uint32_t sysid, mavlink_message_t* msg)
{
//Use this block for message debugging
/*
print_util_dbg_print("\n Received message with ID");
print_util_dbg_print_num(msg->msgid, 10);
print_util_dbg_print(" from system");
print_util_dbg_print_num(msg->sysid, 10);
print_util_dbg_print(" for component");
print_util_dbg_print_num(msg->compid,10);
print_util_dbg_print( "\r\n");
*/
uint8_t i;
mavlink_global_position_int_t packet;
mavlink_msg_global_position_int_decode(msg,&packet);
//Check if coming from a neighbor
if (msg->sysid != (uint8_t)sysid)
{
global_position_t global_pos_neighbor;
local_coordinates_t local_pos_neighbor;
uint8_t actual_neighbor;
global_pos_neighbor.longitude = (double)packet.lon / 10000000.0f;
global_pos_neighbor.latitude = (double)packet.lat / 10000000.0f;
global_pos_neighbor.altitude = (float)packet.alt / 1000.0f;
global_pos_neighbor.heading = (float)packet.hdg;
local_pos_neighbor = coord_conventions_global_to_local_position(global_pos_neighbor,neighbors->position_estimator->local_position.origin);
local_pos_neighbor.pos[2] = -packet.relative_alt / 1000.0f;
bool ID_found = false;
i = 0;
while ((!ID_found)&&(i < neighbors->number_of_neighbors))
{
if (msg->sysid == neighbors->neighbors_list[i].neighbor_ID)
{
ID_found = true;
}
else
{
i++;
}
}
if (i >= neighbors->number_of_neighbors)
{
if (neighbors->number_of_neighbors < MAX_NUM_NEIGHBORS)
{
actual_neighbor = neighbors->number_of_neighbors;
neighbors->number_of_neighbors++;
neighbors->neighbors_list[actual_neighbor].comm_frequency = 0.0f;
neighbors->neighbors_list[actual_neighbor].msg_count = 0;
}
else
{
// This case shouldn't happen
print_util_dbg_print("Error! There is more neighbors than planned!\n");
actual_neighbor = neighbors->number_of_neighbors - 1;
}
}
else
{
actual_neighbor = i;
}
if ( (neighbors->state->mav_state == MAV_STATE_STANDBY)||(neighbors->state->mav_state == MAV_STATE_BOOT) )
{
neighbors->alt_consensus = neighbors->LPF_alt * neighbors->alt_consensus + (1.0f - neighbors->LPF_alt) * global_pos_neighbor.altitude;
// print_util_dbg_print("Alt neighbor (x1000):");
// print_util_dbg_print_num(global_pos_neighbor.altitude*1000.0f,10);
// print_util_dbg_print(", consensus alt (x100):");
// print_util_dbg_print_num(neighbors->alt_consensus*100.0f,10);
// print_util_dbg_print(", alt neighbor(x100):");
// print_util_dbg_print_num(local_pos_neighbor.pos[2]*100.0f,10);
// print_util_dbg_print(", own local alt(x100):");
// print_util_dbg_print_num(neighbors->position_estimator->local_position.pos[2]*100.0f,10);
// print_util_dbg_print("\r\n");
// delay_ms(100);
}
neighbors->neighbors_list[actual_neighbor].neighbor_ID = msg->sysid;
for (i = 0; i < 3; i++)
{
neighbors->neighbors_list[actual_neighbor].position[i] = local_pos_neighbor.pos[i];
}
neighbors->neighbors_list[actual_neighbor].velocity[X] = packet.vx / 100.0f;
neighbors->neighbors_list[actual_neighbor].velocity[Y] = packet.vy / 100.0f;
neighbors->neighbors_list[actual_neighbor].velocity[Z] = packet.vz / 100.0f;
neighbors->neighbors_list[actual_neighbor].size = SIZE_VHC_ORCA;
neighbors->neighbors_list[actual_neighbor].time_msg_received = time_keeper_get_millis();
neighbors->neighbors_list[actual_neighbor].msg_count++;
// print_util_dbg_print("Neighbor with ID ");
// print_util_dbg_print_num(neighbors->neighbors_list[actual_neighbor].neighbor_ID,10);
// print_util_dbg_print(" at position ");
// print_util_dbg_print_vector(neighbors->neighbors_list[actual_neighbor].position,3);
// print_util_dbg_print(" with velocity ");
// delay_ms(100);
// print_util_dbg_print_vector(neighbors->neighbors_list[actual_neighbor].velocity,3);
// print_util_dbg_print(" with relative position ");
// float rel_pos[3];
// uint8_t i;
// for (i = 0; i < 3; i++)
// {
// rel_pos[i] = neighbors->neighbors_list[actual_neighbor].position[i] - neighbors->position_estimator->local_position.pos[i];
// }
// print_util_dbg_print_vector(rel_pos,3);
// print_util_dbg_print(", dist (x100):");
// print_util_dbg_print_num(vectors_norm(rel_pos)*100,10);
// print_util_dbg_print("\r\n");
// delay_ms(100);
}
}
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");
}
