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