void BlockLocalPositionEstimator::publishGlobalPos() { // publish global position double lat = 0; double lon = 0; const Vector<float, n_x> &xLP = _xLowPass.getState(); map_projection_reproject(&_map_ref, xLP(X_x), xLP(X_y), &lat, &lon); float alt = -xLP(X_z) + _altHome; if (PX4_ISFINITE(lat) && PX4_ISFINITE(lon) && PX4_ISFINITE(alt) && PX4_ISFINITE(xLP(X_vx)) && PX4_ISFINITE(xLP(X_vy)) && PX4_ISFINITE(xLP(X_vz))) { _pub_gpos.get().timestamp = _timeStamp; _pub_gpos.get().time_utc_usec = _sub_gps.get().time_utc_usec; _pub_gpos.get().lat = lat; _pub_gpos.get().lon = lon; _pub_gpos.get().alt = alt; _pub_gpos.get().vel_n = xLP(X_vx); _pub_gpos.get().vel_e = xLP(X_vy); _pub_gpos.get().vel_d = xLP(X_vz); _pub_gpos.get().yaw = _sub_att.get().yaw; _pub_gpos.get().eph = sqrtf(_P(X_x, X_x) + _P(X_y, X_y)); _pub_gpos.get().epv = sqrtf(_P(X_z, X_z)); _pub_gpos.get().terrain_alt = _altHome - xLP(X_tz); _pub_gpos.get().terrain_alt_valid = _validTZ; _pub_gpos.get().dead_reckoning = !_validXY && !_xyTimeout; _pub_gpos.get().pressure_alt = _sub_sensor.get().baro_alt_meter[0]; _pub_gpos.update(); } }
void MulticopterPositionControl::update_ref() { if (_local_pos.ref_timestamp != _ref_timestamp) { double lat_sp, lon_sp; float alt_sp; if (_ref_timestamp != 0) { /* calculate current position setpoint in global frame */ map_projection_reproject(&_ref_pos, _pos_sp(0), _pos_sp(1), &lat_sp, &lon_sp); alt_sp = _ref_alt - _pos_sp(2); } /* update local projection reference */ map_projection_init(&_ref_pos, _local_pos.ref_lat, _local_pos.ref_lon); _ref_alt = _local_pos.ref_alt; if (_ref_timestamp != 0) { /* reproject position setpoint to new reference */ map_projection_project(&_ref_pos, lat_sp, lon_sp, &_pos_sp.data[0], &_pos_sp.data[1]); _pos_sp(2) = -(alt_sp - _ref_alt); } _ref_timestamp = _local_pos.ref_timestamp; } }
void BlockLocalPositionEstimator::publishGlobalPos() { // publish global position double lat = 0; double lon = 0; const Vector<float, n_x> &xLP = _xLowPass.getState(); map_projection_reproject(&_map_ref, xLP(X_x), xLP(X_y), &lat, &lon); float alt = -xLP(X_z) + _altOrigin; // lie about eph/epv to allow visual odometry only navigation when velocity est. good float vxy_stddev = sqrtf(_P(X_vx, X_vx) + _P(X_vy, X_vy)); float epv = sqrtf(_P(X_z, X_z)); float eph = sqrtf(_P(X_x, X_x) + _P(X_y, X_y)); float eph_thresh = 3.0f; float epv_thresh = 3.0f; if (vxy_stddev < _vxy_pub_thresh.get()) { if (eph > eph_thresh) { eph = eph_thresh; } if (epv > epv_thresh) { epv = epv_thresh; } } if (PX4_ISFINITE(lat) && PX4_ISFINITE(lon) && PX4_ISFINITE(alt) && PX4_ISFINITE(xLP(X_vx)) && PX4_ISFINITE(xLP(X_vy)) && PX4_ISFINITE(xLP(X_vz))) { _pub_gpos.get().timestamp = _timeStamp; _pub_gpos.get().lat = lat; _pub_gpos.get().lon = lon; _pub_gpos.get().alt = alt; _pub_gpos.get().vel_n = xLP(X_vx); _pub_gpos.get().vel_e = xLP(X_vy); _pub_gpos.get().vel_d = xLP(X_vz); // this estimator does not provide a separate vertical position time derivative estimate, so use the vertical velocity _pub_gpos.get().pos_d_deriv = xLP(X_vz); _pub_gpos.get().yaw = _eul(2); _pub_gpos.get().eph = eph; _pub_gpos.get().epv = epv; _pub_gpos.get().pressure_alt = _sub_sensor.get().baro_alt_meter; _pub_gpos.get().terrain_alt = _altOrigin - xLP(X_tz); _pub_gpos.get().terrain_alt_valid = _estimatorInitialized & EST_TZ; _pub_gpos.get().dead_reckoning = !(_estimatorInitialized & EST_XY); _pub_gpos.update(); // TODO provide calculated values for these _pub_gpos.get().evh = 0.0f; _pub_gpos.get().evv = 0.0f; } }
void BlockLocalPositionEstimator::publishGlobalPos() { // publish global position double lat = 0; double lon = 0; const Vector<float, n_x> &xLP = _xLowPass.getState(); map_projection_reproject(&_map_ref, xLP(X_x), xLP(X_y), &lat, &lon); float alt = -xLP(X_z) + _altOrigin; // lie about eph/epv to allow visual odometry only navigation when velocity est. good float vxy_stddev = sqrtf(_P(X_vx, X_vx) + _P(X_vy, X_vy)); float epv = sqrtf(_P(X_z, X_z)); float eph = sqrtf(_P(X_x, X_x) + _P(X_y, X_y)); float eph_thresh = 3.0f; float epv_thresh = 3.0f; if (vxy_stddev < _vxy_pub_thresh.get()) { if (eph > eph_thresh) { eph = eph_thresh; } if (epv > epv_thresh) { epv = epv_thresh; } } if (PX4_ISFINITE(lat) && PX4_ISFINITE(lon) && PX4_ISFINITE(alt) && PX4_ISFINITE(xLP(X_vx)) && PX4_ISFINITE(xLP(X_vy)) && PX4_ISFINITE(xLP(X_vz))) { _pub_gpos.get().timestamp = _timeStamp; _pub_gpos.get().time_utc_usec = _sub_gps.get().time_utc_usec; _pub_gpos.get().lat = lat; _pub_gpos.get().lon = lon; _pub_gpos.get().alt = alt; _pub_gpos.get().vel_n = xLP(X_vx); _pub_gpos.get().vel_e = xLP(X_vy); _pub_gpos.get().vel_d = xLP(X_vz); _pub_gpos.get().yaw = _eul(2); _pub_gpos.get().eph = eph; _pub_gpos.get().epv = epv; _pub_gpos.get().terrain_alt = _altOrigin - xLP(X_tz); _pub_gpos.get().terrain_alt_valid = _validTZ; _pub_gpos.get().dead_reckoning = !_validXY && !_xyTimeout; _pub_gpos.get().pressure_alt = _sub_sensor.get().baro_alt_meter; _pub_gpos.update(); } }
void AttitudePositionEstimatorEKF::publishGlobalPosition() { _global_pos.timestamp = _local_pos.timestamp; if (_local_pos.xy_global) { double est_lat, est_lon; map_projection_reproject(&_pos_ref, _local_pos.x, _local_pos.y, &est_lat, &est_lon); _global_pos.lat = est_lat; _global_pos.lon = est_lon; _global_pos.time_utc_usec = _gps.time_utc_usec; } if (_local_pos.v_xy_valid) { _global_pos.vel_n = _local_pos.vx; _global_pos.vel_e = _local_pos.vy; } else { _global_pos.vel_n = 0.0f; _global_pos.vel_e = 0.0f; } /* local pos alt is negative, change sign and add alt offsets */ _global_pos.alt = (-_local_pos.z) - _baro_gps_offset; if (_local_pos.v_z_valid) { _global_pos.vel_d = _local_pos.vz; } /* terrain altitude */ _global_pos.terrain_alt = _ekf->hgtRef - _ekf->flowStates[1]; _global_pos.terrain_alt_valid = (_distance_last_valid > 0) && (hrt_elapsed_time(&_distance_last_valid) < 20 * 1000 * 1000); _global_pos.yaw = _local_pos.yaw; _global_pos.eph = _gps.eph; _global_pos.epv = _gps.epv; /* lazily publish the global position only once available */ if (_global_pos_pub != nullptr) { /* publish the global position */ orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &_global_pos); } else { /* advertise and publish */ _global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &_global_pos); } }
void PrecLand::run_state_descend_above_target() { position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); // check if target visible if (!check_state_conditions(PrecLandState::DescendAboveTarget)) { if (!switch_to_state_final_approach()) { PX4_WARN("Lost landing target while landing (descending)."); // Stay at current position for searching for the target pos_sp_triplet->current.lat = _navigator->get_global_position()->lat; pos_sp_triplet->current.lon = _navigator->get_global_position()->lon; pos_sp_triplet->current.alt = _navigator->get_global_position()->alt; if (!switch_to_state_start()) { if (!switch_to_state_fallback()) { PX4_ERR("Can't switch to fallback landing"); } } } return; } // XXX need to transform to GPS coords because mc_pos_control only looks at that double lat, lon; map_projection_reproject(&_map_ref, _target_pose.x_abs, _target_pose.y_abs, &lat, &lon); pos_sp_triplet->current.lat = lat; pos_sp_triplet->current.lon = lon; pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_LAND; _navigator->set_position_setpoint_triplet_updated(); }
void AttitudePositionEstimatorEKF::publishGlobalPosition() { _global_pos.timestamp = _local_pos.timestamp; if (_local_pos.xy_global) { double est_lat, est_lon; map_projection_reproject(&_pos_ref, _local_pos.x, _local_pos.y, &est_lat, &est_lon); _global_pos.lat = est_lat; _global_pos.lon = est_lon; _global_pos.time_utc_usec = _gps.time_utc_usec; } else { _global_pos.lat = 0.0; _global_pos.lon = 0.0; _global_pos.time_utc_usec = 0; } if (_local_pos.v_xy_valid) { _global_pos.vel_n = _local_pos.vx; _global_pos.vel_e = _local_pos.vy; } else { _global_pos.vel_n = 0.0f; _global_pos.vel_e = 0.0f; } /* local pos alt is negative, change sign and add alt offsets */ _global_pos.alt = (-_local_pos.z) - _filter_ref_offset - _baro_gps_offset; if (_local_pos.v_z_valid) { _global_pos.vel_d = _local_pos.vz; } else { _global_pos.vel_d = 0.0f; } /* terrain altitude */ _global_pos.terrain_alt = _ekf->hgtRef - _ekf->flowStates[1]; _global_pos.terrain_alt_valid = (_distance_last_valid > 0) && (hrt_elapsed_time(&_distance_last_valid) < 20 * 1000 * 1000); _global_pos.yaw = _local_pos.yaw; const float dtLastGoodGPS = static_cast<float>(hrt_absolute_time() - _previousGPSTimestamp) / 1e6f; if (_gps.timestamp_position == 0 || (dtLastGoodGPS >= POS_RESET_THRESHOLD)) { _global_pos.eph = EPH_LARGE_VALUE; _global_pos.epv = EPV_LARGE_VALUE; } else { _global_pos.eph = _gps.eph; _global_pos.epv = _gps.epv; } if (!isfinite(_global_pos.lat) || !isfinite(_global_pos.lon) || !isfinite(_global_pos.alt) || !isfinite(_global_pos.vel_n) || !isfinite(_global_pos.vel_e) || !isfinite(_global_pos.vel_d)) { // bad data, abort publication return; } /* lazily publish the global position only once available */ if (_global_pos_pub > 0) { /* publish the global position */ orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &_global_pos); } else { /* advertise and publish */ _global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &_global_pos); } }
void BottleDrop::task_main() { _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); mavlink_log_info(_mavlink_fd, "[bottle_drop] started"); _command_sub = orb_subscribe(ORB_ID(vehicle_command)); _wind_estimate_sub = orb_subscribe(ORB_ID(wind_estimate)); bool updated = false; float z_0; // ground properties float turn_radius; // turn radius of the UAV float precision; // Expected precision of the UAV float ground_distance = _alt_clearance; // Replace by closer estimate in loop // constant float g = CONSTANTS_ONE_G; // constant of gravity [m/s^2] float m = 0.5f; // mass of bottle [kg] float rho = 1.2f; // air density [kg/m^3] float A = ((0.063f * 0.063f) / 4.0f * M_PI_F); // Bottle cross section [m^2] float dt_freefall_prediction = 0.01f; // step size of the free fall prediction [s] // Has to be estimated by experiment float cd = 0.86f; // Drag coefficient for a cylinder with a d/l ratio of 1/3 [] float t_signal = 0.084f; // Time span between sending the signal and the bottle top reaching level height with the bottom of the plane [s] float t_door = 0.7f; // The time the system needs to open the door + safety, is also the time the palyload needs to safely escape the shaft [s] // Definition float h_0; // height over target float az; // acceleration in z direction[m/s^2] float vz; // velocity in z direction [m/s] float z; // fallen distance [m] float h; // height over target [m] float ax; // acceleration in x direction [m/s^2] float vx; // ground speed in x direction [m/s] float x; // traveled distance in x direction [m] float vw; // wind speed [m/s] float vrx; // relative velocity in x direction [m/s] float v; // relative speed vector [m/s] float Fd; // Drag force [N] float Fdx; // Drag force in x direction [N] float Fdz; // Drag force in z direction [N] float x_drop, y_drop; // coordinates of the drop point in reference to the target (projection of NED) float x_t, y_t; // coordinates of the target in reference to the target x_t = 0, y_t = 0 (projection of NED) float x_l, y_l; // local position in projected coordinates float x_f, y_f; // to-be position of the UAV after dt_runs seconds in projected coordinates double x_f_NED, y_f_NED; // to-be position of the UAV after dt_runs seconds in NED float distance_open_door; // The distance the UAV travels during its doors open [m] float approach_error = 0.0f; // The error in radians between current ground vector and desired ground vector float distance_real = 0; // The distance between the UAVs position and the drop point [m] float future_distance = 0; // The distance between the UAVs to-be position and the drop point [m] unsigned counter = 0; param_t param_gproperties = param_find("BD_GPROPERTIES"); param_t param_turn_radius = param_find("BD_TURNRADIUS"); param_t param_precision = param_find("BD_PRECISION"); param_t param_cd = param_find("BD_OBJ_CD"); param_t param_mass = param_find("BD_OBJ_MASS"); param_t param_surface = param_find("BD_OBJ_SURFACE"); param_get(param_precision, &precision); param_get(param_turn_radius, &turn_radius); param_get(param_gproperties, &z_0); param_get(param_cd, &cd); param_get(param_mass, &m); param_get(param_surface, &A); int vehicle_global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); struct parameter_update_s update; memset(&update, 0, sizeof(update)); int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); struct mission_item_s flight_vector_s {}; struct mission_item_s flight_vector_e {}; flight_vector_s.nav_cmd = NAV_CMD_WAYPOINT; flight_vector_s.acceptance_radius = 50; // TODO: make parameter flight_vector_s.autocontinue = true; flight_vector_s.altitude_is_relative = false; flight_vector_e.nav_cmd = NAV_CMD_WAYPOINT; flight_vector_e.acceptance_radius = 50; // TODO: make parameter flight_vector_e.autocontinue = true; flight_vector_s.altitude_is_relative = false; struct wind_estimate_s wind; // wakeup source(s) struct pollfd fds[1]; // Setup of loop fds[0].fd = _command_sub; fds[0].events = POLLIN; // Whatever state the bay is in, we want it closed on startup lock_release(); close_bay(); while (!_task_should_exit) { /* wait for up to 100ms for data */ int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 50); /* this is undesirable but not much we can do - might want to flag unhappy status */ if (pret < 0) { warn("poll error %d, %d", pret, errno); continue; } /* vehicle commands updated */ if (fds[0].revents & POLLIN) { orb_copy(ORB_ID(vehicle_command), _command_sub, &_command); handle_command(&_command); } orb_check(vehicle_global_position_sub, &updated); if (updated) { /* copy global position */ orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, &_global_pos); } if (_global_pos.timestamp == 0) { continue; } const unsigned sleeptime_us = 9500; hrt_abstime last_run = hrt_absolute_time(); float dt_runs = sleeptime_us / 1e6f; // switch to faster updates during the drop while (_drop_state > DROP_STATE_INIT) { // Get wind estimate orb_check(_wind_estimate_sub, &updated); if (updated) { orb_copy(ORB_ID(wind_estimate), _wind_estimate_sub, &wind); } // Get vehicle position orb_check(vehicle_global_position_sub, &updated); if (updated) { // copy global position orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, &_global_pos); } // Get parameter updates orb_check(parameter_update_sub, &updated); if (updated) { // copy global position orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update); // update all parameters param_get(param_gproperties, &z_0); param_get(param_turn_radius, &turn_radius); param_get(param_precision, &precision); } orb_check(_command_sub, &updated); if (updated) { orb_copy(ORB_ID(vehicle_command), _command_sub, &_command); handle_command(&_command); } float windspeed_norm = sqrtf(wind.windspeed_north * wind.windspeed_north + wind.windspeed_east * wind.windspeed_east); float groundspeed_body = sqrtf(_global_pos.vel_n * _global_pos.vel_n + _global_pos.vel_e * _global_pos.vel_e); ground_distance = _global_pos.alt - _target_position.alt; // Distance to drop position and angle error to approach vector // are relevant in all states greater than target valid (which calculates these positions) if (_drop_state > DROP_STATE_TARGET_VALID) { distance_real = fabsf(get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, _drop_position.lat, _drop_position.lon)); float ground_direction = atan2f(_global_pos.vel_e, _global_pos.vel_n); float approach_direction = get_bearing_to_next_waypoint(flight_vector_s.lat, flight_vector_s.lon, flight_vector_e.lat, flight_vector_e.lon); approach_error = _wrap_pi(ground_direction - approach_direction); if (counter % 90 == 0) { mavlink_log_info(_mavlink_fd, "drop distance %u, heading error %u", (unsigned)distance_real, (unsigned)math::degrees(approach_error)); } } switch (_drop_state) { case DROP_STATE_INIT: // do nothing break; case DROP_STATE_TARGET_VALID: { az = g; // acceleration in z direction[m/s^2] vz = 0; // velocity in z direction [m/s] z = 0; // fallen distance [m] h_0 = _global_pos.alt - _target_position.alt; // height over target at start[m] h = h_0; // height over target [m] ax = 0; // acceleration in x direction [m/s^2] vx = groundspeed_body;// XXX project // ground speed in x direction [m/s] x = 0; // traveled distance in x direction [m] vw = 0; // wind speed [m/s] vrx = 0; // relative velocity in x direction [m/s] v = groundspeed_body; // relative speed vector [m/s] Fd = 0; // Drag force [N] Fdx = 0; // Drag force in x direction [N] Fdz = 0; // Drag force in z direction [N] // Compute the distance the bottle will travel after it is dropped in body frame coordinates --> x while (h > 0.05f) { // z-direction vz = vz + az * dt_freefall_prediction; z = z + vz * dt_freefall_prediction; h = h_0 - z; // x-direction vw = windspeed_norm * logf(h / z_0) / logf(ground_distance / z_0); vx = vx + ax * dt_freefall_prediction; x = x + vx * dt_freefall_prediction; vrx = vx + vw; // drag force v = sqrtf(vz * vz + vrx * vrx); Fd = 0.5f * rho * A * cd * (v * v); Fdx = Fd * vrx / v; Fdz = Fd * vz / v; // acceleration az = g - Fdz / m; ax = -Fdx / m; } // compute drop vector x = groundspeed_body * t_signal + x; x_t = 0.0f; y_t = 0.0f; float wind_direction_n, wind_direction_e; if (windspeed_norm < 0.5f) { // If there is no wind, an arbitrarily direction is chosen wind_direction_n = 1.0f; wind_direction_e = 0.0f; } else { wind_direction_n = wind.windspeed_north / windspeed_norm; wind_direction_e = wind.windspeed_east / windspeed_norm; } x_drop = x_t + x * wind_direction_n; y_drop = y_t + x * wind_direction_e; map_projection_reproject(&ref, x_drop, y_drop, &_drop_position.lat, &_drop_position.lon); _drop_position.alt = _target_position.alt + _alt_clearance; // Compute flight vector map_projection_reproject(&ref, x_drop + 2 * turn_radius * wind_direction_n, y_drop + 2 * turn_radius * wind_direction_e, &(flight_vector_s.lat), &(flight_vector_s.lon)); flight_vector_s.altitude = _drop_position.alt; map_projection_reproject(&ref, x_drop - turn_radius * wind_direction_n, y_drop - turn_radius * wind_direction_e, &flight_vector_e.lat, &flight_vector_e.lon); flight_vector_e.altitude = _drop_position.alt; // Save WPs in datamanager const ssize_t len = sizeof(struct mission_item_s); if (dm_write(DM_KEY_WAYPOINTS_ONBOARD, 0, DM_PERSIST_IN_FLIGHT_RESET, &flight_vector_s, len) != len) { warnx("ERROR: could not save onboard WP"); } if (dm_write(DM_KEY_WAYPOINTS_ONBOARD, 1, DM_PERSIST_IN_FLIGHT_RESET, &flight_vector_e, len) != len) { warnx("ERROR: could not save onboard WP"); } _onboard_mission.count = 2; _onboard_mission.current_seq = 0; if (_onboard_mission_pub > 0) { orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission); } else { _onboard_mission_pub = orb_advertise(ORB_ID(onboard_mission), &_onboard_mission); } float approach_direction = get_bearing_to_next_waypoint(flight_vector_s.lat, flight_vector_s.lon, flight_vector_e.lat, flight_vector_e.lon); mavlink_log_critical(_mavlink_fd, "position set, approach heading: %u", (unsigned)distance_real, (unsigned)math::degrees(approach_direction + M_PI_F)); _drop_state = DROP_STATE_TARGET_SET; } break; case DROP_STATE_TARGET_SET: { float distance_wp2 = get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, flight_vector_e.lat, flight_vector_e.lon); if (distance_wp2 < distance_real) { _onboard_mission.current_seq = 0; orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission); } else { // We're close enough - open the bay distance_open_door = math::max(10.0f, 3.0f * fabsf(t_door * groundspeed_body)); if (isfinite(distance_real) && distance_real < distance_open_door && fabsf(approach_error) < math::radians(20.0f)) { open_bay(); _drop_state = DROP_STATE_BAY_OPEN; mavlink_log_info(_mavlink_fd, "#audio: opening bay"); } } } break; case DROP_STATE_BAY_OPEN: { if (_drop_approval) { map_projection_project(&ref, _global_pos.lat, _global_pos.lon, &x_l, &y_l); x_f = x_l + _global_pos.vel_n * dt_runs; y_f = y_l + _global_pos.vel_e * dt_runs; map_projection_reproject(&ref, x_f, y_f, &x_f_NED, &y_f_NED); future_distance = get_distance_to_next_waypoint(x_f_NED, y_f_NED, _drop_position.lat, _drop_position.lon); if (isfinite(distance_real) && (distance_real < precision) && ((distance_real < future_distance))) { drop(); _drop_state = DROP_STATE_DROPPED; mavlink_log_info(_mavlink_fd, "#audio: payload dropped"); } else { float distance_wp2 = get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, flight_vector_e.lat, flight_vector_e.lon); if (distance_wp2 < distance_real) { _onboard_mission.current_seq = 0; orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission); } } } } break; case DROP_STATE_DROPPED: /* 2s after drop, reset and close everything again */ if ((hrt_elapsed_time(&_doors_opened) > 2 * 1000 * 1000)) { _drop_state = DROP_STATE_INIT; _drop_approval = false; lock_release(); close_bay(); mavlink_log_info(_mavlink_fd, "#audio: closing bay"); // remove onboard mission _onboard_mission.current_seq = -1; _onboard_mission.count = 0; orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission); } break; case DROP_STATE_BAY_CLOSED: // do nothing break; } counter++; // update_actuators(); // run at roughly 100 Hz usleep(sleeptime_us); dt_runs = hrt_elapsed_time(&last_run) / 1e6f; last_run = hrt_absolute_time(); } } warnx("exiting."); _main_task = -1; _exit(0); }
void PrecLand::run_state_horizontal_approach() { position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); // check if target visible, if not go to start if (!check_state_conditions(PrecLandState::HorizontalApproach)) { PX4_WARN("Lost landing target while landig (horizontal approach)."); // Stay at current position for searching for the landing target pos_sp_triplet->current.lat = _navigator->get_global_position()->lat; pos_sp_triplet->current.lon = _navigator->get_global_position()->lon; pos_sp_triplet->current.alt = _navigator->get_global_position()->alt; if (!switch_to_state_start()) { if (!switch_to_state_fallback()) { PX4_ERR("Can't switch to fallback landing"); } } return; } if (check_state_conditions(PrecLandState::DescendAboveTarget)) { if (!_point_reached_time) { _point_reached_time = hrt_absolute_time(); } if (hrt_absolute_time() - _point_reached_time > 2000000) { // if close enough for descent above target go to descend above target if (switch_to_state_descend_above_target()) { return; } } } if (hrt_absolute_time() - _state_start_time > STATE_TIMEOUT) { PX4_ERR("Precision landing took too long during horizontal approach phase."); if (switch_to_state_fallback()) { return; } PX4_ERR("Can't switch to fallback landing"); } float x = _target_pose.x_abs; float y = _target_pose.y_abs; slewrate(x, y); // XXX need to transform to GPS coords because mc_pos_control only looks at that double lat, lon; map_projection_reproject(&_map_ref, x, y, &lat, &lon); pos_sp_triplet->current.lat = lat; pos_sp_triplet->current.lon = lon; pos_sp_triplet->current.alt = _approach_alt; pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; _navigator->set_position_setpoint_triplet_updated(); }
__EXPORT int map_projection_global_reproject(float x, float y, double *lat, double *lon) { return map_projection_reproject(&mp_ref, x, y, lat, lon); }
void BlockLocalPositionEstimator::gpsInit() { // check for good gps signal uint8_t nSat = _sub_gps.get().satellites_used; float eph = _sub_gps.get().eph; float epv = _sub_gps.get().epv; uint8_t fix_type = _sub_gps.get().fix_type; if ( nSat < 6 || eph > _param_lpe_eph_max.get() || epv > _param_lpe_epv_max.get() || fix_type < 3 ) { _gpsStats.reset(); return; } // measure Vector<double, n_y_gps> y; if (gpsMeasure(y) != OK) { _gpsStats.reset(); return; } // if finished if (_gpsStats.getCount() > REQ_GPS_INIT_COUNT) { // get mean gps values double gpsLat = _gpsStats.getMean()(0); double gpsLon = _gpsStats.getMean()(1); float gpsAlt = _gpsStats.getMean()(2); _sensorTimeout &= ~SENSOR_GPS; _sensorFault &= ~SENSOR_GPS; _gpsStats.reset(); if (!_receivedGps) { // this is the first time we have received gps _receivedGps = true; // note we subtract X_z which is in down directon so it is // an addition _gpsAltOrigin = gpsAlt + _x(X_z); // find lat, lon of current origin by subtracting x and y // if not using vision position since vision will // have it's own origin, not necessarily where vehicle starts if (!_map_ref.init_done && !(_param_lpe_fusion.get() & FUSE_VIS_POS)) { double gpsLatOrigin = 0; double gpsLonOrigin = 0; // reproject at current coordinates map_projection_init(&_map_ref, gpsLat, gpsLon); // find origin map_projection_reproject(&_map_ref, -_x(X_x), -_x(X_y), &gpsLatOrigin, &gpsLonOrigin); // reinit origin map_projection_init(&_map_ref, gpsLatOrigin, gpsLonOrigin); // set timestamp when origin was set to current time _time_origin = _timeStamp; // always override alt origin on first GPS to fix // possible baro offset in global altitude at init _altOrigin = _gpsAltOrigin; _altOriginInitialized = true; _altOriginGlobal = true; mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] global origin init (gps) : lat %6.2f lon %6.2f alt %5.1f m", gpsLatOrigin, gpsLonOrigin, double(_gpsAltOrigin)); } PX4_INFO("[lpe] gps init " "lat %6.2f lon %6.2f alt %5.1f m", gpsLat, gpsLon, double(gpsAlt)); } } }
void Ekf2::task_main() { // subscribe to relevant topics int sensors_sub = orb_subscribe(ORB_ID(sensor_combined)); int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); int airspeed_sub = orb_subscribe(ORB_ID(airspeed)); int params_sub = orb_subscribe(ORB_ID(parameter_update)); int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow)); int range_finder_sub = orb_subscribe(ORB_ID(distance_sensor)); int ev_pos_sub = orb_subscribe(ORB_ID(vehicle_vision_position)); int ev_att_sub = orb_subscribe(ORB_ID(vehicle_vision_attitude)); int vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected)); int status_sub = orb_subscribe(ORB_ID(vehicle_status)); px4_pollfd_struct_t fds[2] = {}; fds[0].fd = sensors_sub; fds[0].events = POLLIN; fds[1].fd = params_sub; fds[1].events = POLLIN; // initialise parameter cache updateParams(); // initialize data structures outside of loop // because they will else not always be // properly populated sensor_combined_s sensors = {}; vehicle_gps_position_s gps = {}; airspeed_s airspeed = {}; optical_flow_s optical_flow = {}; distance_sensor_s range_finder = {}; vehicle_land_detected_s vehicle_land_detected = {}; vehicle_local_position_s ev_pos = {}; vehicle_attitude_s ev_att = {}; vehicle_status_s vehicle_status = {}; while (!_task_should_exit) { int ret = px4_poll(fds, sizeof(fds) / sizeof(fds[0]), 1000); if (ret < 0) { // Poll error, sleep and try again usleep(10000); continue; } else if (ret == 0) { // Poll timeout or no new data, do nothing continue; } if (fds[1].revents & POLLIN) { // read from param to clear updated flag struct parameter_update_s update; orb_copy(ORB_ID(parameter_update), params_sub, &update); updateParams(); // fetch sensor data in next loop continue; } else if (!(fds[0].revents & POLLIN)) { // no new data continue; } bool gps_updated = false; bool airspeed_updated = false; bool optical_flow_updated = false; bool range_finder_updated = false; bool vehicle_land_detected_updated = false; bool vision_position_updated = false; bool vision_attitude_updated = false; bool vehicle_status_updated = false; orb_copy(ORB_ID(sensor_combined), sensors_sub, &sensors); // update all other topics if they have new data orb_check(status_sub, &vehicle_status_updated); if (vehicle_status_updated) { orb_copy(ORB_ID(vehicle_status), status_sub, &vehicle_status); } orb_check(gps_sub, &gps_updated); if (gps_updated) { orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps); } orb_check(airspeed_sub, &airspeed_updated); if (airspeed_updated) { orb_copy(ORB_ID(airspeed), airspeed_sub, &airspeed); } orb_check(optical_flow_sub, &optical_flow_updated); if (optical_flow_updated) { orb_copy(ORB_ID(optical_flow), optical_flow_sub, &optical_flow); } orb_check(range_finder_sub, &range_finder_updated); if (range_finder_updated) { orb_copy(ORB_ID(distance_sensor), range_finder_sub, &range_finder); if (range_finder.min_distance > range_finder.current_distance || range_finder.max_distance < range_finder.current_distance) { range_finder_updated = false; } } orb_check(ev_pos_sub, &vision_position_updated); if (vision_position_updated) { orb_copy(ORB_ID(vehicle_vision_position), ev_pos_sub, &ev_pos); } orb_check(ev_att_sub, &vision_attitude_updated); if (vision_attitude_updated) { orb_copy(ORB_ID(vehicle_vision_attitude), ev_att_sub, &ev_att); } // in replay mode we are getting the actual timestamp from the sensor topic hrt_abstime now = 0; if (_replay_mode) { now = sensors.timestamp; } else { now = hrt_absolute_time(); } // push imu data into estimator float gyro_integral[3]; gyro_integral[0] = sensors.gyro_rad[0] * sensors.gyro_integral_dt; gyro_integral[1] = sensors.gyro_rad[1] * sensors.gyro_integral_dt; gyro_integral[2] = sensors.gyro_rad[2] * sensors.gyro_integral_dt; float accel_integral[3]; accel_integral[0] = sensors.accelerometer_m_s2[0] * sensors.accelerometer_integral_dt; accel_integral[1] = sensors.accelerometer_m_s2[1] * sensors.accelerometer_integral_dt; accel_integral[2] = sensors.accelerometer_m_s2[2] * sensors.accelerometer_integral_dt; _ekf.setIMUData(now, sensors.gyro_integral_dt * 1.e6f, sensors.accelerometer_integral_dt * 1.e6f, gyro_integral, accel_integral); // read mag data if (sensors.magnetometer_timestamp_relative == sensor_combined_s::RELATIVE_TIMESTAMP_INVALID) { // set a zero timestamp to let the ekf replay program know that this data is not valid _timestamp_mag_us = 0; } else { if ((sensors.timestamp + sensors.magnetometer_timestamp_relative) != _timestamp_mag_us) { _timestamp_mag_us = sensors.timestamp + sensors.magnetometer_timestamp_relative; // If the time last used by the EKF is less than specified, then accumulate the // data and push the average when the 50msec is reached. _mag_time_sum_ms += _timestamp_mag_us / 1000; _mag_sample_count++; _mag_data_sum[0] += sensors.magnetometer_ga[0]; _mag_data_sum[1] += sensors.magnetometer_ga[1]; _mag_data_sum[2] += sensors.magnetometer_ga[2]; uint32_t mag_time_ms = _mag_time_sum_ms / _mag_sample_count; if (mag_time_ms - _mag_time_ms_last_used > _params->sensor_interval_min_ms) { float mag_sample_count_inv = 1.0f / (float)_mag_sample_count; float mag_data_avg_ga[3] = {_mag_data_sum[0] *mag_sample_count_inv, _mag_data_sum[1] *mag_sample_count_inv, _mag_data_sum[2] *mag_sample_count_inv}; _ekf.setMagData(1000 * (uint64_t)mag_time_ms, mag_data_avg_ga); _mag_time_ms_last_used = mag_time_ms; _mag_time_sum_ms = 0; _mag_sample_count = 0; _mag_data_sum[0] = 0.0f; _mag_data_sum[1] = 0.0f; _mag_data_sum[2] = 0.0f; } } } // read baro data if (sensors.baro_timestamp_relative == sensor_combined_s::RELATIVE_TIMESTAMP_INVALID) { // set a zero timestamp to let the ekf replay program know that this data is not valid _timestamp_balt_us = 0; } else { if ((sensors.timestamp + sensors.baro_timestamp_relative) != _timestamp_balt_us) { _timestamp_balt_us = sensors.timestamp + sensors.baro_timestamp_relative; // If the time last used by the EKF is less than specified, then accumulate the // data and push the average when the 50msec is reached. _balt_time_sum_ms += _timestamp_balt_us / 1000; _balt_sample_count++; _balt_data_sum += sensors.baro_alt_meter; uint32_t balt_time_ms = _balt_time_sum_ms / _balt_sample_count; if (balt_time_ms - _balt_time_ms_last_used > (uint32_t)_params->sensor_interval_min_ms) { float balt_data_avg = _balt_data_sum / (float)_balt_sample_count; _ekf.setBaroData(1000 * (uint64_t)balt_time_ms, balt_data_avg); _balt_time_ms_last_used = balt_time_ms; _balt_time_sum_ms = 0; _balt_sample_count = 0; _balt_data_sum = 0.0f; } } } // read gps data if available if (gps_updated) { struct gps_message gps_msg = {}; gps_msg.time_usec = gps.timestamp; gps_msg.lat = gps.lat; gps_msg.lon = gps.lon; gps_msg.alt = gps.alt; gps_msg.fix_type = gps.fix_type; gps_msg.eph = gps.eph; gps_msg.epv = gps.epv; gps_msg.sacc = gps.s_variance_m_s; gps_msg.vel_m_s = gps.vel_m_s; gps_msg.vel_ned[0] = gps.vel_n_m_s; gps_msg.vel_ned[1] = gps.vel_e_m_s; gps_msg.vel_ned[2] = gps.vel_d_m_s; gps_msg.vel_ned_valid = gps.vel_ned_valid; gps_msg.nsats = gps.satellites_used; //TODO add gdop to gps topic gps_msg.gdop = 0.0f; _ekf.setGpsData(gps.timestamp, &gps_msg); } // only set airspeed data if condition for airspeed fusion are met bool fuse_airspeed = airspeed_updated && !vehicle_status.is_rotary_wing && _arspFusionThreshold.get() <= airspeed.true_airspeed_m_s && _arspFusionThreshold.get() >= 0.1f; if (fuse_airspeed) { float eas2tas = airspeed.true_airspeed_m_s / airspeed.indicated_airspeed_m_s; _ekf.setAirspeedData(airspeed.timestamp, airspeed.true_airspeed_m_s, eas2tas); } // only fuse synthetic sideslip measurements if conditions are met bool fuse_beta = !vehicle_status.is_rotary_wing && _fuseBeta.get(); _ekf.set_fuse_beta_flag(fuse_beta); if (optical_flow_updated) { flow_message flow; flow.flowdata(0) = optical_flow.pixel_flow_x_integral; flow.flowdata(1) = optical_flow.pixel_flow_y_integral; flow.quality = optical_flow.quality; flow.gyrodata(0) = optical_flow.gyro_x_rate_integral; flow.gyrodata(1) = optical_flow.gyro_y_rate_integral; flow.gyrodata(2) = optical_flow.gyro_z_rate_integral; flow.dt = optical_flow.integration_timespan; if (PX4_ISFINITE(optical_flow.pixel_flow_y_integral) && PX4_ISFINITE(optical_flow.pixel_flow_x_integral)) { _ekf.setOpticalFlowData(optical_flow.timestamp, &flow); } } if (range_finder_updated) { _ekf.setRangeData(range_finder.timestamp, range_finder.current_distance); } // get external vision data // if error estimates are unavailable, use parameter defined defaults if (vision_position_updated || vision_attitude_updated) { ext_vision_message ev_data; ev_data.posNED(0) = ev_pos.x; ev_data.posNED(1) = ev_pos.y; ev_data.posNED(2) = ev_pos.z; Quaternion q(ev_att.q); ev_data.quat = q; // position measurement error from parameters. TODO : use covariances from topic ev_data.posErr = _default_ev_pos_noise; ev_data.angErr = _default_ev_ang_noise; // use timestamp from external computer, clocks are synchronized when using MAVROS _ekf.setExtVisionData(vision_position_updated ? ev_pos.timestamp : ev_att.timestamp, &ev_data); } orb_check(vehicle_land_detected_sub, &vehicle_land_detected_updated); if (vehicle_land_detected_updated) { orb_copy(ORB_ID(vehicle_land_detected), vehicle_land_detected_sub, &vehicle_land_detected); _ekf.set_in_air_status(!vehicle_land_detected.landed); } // run the EKF update and output if (_ekf.update()) { matrix::Quaternion<float> q; _ekf.copy_quaternion(q.data()); float velocity[3]; _ekf.get_velocity(velocity); float gyro_rad[3]; { // generate control state data control_state_s ctrl_state = {}; float gyro_bias[3] = {}; _ekf.get_gyro_bias(gyro_bias); ctrl_state.timestamp = _replay_mode ? now : hrt_absolute_time(); gyro_rad[0] = sensors.gyro_rad[0] - gyro_bias[0]; gyro_rad[1] = sensors.gyro_rad[1] - gyro_bias[1]; gyro_rad[2] = sensors.gyro_rad[2] - gyro_bias[2]; ctrl_state.roll_rate = _lp_roll_rate.apply(gyro_rad[0]); ctrl_state.pitch_rate = _lp_pitch_rate.apply(gyro_rad[1]); ctrl_state.yaw_rate = _lp_yaw_rate.apply(gyro_rad[2]); ctrl_state.roll_rate_bias = gyro_bias[0]; ctrl_state.pitch_rate_bias = gyro_bias[1]; ctrl_state.yaw_rate_bias = gyro_bias[2]; // Velocity in body frame Vector3f v_n(velocity); matrix::Dcm<float> R_to_body(q.inversed()); Vector3f v_b = R_to_body * v_n; ctrl_state.x_vel = v_b(0); ctrl_state.y_vel = v_b(1); ctrl_state.z_vel = v_b(2); // Local Position NED float position[3]; _ekf.get_position(position); ctrl_state.x_pos = position[0]; ctrl_state.y_pos = position[1]; ctrl_state.z_pos = position[2]; // Attitude quaternion q.copyTo(ctrl_state.q); _ekf.get_quat_reset(&ctrl_state.delta_q_reset[0], &ctrl_state.quat_reset_counter); // Acceleration data matrix::Vector<float, 3> acceleration(sensors.accelerometer_m_s2); float accel_bias[3]; _ekf.get_accel_bias(accel_bias); ctrl_state.x_acc = acceleration(0) - accel_bias[0]; ctrl_state.y_acc = acceleration(1) - accel_bias[1]; ctrl_state.z_acc = acceleration(2) - accel_bias[2]; // compute lowpass filtered horizontal acceleration acceleration = R_to_body.transpose() * acceleration; _acc_hor_filt = 0.95f * _acc_hor_filt + 0.05f * sqrtf(acceleration(0) * acceleration(0) + acceleration(1) * acceleration(1)); ctrl_state.horz_acc_mag = _acc_hor_filt; ctrl_state.airspeed_valid = false; // use estimated velocity for airspeed estimate if (_airspeed_mode.get() == control_state_s::AIRSPD_MODE_MEAS) { // use measured airspeed if (PX4_ISFINITE(airspeed.indicated_airspeed_m_s) && hrt_absolute_time() - airspeed.timestamp < 1e6 && airspeed.timestamp > 0) { ctrl_state.airspeed = airspeed.indicated_airspeed_m_s; ctrl_state.airspeed_valid = true; } } else if (_airspeed_mode.get() == control_state_s::AIRSPD_MODE_EST) { if (_ekf.local_position_is_valid()) { ctrl_state.airspeed = sqrtf(velocity[0] * velocity[0] + velocity[1] * velocity[1] + velocity[2] * velocity[2]); ctrl_state.airspeed_valid = true; } } else if (_airspeed_mode.get() == control_state_s::AIRSPD_MODE_DISABLED) { // do nothing, airspeed has been declared as non-valid above, controllers // will handle this assuming always trim airspeed } // publish control state data if (_control_state_pub == nullptr) { _control_state_pub = orb_advertise(ORB_ID(control_state), &ctrl_state); } else { orb_publish(ORB_ID(control_state), _control_state_pub, &ctrl_state); } } { // generate vehicle attitude quaternion data struct vehicle_attitude_s att = {}; att.timestamp = _replay_mode ? now : hrt_absolute_time(); q.copyTo(att.q); att.rollspeed = gyro_rad[0]; att.pitchspeed = gyro_rad[1]; att.yawspeed = gyro_rad[2]; // publish vehicle attitude data if (_att_pub == nullptr) { _att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att); } else { orb_publish(ORB_ID(vehicle_attitude), _att_pub, &att); } } // generate vehicle local position data struct vehicle_local_position_s lpos = {}; float pos[3] = {}; lpos.timestamp = _replay_mode ? now : hrt_absolute_time(); // Position of body origin in local NED frame _ekf.get_position(pos); lpos.x = (_ekf.local_position_is_valid()) ? pos[0] : 0.0f; lpos.y = (_ekf.local_position_is_valid()) ? pos[1] : 0.0f; lpos.z = pos[2]; // Velocity of body origin in local NED frame (m/s) lpos.vx = velocity[0]; lpos.vy = velocity[1]; lpos.vz = velocity[2]; // TODO: better status reporting lpos.xy_valid = _ekf.local_position_is_valid(); lpos.z_valid = true; lpos.v_xy_valid = _ekf.local_position_is_valid(); lpos.v_z_valid = true; // Position of local NED origin in GPS / WGS84 frame struct map_projection_reference_s ekf_origin = {}; // true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon) _ekf.get_ekf_origin(&lpos.ref_timestamp, &ekf_origin, &lpos.ref_alt); lpos.xy_global = _ekf.global_position_is_valid(); lpos.z_global = true; // true if z is valid and has valid global reference (ref_alt) lpos.ref_lat = ekf_origin.lat_rad * 180.0 / M_PI; // Reference point latitude in degrees lpos.ref_lon = ekf_origin.lon_rad * 180.0 / M_PI; // Reference point longitude in degrees // The rotation of the tangent plane vs. geographical north matrix::Eulerf euler(q); lpos.yaw = euler.psi(); float terrain_vpos; lpos.dist_bottom_valid = _ekf.get_terrain_vert_pos(&terrain_vpos); lpos.dist_bottom = terrain_vpos - pos[2]; // Distance to bottom surface (ground) in meters lpos.dist_bottom_rate = -velocity[2]; // Distance to bottom surface (ground) change rate lpos.surface_bottom_timestamp = hrt_absolute_time(); // Time when new bottom surface found // TODO: uORB definition does not define what these variables are. We have assumed them to be horizontal and vertical 1-std dev accuracy in metres Vector3f pos_var, vel_var; _ekf.get_pos_var(pos_var); _ekf.get_vel_var(vel_var); lpos.eph = sqrtf(pos_var(0) + pos_var(1)); lpos.epv = sqrtf(pos_var(2)); // get state reset information of position and velocity _ekf.get_posD_reset(&lpos.delta_z, &lpos.z_reset_counter); _ekf.get_velD_reset(&lpos.delta_vz, &lpos.vz_reset_counter); _ekf.get_posNE_reset(&lpos.delta_xy[0], &lpos.xy_reset_counter); _ekf.get_velNE_reset(&lpos.delta_vxy[0], &lpos.vxy_reset_counter); // publish vehicle local position data if (_lpos_pub == nullptr) { _lpos_pub = orb_advertise(ORB_ID(vehicle_local_position), &lpos); } else { orb_publish(ORB_ID(vehicle_local_position), _lpos_pub, &lpos); } if (_ekf.global_position_is_valid()) { // generate and publish global position data struct vehicle_global_position_s global_pos = {}; global_pos.timestamp = _replay_mode ? now : hrt_absolute_time(); global_pos.time_utc_usec = gps.time_utc_usec; // GPS UTC timestamp in microseconds double est_lat, est_lon, lat_pre_reset, lon_pre_reset; map_projection_reproject(&ekf_origin, lpos.x, lpos.y, &est_lat, &est_lon); global_pos.lat = est_lat; // Latitude in degrees global_pos.lon = est_lon; // Longitude in degrees map_projection_reproject(&ekf_origin, lpos.x - lpos.delta_xy[0], lpos.y - lpos.delta_xy[1], &lat_pre_reset, &lon_pre_reset); global_pos.delta_lat_lon[0] = est_lat - lat_pre_reset; global_pos.delta_lat_lon[1] = est_lon - lon_pre_reset; global_pos.lat_lon_reset_counter = lpos.xy_reset_counter; global_pos.alt = -pos[2] + lpos.ref_alt; // Altitude AMSL in meters _ekf.get_posD_reset(&global_pos.delta_alt, &global_pos.alt_reset_counter); // global altitude has opposite sign of local down position global_pos.delta_alt *= -1.0f; global_pos.vel_n = velocity[0]; // Ground north velocity, m/s global_pos.vel_e = velocity[1]; // Ground east velocity, m/s global_pos.vel_d = velocity[2]; // Ground downside velocity, m/s global_pos.yaw = euler.psi(); // Yaw in radians -PI..+PI. global_pos.eph = sqrtf(pos_var(0) + pos_var(1));; // Standard deviation of position estimate horizontally global_pos.epv = sqrtf(pos_var(2)); // Standard deviation of position vertically if (lpos.dist_bottom_valid) { global_pos.terrain_alt = lpos.ref_alt - terrain_vpos; // Terrain altitude in m, WGS84 global_pos.terrain_alt_valid = true; // Terrain altitude estimate is valid } else { global_pos.terrain_alt = 0.0f; // Terrain altitude in m, WGS84 global_pos.terrain_alt_valid = false; // Terrain altitude estimate is valid } // TODO use innovatun consistency check timouts to set this global_pos.dead_reckoning = false; // True if this position is estimated through dead-reckoning global_pos.pressure_alt = sensors.baro_alt_meter; // Pressure altitude AMSL (m) if (_vehicle_global_position_pub == nullptr) { _vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos); } else { orb_publish(ORB_ID(vehicle_global_position), _vehicle_global_position_pub, &global_pos); } } } else if (_replay_mode) { // in replay mode we have to tell the replay module not to wait for an update // we do this by publishing an attitude with zero timestamp struct vehicle_attitude_s att = {}; att.timestamp = now; if (_att_pub == nullptr) { _att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att); } else { orb_publish(ORB_ID(vehicle_attitude), _att_pub, &att); } } // publish estimator status struct estimator_status_s status = {}; status.timestamp = _replay_mode ? now : hrt_absolute_time(); _ekf.get_state_delayed(status.states); _ekf.get_covariances(status.covariances); _ekf.get_gps_check_status(&status.gps_check_fail_flags); _ekf.get_control_mode(&status.control_mode_flags); _ekf.get_filter_fault_status(&status.filter_fault_flags); _ekf.get_innovation_test_status(&status.innovation_check_flags, &status.mag_test_ratio, &status.vel_test_ratio, &status.pos_test_ratio, &status.hgt_test_ratio, &status.tas_test_ratio, &status.hagl_test_ratio); bool dead_reckoning; _ekf.get_ekf_lpos_accuracy(&status.pos_horiz_accuracy, &status.pos_vert_accuracy, &dead_reckoning); _ekf.get_ekf_soln_status(&status.solution_status_flags); _ekf.get_imu_vibe_metrics(status.vibe); if (_estimator_status_pub == nullptr) { _estimator_status_pub = orb_advertise(ORB_ID(estimator_status), &status); } else { orb_publish(ORB_ID(estimator_status), _estimator_status_pub, &status); } // Publish wind estimate struct wind_estimate_s wind_estimate = {}; wind_estimate.timestamp = _replay_mode ? now : hrt_absolute_time(); wind_estimate.windspeed_north = status.states[22]; wind_estimate.windspeed_east = status.states[23]; wind_estimate.covariance_north = status.covariances[22]; wind_estimate.covariance_east = status.covariances[23]; if (_wind_pub == nullptr) { _wind_pub = orb_advertise(ORB_ID(wind_estimate), &wind_estimate); } else { orb_publish(ORB_ID(wind_estimate), _wind_pub, &wind_estimate); } // publish estimator innovation data { struct ekf2_innovations_s innovations = {}; innovations.timestamp = _replay_mode ? now : hrt_absolute_time(); _ekf.get_vel_pos_innov(&innovations.vel_pos_innov[0]); _ekf.get_mag_innov(&innovations.mag_innov[0]); _ekf.get_heading_innov(&innovations.heading_innov); _ekf.get_airspeed_innov(&innovations.airspeed_innov); _ekf.get_beta_innov(&innovations.beta_innov); _ekf.get_flow_innov(&innovations.flow_innov[0]); _ekf.get_hagl_innov(&innovations.hagl_innov); _ekf.get_vel_pos_innov_var(&innovations.vel_pos_innov_var[0]); _ekf.get_mag_innov_var(&innovations.mag_innov_var[0]); _ekf.get_heading_innov_var(&innovations.heading_innov_var); _ekf.get_airspeed_innov_var(&innovations.airspeed_innov_var); _ekf.get_beta_innov_var(&innovations.beta_innov_var); _ekf.get_flow_innov_var(&innovations.flow_innov_var[0]); _ekf.get_hagl_innov_var(&innovations.hagl_innov_var); _ekf.get_output_tracking_error(&innovations.output_tracking_error[0]); if (_estimator_innovations_pub == nullptr) { _estimator_innovations_pub = orb_advertise(ORB_ID(ekf2_innovations), &innovations); } else { orb_publish(ORB_ID(ekf2_innovations), _estimator_innovations_pub, &innovations); } } // save the declination to the EKF2_MAG_DECL parameter when a land event is detected if ((_params->mag_declination_source & (1 << 1)) && !_prev_landed && vehicle_land_detected.landed) { float decl_deg; _ekf.copy_mag_decl_deg(&decl_deg); _mag_declination_deg.set(decl_deg); } _prev_landed = vehicle_land_detected.landed; // publish ekf2_timestamps (using 0.1 ms relative timestamps) { ekf2_timestamps_s ekf2_timestamps; ekf2_timestamps.timestamp = sensors.timestamp; if (gps_updated) { // divide individually to get consistent rounding behavior ekf2_timestamps.gps_timestamp_rel = (int16_t)((int64_t)gps.timestamp / 100 - (int64_t)ekf2_timestamps.timestamp / 100); } else { ekf2_timestamps.gps_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID; } if (optical_flow_updated) { ekf2_timestamps.optical_flow_timestamp_rel = (int16_t)((int64_t)optical_flow.timestamp / 100 - (int64_t)ekf2_timestamps.timestamp / 100); } else { ekf2_timestamps.optical_flow_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID; } if (range_finder_updated) { ekf2_timestamps.distance_sensor_timestamp_rel = (int16_t)((int64_t)range_finder.timestamp / 100 - (int64_t)ekf2_timestamps.timestamp / 100); } else { ekf2_timestamps.distance_sensor_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID; } if (airspeed_updated) { ekf2_timestamps.airspeed_timestamp_rel = (int16_t)((int64_t)airspeed.timestamp / 100 - (int64_t)ekf2_timestamps.timestamp / 100); } else { ekf2_timestamps.airspeed_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID; } if (vision_position_updated) { ekf2_timestamps.vision_position_timestamp_rel = (int16_t)((int64_t)ev_pos.timestamp / 100 - (int64_t)ekf2_timestamps.timestamp / 100); } else { ekf2_timestamps.vision_position_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID; } if (vision_attitude_updated) { ekf2_timestamps.vision_attitude_timestamp_rel = (int16_t)((int64_t)ev_att.timestamp / 100 - (int64_t)ekf2_timestamps.timestamp / 100); } else { ekf2_timestamps.vision_attitude_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID; } if (_ekf2_timestamps_pub == nullptr) { _ekf2_timestamps_pub = orb_advertise(ORB_ID(ekf2_timestamps), &ekf2_timestamps); } else { orb_publish(ORB_ID(ekf2_timestamps), _ekf2_timestamps_pub, &ekf2_timestamps); } } // publish replay message if in replay mode bool publish_replay_message = (bool)_param_record_replay_msg.get(); if (publish_replay_message) { struct ekf2_replay_s replay = {}; replay.timestamp = now; replay.gyro_integral_dt = sensors.gyro_integral_dt; replay.accelerometer_integral_dt = sensors.accelerometer_integral_dt; replay.magnetometer_timestamp = _timestamp_mag_us; replay.baro_timestamp = _timestamp_balt_us; memcpy(replay.gyro_rad, sensors.gyro_rad, sizeof(replay.gyro_rad)); memcpy(replay.accelerometer_m_s2, sensors.accelerometer_m_s2, sizeof(replay.accelerometer_m_s2)); memcpy(replay.magnetometer_ga, sensors.magnetometer_ga, sizeof(replay.magnetometer_ga)); replay.baro_alt_meter = sensors.baro_alt_meter; // only write gps data if we had a gps update. if (gps_updated) { replay.time_usec = gps.timestamp; replay.lat = gps.lat; replay.lon = gps.lon; replay.alt = gps.alt; replay.fix_type = gps.fix_type; replay.nsats = gps.satellites_used; replay.eph = gps.eph; replay.epv = gps.epv; replay.sacc = gps.s_variance_m_s; replay.vel_m_s = gps.vel_m_s; replay.vel_n_m_s = gps.vel_n_m_s; replay.vel_e_m_s = gps.vel_e_m_s; replay.vel_d_m_s = gps.vel_d_m_s; replay.vel_ned_valid = gps.vel_ned_valid; } else { // this will tell the logging app not to bother logging any gps replay data replay.time_usec = 0; } if (optical_flow_updated) { replay.flow_timestamp = optical_flow.timestamp; replay.flow_pixel_integral[0] = optical_flow.pixel_flow_x_integral; replay.flow_pixel_integral[1] = optical_flow.pixel_flow_y_integral; replay.flow_gyro_integral[0] = optical_flow.gyro_x_rate_integral; replay.flow_gyro_integral[1] = optical_flow.gyro_y_rate_integral; replay.flow_time_integral = optical_flow.integration_timespan; replay.flow_quality = optical_flow.quality; } else { replay.flow_timestamp = 0; } if (range_finder_updated) { replay.rng_timestamp = range_finder.timestamp; replay.range_to_ground = range_finder.current_distance; } else { replay.rng_timestamp = 0; } if (airspeed_updated) { replay.asp_timestamp = airspeed.timestamp; replay.indicated_airspeed_m_s = airspeed.indicated_airspeed_m_s; replay.true_airspeed_m_s = airspeed.true_airspeed_m_s; } else { replay.asp_timestamp = 0; } if (vision_position_updated || vision_attitude_updated) { replay.ev_timestamp = vision_position_updated ? ev_pos.timestamp : ev_att.timestamp; replay.pos_ev[0] = ev_pos.x; replay.pos_ev[1] = ev_pos.y; replay.pos_ev[2] = ev_pos.z; replay.quat_ev[0] = ev_att.q[0]; replay.quat_ev[1] = ev_att.q[1]; replay.quat_ev[2] = ev_att.q[2]; replay.quat_ev[3] = ev_att.q[3]; // TODO : switch to covariances from topic later replay.pos_err = _default_ev_pos_noise; replay.ang_err = _default_ev_ang_noise; } else { replay.ev_timestamp = 0; } if (_replay_pub == nullptr) { _replay_pub = orb_advertise(ORB_ID(ekf2_replay), &replay); } else { orb_publish(ORB_ID(ekf2_replay), _replay_pub, &replay); } } } orb_unsubscribe(sensors_sub); orb_unsubscribe(gps_sub); orb_unsubscribe(airspeed_sub); orb_unsubscribe(params_sub); orb_unsubscribe(optical_flow_sub); orb_unsubscribe(range_finder_sub); orb_unsubscribe(ev_pos_sub); orb_unsubscribe(vehicle_land_detected_sub); orb_unsubscribe(status_sub); delete ekf2::instance; ekf2::instance = nullptr; }
/**************************************************************************** * main ****************************************************************************/ int position_estimator_inav_thread_main(int argc, char *argv[]) { int mavlink_fd; mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0); float x_est[2] = { 0.0f, 0.0f }; // pos, vel float y_est[2] = { 0.0f, 0.0f }; // pos, vel float z_est[2] = { 0.0f, 0.0f }; // pos, vel float est_buf[EST_BUF_SIZE][3][2]; // estimated position buffer float R_buf[EST_BUF_SIZE][3][3]; // rotation matrix buffer float R_gps[3][3]; // rotation matrix for GPS correction moment memset(est_buf, 0, sizeof(est_buf)); memset(R_buf, 0, sizeof(R_buf)); memset(R_gps, 0, sizeof(R_gps)); int buf_ptr = 0; static const float min_eph_epv = 2.0f; // min EPH/EPV, used for weight calculation static const float max_eph_epv = 20.0f; // max EPH/EPV acceptable for estimation float eph = max_eph_epv; float epv = 1.0f; float eph_flow = 1.0f; float eph_vision = 0.2f; float epv_vision = 0.2f; float eph_mocap = 0.05f; float epv_mocap = 0.05f; float x_est_prev[2], y_est_prev[2], z_est_prev[2]; memset(x_est_prev, 0, sizeof(x_est_prev)); memset(y_est_prev, 0, sizeof(y_est_prev)); memset(z_est_prev, 0, sizeof(z_est_prev)); int baro_init_cnt = 0; int baro_init_num = 200; float baro_offset = 0.0f; // baro offset for reference altitude, initialized on start, then adjusted float surface_offset = 0.0f; // ground level offset from reference altitude float surface_offset_rate = 0.0f; // surface offset change rate hrt_abstime accel_timestamp = 0; hrt_abstime baro_timestamp = 0; bool ref_inited = false; hrt_abstime ref_init_start = 0; const hrt_abstime ref_init_delay = 1000000; // wait for 1s after 3D fix struct map_projection_reference_s ref; memset(&ref, 0, sizeof(ref)); hrt_abstime home_timestamp = 0; uint16_t accel_updates = 0; uint16_t baro_updates = 0; uint16_t gps_updates = 0; uint16_t attitude_updates = 0; uint16_t flow_updates = 0; uint16_t vision_updates = 0; uint16_t mocap_updates = 0; hrt_abstime updates_counter_start = hrt_absolute_time(); hrt_abstime pub_last = hrt_absolute_time(); hrt_abstime t_prev = 0; /* store error when sensor updates, but correct on each time step to avoid jumps in estimated value */ float acc[] = { 0.0f, 0.0f, 0.0f }; // N E D float acc_bias[] = { 0.0f, 0.0f, 0.0f }; // body frame float corr_baro = 0.0f; // D float corr_gps[3][2] = { { 0.0f, 0.0f }, // N (pos, vel) { 0.0f, 0.0f }, // E (pos, vel) { 0.0f, 0.0f }, // D (pos, vel) }; float w_gps_xy = 1.0f; float w_gps_z = 1.0f; float corr_vision[3][2] = { { 0.0f, 0.0f }, // N (pos, vel) { 0.0f, 0.0f }, // E (pos, vel) { 0.0f, 0.0f }, // D (pos, vel) }; float corr_mocap[3][1] = { { 0.0f }, // N (pos) { 0.0f }, // E (pos) { 0.0f }, // D (pos) }; float corr_sonar = 0.0f; float corr_sonar_filtered = 0.0f; float corr_flow[] = { 0.0f, 0.0f }; // N E float w_flow = 0.0f; float sonar_prev = 0.0f; //hrt_abstime flow_prev = 0; // time of last flow measurement hrt_abstime sonar_time = 0; // time of last sonar measurement (not filtered) hrt_abstime sonar_valid_time = 0; // time of last sonar measurement used for correction (filtered) bool gps_valid = false; // GPS is valid bool sonar_valid = false; // sonar is valid bool flow_valid = false; // flow is valid bool flow_accurate = false; // flow should be accurate (this flag not updated if flow_valid == false) bool vision_valid = false; // vision is valid bool mocap_valid = false; // mocap is valid /* declare and safely initialize all structs */ struct actuator_controls_s actuator; memset(&actuator, 0, sizeof(actuator)); struct actuator_armed_s armed; memset(&armed, 0, sizeof(armed)); struct sensor_combined_s sensor; memset(&sensor, 0, sizeof(sensor)); struct vehicle_gps_position_s gps; memset(&gps, 0, sizeof(gps)); struct home_position_s home; memset(&home, 0, sizeof(home)); struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); struct vehicle_local_position_s local_pos; memset(&local_pos, 0, sizeof(local_pos)); struct optical_flow_s flow; memset(&flow, 0, sizeof(flow)); struct vision_position_estimate_s vision; memset(&vision, 0, sizeof(vision)); struct att_pos_mocap_s mocap; memset(&mocap, 0, sizeof(mocap)); struct vehicle_global_position_s global_pos; memset(&global_pos, 0, sizeof(global_pos)); /* subscribe */ int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); int actuator_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); int armed_sub = orb_subscribe(ORB_ID(actuator_armed)); int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow)); int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); int vision_position_estimate_sub = orb_subscribe(ORB_ID(vision_position_estimate)); int att_pos_mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap)); int home_position_sub = orb_subscribe(ORB_ID(home_position)); /* advertise */ orb_advert_t vehicle_local_position_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos); orb_advert_t vehicle_global_position_pub = NULL; struct position_estimator_inav_params params; struct position_estimator_inav_param_handles pos_inav_param_handles; /* initialize parameter handles */ inav_parameters_init(&pos_inav_param_handles); /* first parameters read at start up */ struct parameter_update_s param_update; orb_copy(ORB_ID(parameter_update), parameter_update_sub, ¶m_update); /* read from param topic to clear updated flag */ /* first parameters update */ inav_parameters_update(&pos_inav_param_handles, ¶ms); px4_pollfd_struct_t fds_init[1] = { { .fd = sensor_combined_sub, .events = POLLIN }, }; /* wait for initial baro value */ bool wait_baro = true; thread_running = true; while (wait_baro && !thread_should_exit) { int ret = px4_poll(fds_init, 1, 1000); if (ret < 0) { /* poll error */ mavlink_log_info(mavlink_fd, "[inav] poll error on init"); } else if (ret > 0) { if (fds_init[0].revents & POLLIN) { orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); if (wait_baro && sensor.baro_timestamp != baro_timestamp) { baro_timestamp = sensor.baro_timestamp; /* mean calculation over several measurements */ if (baro_init_cnt < baro_init_num) { if (PX4_ISFINITE(sensor.baro_alt_meter)) { baro_offset += sensor.baro_alt_meter; baro_init_cnt++; } } else { wait_baro = false; baro_offset /= (float) baro_init_cnt; warnx("baro offset: %d m", (int)baro_offset); mavlink_log_info(mavlink_fd, "[inav] baro offset: %d m", (int)baro_offset); local_pos.z_valid = true; local_pos.v_z_valid = true; } } } } } /* main loop */ px4_pollfd_struct_t fds[1] = { { .fd = vehicle_attitude_sub, .events = POLLIN }, }; while (!thread_should_exit) { int ret = px4_poll(fds, 1, 20); // wait maximal 20 ms = 50 Hz minimum rate hrt_abstime t = hrt_absolute_time(); if (ret < 0) { /* poll error */ mavlink_log_info(mavlink_fd, "[inav] poll error on init"); continue; } else if (ret > 0) { /* act on attitude updates */ /* vehicle attitude */ orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); attitude_updates++; bool updated; /* parameter update */ orb_check(parameter_update_sub, &updated); if (updated) { struct parameter_update_s update; orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update); inav_parameters_update(&pos_inav_param_handles, ¶ms); } /* actuator */ orb_check(actuator_sub, &updated); if (updated) { orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_sub, &actuator); } /* armed */ orb_check(armed_sub, &updated); if (updated) { orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); } /* sensor combined */ orb_check(sensor_combined_sub, &updated); if (updated) { orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); if (sensor.accelerometer_timestamp != accel_timestamp) { if (att.R_valid) { /* correct accel bias */ sensor.accelerometer_m_s2[0] -= acc_bias[0]; sensor.accelerometer_m_s2[1] -= acc_bias[1]; sensor.accelerometer_m_s2[2] -= acc_bias[2]; /* transform acceleration vector from body frame to NED frame */ for (int i = 0; i < 3; i++) { acc[i] = 0.0f; for (int j = 0; j < 3; j++) { acc[i] += PX4_R(att.R, i, j) * sensor.accelerometer_m_s2[j]; } } acc[2] += CONSTANTS_ONE_G; } else { memset(acc, 0, sizeof(acc)); } accel_timestamp = sensor.accelerometer_timestamp; accel_updates++; } if (sensor.baro_timestamp != baro_timestamp) { corr_baro = baro_offset - sensor.baro_alt_meter - z_est[0]; baro_timestamp = sensor.baro_timestamp; baro_updates++; } } /* optical flow */ orb_check(optical_flow_sub, &updated); if (updated) { orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow); /* calculate time from previous update */ // float flow_dt = flow_prev > 0 ? (flow.flow_timestamp - flow_prev) * 1e-6f : 0.1f; // flow_prev = flow.flow_timestamp; if ((flow.ground_distance_m > 0.31f) && (flow.ground_distance_m < 4.0f) && (PX4_R(att.R, 2, 2) > 0.7f) && (fabsf(flow.ground_distance_m - sonar_prev) > FLT_EPSILON)) { sonar_time = t; sonar_prev = flow.ground_distance_m; corr_sonar = flow.ground_distance_m + surface_offset + z_est[0]; corr_sonar_filtered += (corr_sonar - corr_sonar_filtered) * params.sonar_filt; if (fabsf(corr_sonar) > params.sonar_err) { /* correction is too large: spike or new ground level? */ if (fabsf(corr_sonar - corr_sonar_filtered) > params.sonar_err) { /* spike detected, ignore */ corr_sonar = 0.0f; sonar_valid = false; } else { /* new ground level */ surface_offset -= corr_sonar; surface_offset_rate = 0.0f; corr_sonar = 0.0f; corr_sonar_filtered = 0.0f; sonar_valid_time = t; sonar_valid = true; local_pos.surface_bottom_timestamp = t; mavlink_log_info(mavlink_fd, "[inav] new surface level: %d", (int)surface_offset); } } else { /* correction is ok, use it */ sonar_valid_time = t; sonar_valid = true; } } float flow_q = flow.quality / 255.0f; float dist_bottom = - z_est[0] - surface_offset; if (dist_bottom > 0.3f && flow_q > params.flow_q_min && (t < sonar_valid_time + sonar_valid_timeout) && PX4_R(att.R, 2, 2) > 0.7f) { /* distance to surface */ float flow_dist = dist_bottom / PX4_R(att.R, 2, 2); /* check if flow if too large for accurate measurements */ /* calculate estimated velocity in body frame */ float body_v_est[2] = { 0.0f, 0.0f }; for (int i = 0; i < 2; i++) { body_v_est[i] = PX4_R(att.R, 0, i) * x_est[1] + PX4_R(att.R, 1, i) * y_est[1] + PX4_R(att.R, 2, i) * z_est[1]; } /* set this flag if flow should be accurate according to current velocity and attitude rate estimate */ flow_accurate = fabsf(body_v_est[1] / flow_dist - att.rollspeed) < max_flow && fabsf(body_v_est[0] / flow_dist + att.pitchspeed) < max_flow; /* convert raw flow to angular flow (rad/s) */ float flow_ang[2]; //todo check direction of x und y axis flow_ang[0] = flow.pixel_flow_x_integral/(float)flow.integration_timespan*1000000.0f;//flow.flow_raw_x * params.flow_k / 1000.0f / flow_dt; flow_ang[1] = flow.pixel_flow_y_integral/(float)flow.integration_timespan*1000000.0f;//flow.flow_raw_y * params.flow_k / 1000.0f / flow_dt; /* flow measurements vector */ float flow_m[3]; flow_m[0] = -flow_ang[0] * flow_dist; flow_m[1] = -flow_ang[1] * flow_dist; flow_m[2] = z_est[1]; /* velocity in NED */ float flow_v[2] = { 0.0f, 0.0f }; /* project measurements vector to NED basis, skip Z component */ for (int i = 0; i < 2; i++) { for (int j = 0; j < 3; j++) { flow_v[i] += PX4_R(att.R, i, j) * flow_m[j]; } } /* velocity correction */ corr_flow[0] = flow_v[0] - x_est[1]; corr_flow[1] = flow_v[1] - y_est[1]; /* adjust correction weight */ float flow_q_weight = (flow_q - params.flow_q_min) / (1.0f - params.flow_q_min); w_flow = PX4_R(att.R, 2, 2) * flow_q_weight / fmaxf(1.0f, flow_dist); /* if flow is not accurate, reduce weight for it */ // TODO make this more fuzzy if (!flow_accurate) { w_flow *= 0.05f; } /* under ideal conditions, on 1m distance assume EPH = 10cm */ eph_flow = 0.1f / w_flow; flow_valid = true; } else { w_flow = 0.0f; flow_valid = false; } flow_updates++; } /* home position */ orb_check(home_position_sub, &updated); if (updated) { orb_copy(ORB_ID(home_position), home_position_sub, &home); if (home.timestamp != home_timestamp) { home_timestamp = home.timestamp; double est_lat, est_lon; float est_alt; if (ref_inited) { /* calculate current estimated position in global frame */ est_alt = local_pos.ref_alt - local_pos.z; map_projection_reproject(&ref, local_pos.x, local_pos.y, &est_lat, &est_lon); } /* update reference */ map_projection_init(&ref, home.lat, home.lon); /* update baro offset */ baro_offset += home.alt - local_pos.ref_alt; local_pos.ref_lat = home.lat; local_pos.ref_lon = home.lon; local_pos.ref_alt = home.alt; local_pos.ref_timestamp = home.timestamp; if (ref_inited) { /* reproject position estimate with new reference */ map_projection_project(&ref, est_lat, est_lon, &x_est[0], &y_est[0]); z_est[0] = -(est_alt - local_pos.ref_alt); } ref_inited = true; } } /* check no vision circuit breaker is set */ if (params.no_vision != CBRK_NO_VISION_KEY) { /* vehicle vision position */ orb_check(vision_position_estimate_sub, &updated); if (updated) { orb_copy(ORB_ID(vision_position_estimate), vision_position_estimate_sub, &vision); static float last_vision_x = 0.0f; static float last_vision_y = 0.0f; static float last_vision_z = 0.0f; /* reset position estimate on first vision update */ if (!vision_valid) { x_est[0] = vision.x; x_est[1] = vision.vx; y_est[0] = vision.y; y_est[1] = vision.vy; /* only reset the z estimate if the z weight parameter is not zero */ if (params.w_z_vision_p > MIN_VALID_W) { z_est[0] = vision.z; z_est[1] = vision.vz; } vision_valid = true; last_vision_x = vision.x; last_vision_y = vision.y; last_vision_z = vision.z; warnx("VISION estimate valid"); mavlink_log_info(mavlink_fd, "[inav] VISION estimate valid"); } /* calculate correction for position */ corr_vision[0][0] = vision.x - x_est[0]; corr_vision[1][0] = vision.y - y_est[0]; corr_vision[2][0] = vision.z - z_est[0]; static hrt_abstime last_vision_time = 0; float vision_dt = (vision.timestamp_boot - last_vision_time) / 1e6f; last_vision_time = vision.timestamp_boot; if (vision_dt > 0.000001f && vision_dt < 0.2f) { vision.vx = (vision.x - last_vision_x) / vision_dt; vision.vy = (vision.y - last_vision_y) / vision_dt; vision.vz = (vision.z - last_vision_z) / vision_dt; last_vision_x = vision.x; last_vision_y = vision.y; last_vision_z = vision.z; /* calculate correction for velocity */ corr_vision[0][1] = vision.vx - x_est[1]; corr_vision[1][1] = vision.vy - y_est[1]; corr_vision[2][1] = vision.vz - z_est[1]; } else { /* assume zero motion */ corr_vision[0][1] = 0.0f - x_est[1]; corr_vision[1][1] = 0.0f - y_est[1]; corr_vision[2][1] = 0.0f - z_est[1]; } vision_updates++; } } /* vehicle mocap position */ orb_check(att_pos_mocap_sub, &updated); if (updated) { orb_copy(ORB_ID(att_pos_mocap), att_pos_mocap_sub, &mocap); /* reset position estimate on first mocap update */ if (!mocap_valid) { x_est[0] = mocap.x; y_est[0] = mocap.y; z_est[0] = mocap.z; mocap_valid = true; warnx("MOCAP data valid"); mavlink_log_info(mavlink_fd, "[inav] MOCAP data valid"); } /* calculate correction for position */ corr_mocap[0][0] = mocap.x - x_est[0]; corr_mocap[1][0] = mocap.y - y_est[0]; corr_mocap[2][0] = mocap.z - z_est[0]; mocap_updates++; } /* vehicle GPS position */ orb_check(vehicle_gps_position_sub, &updated); if (updated) { orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); bool reset_est = false; /* hysteresis for GPS quality */ if (gps_valid) { if (gps.eph > max_eph_epv || gps.epv > max_eph_epv || gps.fix_type < 3) { gps_valid = false; mavlink_log_info(mavlink_fd, "[inav] GPS signal lost"); } } else { if (gps.eph < max_eph_epv * 0.7f && gps.epv < max_eph_epv * 0.7f && gps.fix_type >= 3) { gps_valid = true; reset_est = true; mavlink_log_info(mavlink_fd, "[inav] GPS signal found"); } } if (gps_valid) { double lat = gps.lat * 1e-7; double lon = gps.lon * 1e-7; float alt = gps.alt * 1e-3; /* initialize reference position if needed */ if (!ref_inited) { if (ref_init_start == 0) { ref_init_start = t; } else if (t > ref_init_start + ref_init_delay) { ref_inited = true; /* set position estimate to (0, 0, 0), use GPS velocity for XY */ x_est[0] = 0.0f; x_est[1] = gps.vel_n_m_s; y_est[0] = 0.0f; y_est[1] = gps.vel_e_m_s; local_pos.ref_lat = lat; local_pos.ref_lon = lon; local_pos.ref_alt = alt + z_est[0]; local_pos.ref_timestamp = t; /* initialize projection */ map_projection_init(&ref, lat, lon); // XXX replace this print warnx("init ref: lat=%.7f, lon=%.7f, alt=%8.4f", (double)lat, (double)lon, (double)alt); mavlink_log_info(mavlink_fd, "[inav] init ref: %.7f, %.7f, %8.4f", (double)lat, (double)lon, (double)alt); } } if (ref_inited) { /* project GPS lat lon to plane */ float gps_proj[2]; map_projection_project(&ref, lat, lon, &(gps_proj[0]), &(gps_proj[1])); /* reset position estimate when GPS becomes good */ if (reset_est) { x_est[0] = gps_proj[0]; x_est[1] = gps.vel_n_m_s; y_est[0] = gps_proj[1]; y_est[1] = gps.vel_e_m_s; } /* calculate index of estimated values in buffer */ int est_i = buf_ptr - 1 - min(EST_BUF_SIZE - 1, max(0, (int)(params.delay_gps * 1000000.0f / PUB_INTERVAL))); if (est_i < 0) { est_i += EST_BUF_SIZE; } /* calculate correction for position */ corr_gps[0][0] = gps_proj[0] - est_buf[est_i][0][0]; corr_gps[1][0] = gps_proj[1] - est_buf[est_i][1][0]; corr_gps[2][0] = local_pos.ref_alt - alt - est_buf[est_i][2][0]; /* calculate correction for velocity */ if (gps.vel_ned_valid) { corr_gps[0][1] = gps.vel_n_m_s - est_buf[est_i][0][1]; corr_gps[1][1] = gps.vel_e_m_s - est_buf[est_i][1][1]; corr_gps[2][1] = gps.vel_d_m_s - est_buf[est_i][2][1]; } else { corr_gps[0][1] = 0.0f; corr_gps[1][1] = 0.0f; corr_gps[2][1] = 0.0f; } /* save rotation matrix at this moment */ memcpy(R_gps, R_buf[est_i], sizeof(R_gps)); w_gps_xy = min_eph_epv / fmaxf(min_eph_epv, gps.eph); w_gps_z = min_eph_epv / fmaxf(min_eph_epv, gps.epv); } } else { /* no GPS lock */ memset(corr_gps, 0, sizeof(corr_gps)); ref_init_start = 0; } gps_updates++; } } /* check for timeout on FLOW topic */ if ((flow_valid || sonar_valid) && t > flow.timestamp + flow_topic_timeout) { flow_valid = false; sonar_valid = false; warnx("FLOW timeout"); mavlink_log_info(mavlink_fd, "[inav] FLOW timeout"); } /* check for timeout on GPS topic */ if (gps_valid && (t > (gps.timestamp_position + gps_topic_timeout))) { gps_valid = false; warnx("GPS timeout"); mavlink_log_info(mavlink_fd, "[inav] GPS timeout"); } /* check for timeout on vision topic */ if (vision_valid && (t > (vision.timestamp_boot + vision_topic_timeout))) { vision_valid = false; warnx("VISION timeout"); mavlink_log_info(mavlink_fd, "[inav] VISION timeout"); } /* check for timeout on mocap topic */ if (mocap_valid && (t > (mocap.timestamp_boot + mocap_topic_timeout))) { mocap_valid = false; warnx("MOCAP timeout"); mavlink_log_info(mavlink_fd, "[inav] MOCAP timeout"); } /* check for sonar measurement timeout */ if (sonar_valid && (t > (sonar_time + sonar_timeout))) { corr_sonar = 0.0f; sonar_valid = false; } float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f; dt = fmaxf(fminf(0.02, dt), 0.002); // constrain dt from 2 to 20 ms t_prev = t; /* increase EPH/EPV on each step */ if (eph < max_eph_epv) { eph *= 1.0f + dt; } if (epv < max_eph_epv) { epv += 0.005f * dt; // add 1m to EPV each 200s (baro drift) } /* use GPS if it's valid and reference position initialized */ bool use_gps_xy = ref_inited && gps_valid && params.w_xy_gps_p > MIN_VALID_W; bool use_gps_z = ref_inited && gps_valid && params.w_z_gps_p > MIN_VALID_W; /* use VISION if it's valid and has a valid weight parameter */ bool use_vision_xy = vision_valid && params.w_xy_vision_p > MIN_VALID_W; bool use_vision_z = vision_valid && params.w_z_vision_p > MIN_VALID_W; /* use MOCAP if it's valid and has a valid weight parameter */ bool use_mocap = mocap_valid && params.w_mocap_p > MIN_VALID_W; /* use flow if it's valid and (accurate or no GPS available) */ bool use_flow = flow_valid && (flow_accurate || !use_gps_xy); bool can_estimate_xy = (eph < max_eph_epv) || use_gps_xy || use_flow || use_vision_xy || use_mocap; bool dist_bottom_valid = (t < sonar_valid_time + sonar_valid_timeout); if (dist_bottom_valid) { /* surface distance prediction */ surface_offset += surface_offset_rate * dt; /* surface distance correction */ if (sonar_valid) { surface_offset_rate -= corr_sonar * 0.5f * params.w_z_sonar * params.w_z_sonar * dt; surface_offset -= corr_sonar * params.w_z_sonar * dt; } } float w_xy_gps_p = params.w_xy_gps_p * w_gps_xy; float w_xy_gps_v = params.w_xy_gps_v * w_gps_xy; float w_z_gps_p = params.w_z_gps_p * w_gps_z; float w_z_gps_v = params.w_z_gps_v * w_gps_z; float w_xy_vision_p = params.w_xy_vision_p; float w_xy_vision_v = params.w_xy_vision_v; float w_z_vision_p = params.w_z_vision_p; float w_mocap_p = params.w_mocap_p; /* reduce GPS weight if optical flow is good */ if (use_flow && flow_accurate) { w_xy_gps_p *= params.w_gps_flow; w_xy_gps_v *= params.w_gps_flow; } /* baro offset correction */ if (use_gps_z) { float offs_corr = corr_gps[2][0] * w_z_gps_p * dt; baro_offset += offs_corr; corr_baro += offs_corr; } /* accelerometer bias correction for GPS (use buffered rotation matrix) */ float accel_bias_corr[3] = { 0.0f, 0.0f, 0.0f }; if (use_gps_xy) { accel_bias_corr[0] -= corr_gps[0][0] * w_xy_gps_p * w_xy_gps_p; accel_bias_corr[0] -= corr_gps[0][1] * w_xy_gps_v; accel_bias_corr[1] -= corr_gps[1][0] * w_xy_gps_p * w_xy_gps_p; accel_bias_corr[1] -= corr_gps[1][1] * w_xy_gps_v; } if (use_gps_z) { accel_bias_corr[2] -= corr_gps[2][0] * w_z_gps_p * w_z_gps_p; accel_bias_corr[2] -= corr_gps[2][1] * w_z_gps_v; } /* transform error vector from NED frame to body frame */ for (int i = 0; i < 3; i++) { float c = 0.0f; for (int j = 0; j < 3; j++) { c += R_gps[j][i] * accel_bias_corr[j]; } if (isfinite(c)) { acc_bias[i] += c * params.w_acc_bias * dt; } } /* accelerometer bias correction for VISION (use buffered rotation matrix) */ accel_bias_corr[0] = 0.0f; accel_bias_corr[1] = 0.0f; accel_bias_corr[2] = 0.0f; if (use_vision_xy) { accel_bias_corr[0] -= corr_vision[0][0] * w_xy_vision_p * w_xy_vision_p; accel_bias_corr[0] -= corr_vision[0][1] * w_xy_vision_v; accel_bias_corr[1] -= corr_vision[1][0] * w_xy_vision_p * w_xy_vision_p; accel_bias_corr[1] -= corr_vision[1][1] * w_xy_vision_v; } if (use_vision_z) { accel_bias_corr[2] -= corr_vision[2][0] * w_z_vision_p * w_z_vision_p; } /* accelerometer bias correction for MOCAP (use buffered rotation matrix) */ accel_bias_corr[0] = 0.0f; accel_bias_corr[1] = 0.0f; accel_bias_corr[2] = 0.0f; if (use_mocap) { accel_bias_corr[0] -= corr_mocap[0][0] * w_mocap_p * w_mocap_p; accel_bias_corr[1] -= corr_mocap[1][0] * w_mocap_p * w_mocap_p; accel_bias_corr[2] -= corr_mocap[2][0] * w_mocap_p * w_mocap_p; } /* transform error vector from NED frame to body frame */ for (int i = 0; i < 3; i++) { float c = 0.0f; for (int j = 0; j < 3; j++) { c += PX4_R(att.R, j, i) * accel_bias_corr[j]; } if (isfinite(c)) { acc_bias[i] += c * params.w_acc_bias * dt; } } /* accelerometer bias correction for flow and baro (assume that there is no delay) */ accel_bias_corr[0] = 0.0f; accel_bias_corr[1] = 0.0f; accel_bias_corr[2] = 0.0f; if (use_flow) { accel_bias_corr[0] -= corr_flow[0] * params.w_xy_flow; accel_bias_corr[1] -= corr_flow[1] * params.w_xy_flow; } accel_bias_corr[2] -= corr_baro * params.w_z_baro * params.w_z_baro; /* transform error vector from NED frame to body frame */ for (int i = 0; i < 3; i++) { float c = 0.0f; for (int j = 0; j < 3; j++) { c += PX4_R(att.R, j, i) * accel_bias_corr[j]; } if (isfinite(c)) { acc_bias[i] += c * params.w_acc_bias * dt; } } /* inertial filter prediction for altitude */ inertial_filter_predict(dt, z_est, acc[2]); if (!(isfinite(z_est[0]) && isfinite(z_est[1]))) { write_debug_log("BAD ESTIMATE AFTER Z PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p, corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v); memcpy(z_est, z_est_prev, sizeof(z_est)); } /* inertial filter correction for altitude */ inertial_filter_correct(corr_baro, dt, z_est, 0, params.w_z_baro); if (use_gps_z) { epv = fminf(epv, gps.epv); inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p); inertial_filter_correct(corr_gps[2][1], dt, z_est, 1, w_z_gps_v); } if (use_vision_z) { epv = fminf(epv, epv_vision); inertial_filter_correct(corr_vision[2][0], dt, z_est, 0, w_z_vision_p); } if (use_mocap) { epv = fminf(epv, epv_mocap); inertial_filter_correct(corr_mocap[2][0], dt, z_est, 0, w_mocap_p); } if (!(isfinite(z_est[0]) && isfinite(z_est[1]))) { write_debug_log("BAD ESTIMATE AFTER Z CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p, corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v); memcpy(z_est, z_est_prev, sizeof(z_est)); memset(corr_gps, 0, sizeof(corr_gps)); memset(corr_vision, 0, sizeof(corr_vision)); memset(corr_mocap, 0, sizeof(corr_mocap)); corr_baro = 0; } else { memcpy(z_est_prev, z_est, sizeof(z_est)); } if (can_estimate_xy) { /* inertial filter prediction for position */ inertial_filter_predict(dt, x_est, acc[0]); inertial_filter_predict(dt, y_est, acc[1]); if (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(y_est[0]) && isfinite(y_est[1]))) { write_debug_log("BAD ESTIMATE AFTER PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p, corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v); memcpy(x_est, x_est_prev, sizeof(x_est)); memcpy(y_est, y_est_prev, sizeof(y_est)); } /* inertial filter correction for position */ if (use_flow) { eph = fminf(eph, eph_flow); inertial_filter_correct(corr_flow[0], dt, x_est, 1, params.w_xy_flow * w_flow); inertial_filter_correct(corr_flow[1], dt, y_est, 1, params.w_xy_flow * w_flow); } if (use_gps_xy) { eph = fminf(eph, gps.eph); inertial_filter_correct(corr_gps[0][0], dt, x_est, 0, w_xy_gps_p); inertial_filter_correct(corr_gps[1][0], dt, y_est, 0, w_xy_gps_p); if (gps.vel_ned_valid && t < gps.timestamp_velocity + gps_topic_timeout) { inertial_filter_correct(corr_gps[0][1], dt, x_est, 1, w_xy_gps_v); inertial_filter_correct(corr_gps[1][1], dt, y_est, 1, w_xy_gps_v); } } if (use_vision_xy) { eph = fminf(eph, eph_vision); inertial_filter_correct(corr_vision[0][0], dt, x_est, 0, w_xy_vision_p); inertial_filter_correct(corr_vision[1][0], dt, y_est, 0, w_xy_vision_p); if (w_xy_vision_v > MIN_VALID_W) { inertial_filter_correct(corr_vision[0][1], dt, x_est, 1, w_xy_vision_v); inertial_filter_correct(corr_vision[1][1], dt, y_est, 1, w_xy_vision_v); } } if (use_mocap) { eph = fminf(eph, eph_mocap); inertial_filter_correct(corr_mocap[0][0], dt, x_est, 0, w_mocap_p); inertial_filter_correct(corr_mocap[1][0], dt, y_est, 0, w_mocap_p); } if (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(y_est[0]) && isfinite(y_est[1]))) { write_debug_log("BAD ESTIMATE AFTER CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p, corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v); memcpy(x_est, x_est_prev, sizeof(x_est)); memcpy(y_est, y_est_prev, sizeof(y_est)); memset(corr_gps, 0, sizeof(corr_gps)); memset(corr_vision, 0, sizeof(corr_vision)); memset(corr_mocap, 0, sizeof(corr_mocap)); memset(corr_flow, 0, sizeof(corr_flow)); } else { memcpy(x_est_prev, x_est, sizeof(x_est)); memcpy(y_est_prev, y_est, sizeof(y_est)); } } else { /* gradually reset xy velocity estimates */ inertial_filter_correct(-x_est[1], dt, x_est, 1, params.w_xy_res_v); inertial_filter_correct(-y_est[1], dt, y_est, 1, params.w_xy_res_v); } if (inav_verbose_mode) { /* print updates rate */ if (t > updates_counter_start + updates_counter_len) { float updates_dt = (t - updates_counter_start) * 0.000001f; warnx( "updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s, flow = %.1f/s, vision = %.1f/s, mocap = %.1f/s", (double)(accel_updates / updates_dt), (double)(baro_updates / updates_dt), (double)(gps_updates / updates_dt), (double)(attitude_updates / updates_dt), (double)(flow_updates / updates_dt), (double)(vision_updates / updates_dt), (double)(mocap_updates / updates_dt)); updates_counter_start = t; accel_updates = 0; baro_updates = 0; gps_updates = 0; attitude_updates = 0; flow_updates = 0; vision_updates = 0; mocap_updates = 0; } } if (t > pub_last + PUB_INTERVAL) { pub_last = t; /* push current estimate to buffer */ est_buf[buf_ptr][0][0] = x_est[0]; est_buf[buf_ptr][0][1] = x_est[1]; est_buf[buf_ptr][1][0] = y_est[0]; est_buf[buf_ptr][1][1] = y_est[1]; est_buf[buf_ptr][2][0] = z_est[0]; est_buf[buf_ptr][2][1] = z_est[1]; /* push current rotation matrix to buffer */ memcpy(R_buf[buf_ptr], att.R, sizeof(att.R)); buf_ptr++; if (buf_ptr >= EST_BUF_SIZE) { buf_ptr = 0; } /* publish local position */ local_pos.xy_valid = can_estimate_xy; local_pos.v_xy_valid = can_estimate_xy; local_pos.xy_global = local_pos.xy_valid && use_gps_xy; local_pos.z_global = local_pos.z_valid && use_gps_z; local_pos.x = x_est[0]; local_pos.vx = x_est[1]; local_pos.y = y_est[0]; local_pos.vy = y_est[1]; local_pos.z = z_est[0]; local_pos.vz = z_est[1]; local_pos.yaw = att.yaw; local_pos.dist_bottom_valid = dist_bottom_valid; local_pos.eph = eph; local_pos.epv = epv; if (local_pos.dist_bottom_valid) { local_pos.dist_bottom = -z_est[0] - surface_offset; local_pos.dist_bottom_rate = -z_est[1] - surface_offset_rate; } local_pos.timestamp = t; orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos); if (local_pos.xy_global && local_pos.z_global) { /* publish global position */ global_pos.timestamp = t; global_pos.time_utc_usec = gps.time_utc_usec; double est_lat, est_lon; map_projection_reproject(&ref, local_pos.x, local_pos.y, &est_lat, &est_lon); global_pos.lat = est_lat; global_pos.lon = est_lon; global_pos.alt = local_pos.ref_alt - local_pos.z; global_pos.vel_n = local_pos.vx; global_pos.vel_e = local_pos.vy; global_pos.vel_d = local_pos.vz; global_pos.yaw = local_pos.yaw; global_pos.eph = eph; global_pos.epv = epv; if (vehicle_global_position_pub == NULL) { vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos); } else { orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos); } } } } warnx("stopped"); mavlink_log_info(mavlink_fd, "[inav] stopped"); thread_running = false; return 0; }
void Ekf2::task_main() { // subscribe to relevant topics _sensors_sub = orb_subscribe(ORB_ID(sensor_combined)); _gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); px4_pollfd_struct_t fds[2] = {}; fds[0].fd = _sensors_sub; fds[0].events = POLLIN; fds[1].fd = _params_sub; fds[1].events = POLLIN; // initialise parameter cache updateParams(); vehicle_gps_position_s gps = {}; while (!_task_should_exit) { int ret = px4_poll(fds, sizeof(fds) / sizeof(fds[0]), 1000); if (ret < 0) { // Poll error, sleep and try again usleep(10000); continue; } else if (ret == 0) { // Poll timeout or no new data, do nothing continue; } if (fds[1].revents & POLLIN) { // read from param to clear updated flag struct parameter_update_s update; orb_copy(ORB_ID(parameter_update), _params_sub, &update); updateParams(); // fetch sensor data in next loop continue; } else if (!(fds[0].revents & POLLIN)) { // no new data continue; } bool gps_updated = false; bool airspeed_updated = false; bool control_mode_updated = false; bool vehicle_status_updated = false; sensor_combined_s sensors = {}; airspeed_s airspeed = {}; vehicle_control_mode_s vehicle_control_mode = {}; orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors); // update all other topics if they have new data orb_check(_gps_sub, &gps_updated); if (gps_updated) { orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &gps); } orb_check(_airspeed_sub, &airspeed_updated); if (airspeed_updated) { orb_copy(ORB_ID(airspeed), _airspeed_sub, &airspeed); } // Use the control model data to determine if the motors are armed as a surrogate for an on-ground vs in-air status // TODO implement a global vehicle on-ground/in-air check orb_check(_control_mode_sub, &control_mode_updated); if (control_mode_updated) { orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &vehicle_control_mode); _ekf->set_arm_status(vehicle_control_mode.flag_armed); } hrt_abstime now = hrt_absolute_time(); // push imu data into estimator _ekf->setIMUData(now, sensors.gyro_integral_dt[0], sensors.accelerometer_integral_dt[0], &sensors.gyro_integral_rad[0], &sensors.accelerometer_integral_m_s[0]); // read mag data _ekf->setMagData(sensors.magnetometer_timestamp[0], &sensors.magnetometer_ga[0]); // read baro data _ekf->setBaroData(sensors.baro_timestamp[0], &sensors.baro_alt_meter[0]); // read gps data if available if (gps_updated) { struct gps_message gps_msg = {}; gps_msg.time_usec = gps.timestamp_position; gps_msg.lat = gps.lat; gps_msg.lon = gps.lon; gps_msg.alt = gps.alt; gps_msg.fix_type = gps.fix_type; gps_msg.eph = gps.eph; gps_msg.epv = gps.epv; gps_msg.sacc = gps.s_variance_m_s; gps_msg.time_usec_vel = gps.timestamp_velocity; gps_msg.vel_m_s = gps.vel_m_s; gps_msg.vel_ned[0] = gps.vel_n_m_s; gps_msg.vel_ned[1] = gps.vel_e_m_s; gps_msg.vel_ned[2] = gps.vel_d_m_s; gps_msg.vel_ned_valid = gps.vel_ned_valid; gps_msg.nsats = gps.satellites_used; //TODO add gdop to gps topic gps_msg.gdop = 0.0f; _ekf->setGpsData(gps.timestamp_position, &gps_msg); } // read airspeed data if available if (airspeed_updated) { _ekf->setAirspeedData(airspeed.timestamp, &airspeed.indicated_airspeed_m_s); } // read vehicle status if available for 'landed' information orb_check(_vehicle_status_sub, &vehicle_status_updated); if (vehicle_status_updated) { struct vehicle_status_s status = {}; orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &status); _ekf->set_in_air_status(!status.condition_landed); } // run the EKF update _ekf->update(); // generate vehicle attitude data struct vehicle_attitude_s att = {}; att.timestamp = hrt_absolute_time(); _ekf->copy_quaternion(att.q); matrix::Quaternion<float> q(att.q[0], att.q[1], att.q[2], att.q[3]); matrix::Euler<float> euler(q); att.roll = euler(0); att.pitch = euler(1); att.yaw = euler(2); // generate vehicle local position data struct vehicle_local_position_s lpos = {}; float pos[3] = {}; float vel[3] = {}; lpos.timestamp = hrt_absolute_time(); // Position in local NED frame _ekf->copy_position(pos); lpos.x = pos[0]; lpos.y = pos[1]; lpos.z = pos[2]; // Velocity in NED frame (m/s) _ekf->copy_velocity(vel); lpos.vx = vel[0]; lpos.vy = vel[1]; lpos.vz = vel[2]; // TODO: better status reporting lpos.xy_valid = _ekf->position_is_valid(); lpos.z_valid = true; lpos.v_xy_valid = _ekf->position_is_valid(); lpos.v_z_valid = true; // Position of local NED origin in GPS / WGS84 frame struct map_projection_reference_s ekf_origin = {}; _ekf->get_ekf_origin(&lpos.ref_timestamp, &ekf_origin, &lpos.ref_alt); lpos.xy_global = _ekf->position_is_valid(); // true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon) lpos.z_global = true; // true if z is valid and has valid global reference (ref_alt) lpos.ref_lat = ekf_origin.lat_rad * 180.0 / M_PI; // Reference point latitude in degrees lpos.ref_lon = ekf_origin.lon_rad * 180.0 / M_PI; // Reference point longitude in degrees // The rotation of the tangent plane vs. geographical north lpos.yaw = 0.0f; lpos.dist_bottom = 0.0f; // Distance to bottom surface (ground) in meters lpos.dist_bottom_rate = 0.0f; // Distance to bottom surface (ground) change rate lpos.surface_bottom_timestamp = 0; // Time when new bottom surface found lpos.dist_bottom_valid = false; // true if distance to bottom surface is valid // TODO: uORB definition does not define what thes variables are. We have assumed them to be horizontal and vertical 1-std dev accuracy in metres // TODO: Should use sqrt of filter position variances lpos.eph = gps.eph; lpos.epv = gps.epv; // publish vehicle local position data if (_lpos_pub == nullptr) { _lpos_pub = orb_advertise(ORB_ID(vehicle_local_position), &lpos); } else { orb_publish(ORB_ID(vehicle_local_position), _lpos_pub, &lpos); } // generate control state data control_state_s ctrl_state = {}; ctrl_state.timestamp = hrt_absolute_time(); ctrl_state.roll_rate = _lp_roll_rate.apply(sensors.gyro_rad_s[0]); ctrl_state.pitch_rate = _lp_pitch_rate.apply(sensors.gyro_rad_s[1]); ctrl_state.yaw_rate = _lp_yaw_rate.apply(sensors.gyro_rad_s[2]); ctrl_state.q[0] = q(0); ctrl_state.q[1] = q(1); ctrl_state.q[2] = q(2); ctrl_state.q[3] = q(3); // publish control state data if (_control_state_pub == nullptr) { _control_state_pub = orb_advertise(ORB_ID(control_state), &ctrl_state); } else { orb_publish(ORB_ID(control_state), _control_state_pub, &ctrl_state); } // generate vehicle attitude data att.q[0] = q(0); att.q[1] = q(1); att.q[2] = q(2); att.q[3] = q(3); att.q_valid = true; att.rollspeed = sensors.gyro_rad_s[0]; att.pitchspeed = sensors.gyro_rad_s[1]; att.yawspeed = sensors.gyro_rad_s[2]; // publish vehicle attitude data if (_att_pub == nullptr) { _att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att); } else { orb_publish(ORB_ID(vehicle_attitude), _att_pub, &att); } // generate and publish global position data struct vehicle_global_position_s global_pos = {}; if (_ekf->position_is_valid()) { // TODO: local origin is currenlty at GPS height origin - this is different to ekf_att_pos_estimator global_pos.timestamp = hrt_absolute_time(); // Time of this estimate, in microseconds since system start global_pos.time_utc_usec = gps.time_utc_usec; // GPS UTC timestamp in microseconds double est_lat, est_lon; map_projection_reproject(&ekf_origin, lpos.x, lpos.y, &est_lat, &est_lon); global_pos.lat = est_lat; // Latitude in degrees global_pos.lon = est_lon; // Longitude in degrees global_pos.alt = -pos[2] + lpos.ref_alt; // Altitude AMSL in meters global_pos.vel_n = vel[0]; // Ground north velocity, m/s global_pos.vel_e = vel[1]; // Ground east velocity, m/s global_pos.vel_d = vel[2]; // Ground downside velocity, m/s global_pos.yaw = euler(2); // Yaw in radians -PI..+PI. global_pos.eph = gps.eph; // Standard deviation of position estimate horizontally global_pos.epv = gps.epv; // Standard deviation of position vertically // TODO: implement terrain estimator global_pos.terrain_alt = 0.0f; // Terrain altitude in m, WGS84 global_pos.terrain_alt_valid = false; // Terrain altitude estimate is valid // TODO use innovatun consistency check timouts to set this global_pos.dead_reckoning = false; // True if this position is estimated through dead-reckoning global_pos.pressure_alt = sensors.baro_alt_meter[0]; // Pressure altitude AMSL (m) if (_vehicle_global_position_pub == nullptr) { _vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos); } else { orb_publish(ORB_ID(vehicle_global_position), _vehicle_global_position_pub, &global_pos); } } // publish estimator status struct estimator_status_s status = {}; status.timestamp = hrt_absolute_time(); _ekf->get_state_delayed(status.states); _ekf->get_covariances(status.covariances); //status.gps_check_fail_flags = _ekf->_gps_check_fail_status.value; if (_estimator_status_pub == nullptr) { _estimator_status_pub = orb_advertise(ORB_ID(estimator_status), &status); } else { orb_publish(ORB_ID(estimator_status), _estimator_status_pub, &status); } // publish estimator innovation data struct ekf2_innovations_s innovations = {}; innovations.timestamp = hrt_absolute_time(); _ekf->get_vel_pos_innov(&innovations.vel_pos_innov[0]); _ekf->get_mag_innov(&innovations.mag_innov[0]); _ekf->get_heading_innov(&innovations.heading_innov); _ekf->get_vel_pos_innov_var(&innovations.vel_pos_innov_var[0]); _ekf->get_mag_innov_var(&innovations.mag_innov_var[0]); _ekf->get_heading_innov_var(&innovations.heading_innov_var); if (_estimator_innovations_pub == nullptr) { _estimator_innovations_pub = orb_advertise(ORB_ID(ekf2_innovations), &innovations); } else { orb_publish(ORB_ID(ekf2_innovations), _estimator_innovations_pub, &innovations); } // save the declination to the EKF2_MAG_DECL parameter when a dis-arm event is detected if ((_params->mag_declination_source & (1 << 1)) && _prev_motors_armed && !vehicle_control_mode.flag_armed) { float decl_deg; _ekf->copy_mag_decl_deg(&decl_deg); _mag_declination_deg->set(decl_deg); } _prev_motors_armed = vehicle_control_mode.flag_armed; } delete ekf2::instance; ekf2::instance = nullptr; }
void FixedwingEstimator::task_main() { _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); _ekf = new AttPosEKF(); float dt = 0.0f; // time lapsed since last covariance prediction _filter_start_time = hrt_absolute_time(); if (!_ekf) { errx(1, "OUT OF MEM!"); } /* * do subscriptions */ _baro_sub = orb_subscribe(ORB_ID(sensor_baro)); _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _home_sub = orb_subscribe(ORB_ID(home_position)); /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_vstatus_sub, 200); #ifndef SENSOR_COMBINED_SUB _gyro_sub = orb_subscribe(ORB_ID(sensor_gyro)); _accel_sub = orb_subscribe(ORB_ID(sensor_accel)); _mag_sub = orb_subscribe(ORB_ID(sensor_mag)); /* rate limit gyro updates to 50 Hz */ /* XXX remove this!, BUT increase the data buffer size! */ orb_set_interval(_gyro_sub, 4); #else _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); /* XXX remove this!, BUT increase the data buffer size! */ orb_set_interval(_sensor_combined_sub, 9); #endif /* sets also parameters in the EKF object */ parameters_update(); Vector3f lastAngRate; Vector3f lastAccel; /* wakeup source(s) */ struct pollfd fds[2]; /* Setup of loop */ fds[0].fd = _params_sub; fds[0].events = POLLIN; #ifndef SENSOR_COMBINED_SUB fds[1].fd = _gyro_sub; fds[1].events = POLLIN; #else fds[1].fd = _sensor_combined_sub; fds[1].events = POLLIN; #endif bool newDataGps = false; bool newHgtData = false; bool newAdsData = false; bool newDataMag = false; float posNED[3] = {0.0f, 0.0f, 0.0f}; // North, East Down position (m) uint64_t last_gps = 0; _gps.vel_n_m_s = 0.0f; _gps.vel_e_m_s = 0.0f; _gps.vel_d_m_s = 0.0f; while (!_task_should_exit) { /* wait for up to 500ms for data */ int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); /* timed out - periodic check for _task_should_exit, etc. */ if (pret == 0) continue; /* this is undesirable but not much we can do - might want to flag unhappy status */ if (pret < 0) { warn("POLL ERR %d, %d", pret, errno); continue; } perf_begin(_loop_perf); /* only update parameters if they changed */ if (fds[0].revents & POLLIN) { /* read from param to clear updated flag */ struct parameter_update_s update; orb_copy(ORB_ID(parameter_update), _params_sub, &update); /* update parameters from storage */ parameters_update(); } /* only run estimator if gyro updated */ if (fds[1].revents & POLLIN) { /* check vehicle status for changes to publication state */ bool prev_hil = (_vstatus.hil_state == HIL_STATE_ON); vehicle_status_poll(); bool accel_updated; bool mag_updated; perf_count(_perf_gyro); /* Reset baro reference if switching to HIL, reset sensor states */ if (!prev_hil && (_vstatus.hil_state == HIL_STATE_ON)) { /* system is in HIL now, wait for measurements to come in one last round */ usleep(60000); #ifndef SENSOR_COMBINED_SUB orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &_gyro); orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel); orb_copy(ORB_ID(sensor_mag), _mag_sub, &_mag); #else /* now read all sensor publications to ensure all real sensor data is purged */ orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined); #endif /* set sensors to de-initialized state */ _gyro_valid = false; _accel_valid = false; _mag_valid = false; _baro_init = false; _gps_initialized = false; _last_sensor_timestamp = hrt_absolute_time(); _last_run = _last_sensor_timestamp; _ekf->ZeroVariables(); _ekf->dtIMU = 0.01f; _filter_start_time = _last_sensor_timestamp; /* now skip this loop and get data on the next one, which will also re-init the filter */ continue; } /** * PART ONE: COLLECT ALL DATA **/ /* load local copies */ #ifndef SENSOR_COMBINED_SUB orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &_gyro); orb_check(_accel_sub, &accel_updated); if (accel_updated) { orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel); } _last_sensor_timestamp = _gyro.timestamp; IMUmsec = _gyro.timestamp / 1e3f; float deltaT = (_gyro.timestamp - _last_run) / 1e6f; _last_run = _gyro.timestamp; /* guard against too large deltaT's */ if (!isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) { deltaT = 0.01f; } // Always store data, independent of init status /* fill in last data set */ _ekf->dtIMU = deltaT; if (isfinite(_gyro.x) && isfinite(_gyro.y) && isfinite(_gyro.z)) { _ekf->angRate.x = _gyro.x; _ekf->angRate.y = _gyro.y; _ekf->angRate.z = _gyro.z; if (!_gyro_valid) { lastAngRate = _ekf->angRate; } _gyro_valid = true; } if (accel_updated) { _ekf->accel.x = _accel.x; _ekf->accel.y = _accel.y; _ekf->accel.z = _accel.z; if (!_accel_valid) { lastAccel = _ekf->accel; } _accel_valid = true; } _ekf->dAngIMU = 0.5f * (angRate + lastAngRate) * dtIMU; _ekf->lastAngRate = angRate; _ekf->dVelIMU = 0.5f * (accel + lastAccel) * dtIMU; _ekf->lastAccel = accel; #else orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined); static hrt_abstime last_accel = 0; static hrt_abstime last_mag = 0; if (last_accel != _sensor_combined.accelerometer_timestamp) { accel_updated = true; } else { accel_updated = false; } last_accel = _sensor_combined.accelerometer_timestamp; // Copy gyro and accel _last_sensor_timestamp = _sensor_combined.timestamp; IMUmsec = _sensor_combined.timestamp / 1e3f; float deltaT = (_sensor_combined.timestamp - _last_run) / 1e6f; /* guard against too large deltaT's */ if (!isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) { deltaT = 0.01f; } _last_run = _sensor_combined.timestamp; // Always store data, independent of init status /* fill in last data set */ _ekf->dtIMU = deltaT; if (isfinite(_sensor_combined.gyro_rad_s[0]) && isfinite(_sensor_combined.gyro_rad_s[1]) && isfinite(_sensor_combined.gyro_rad_s[2])) { _ekf->angRate.x = _sensor_combined.gyro_rad_s[0]; _ekf->angRate.y = _sensor_combined.gyro_rad_s[1]; _ekf->angRate.z = _sensor_combined.gyro_rad_s[2]; if (!_gyro_valid) { lastAngRate = _ekf->angRate; } _gyro_valid = true; perf_count(_perf_gyro); } if (accel_updated) { _ekf->accel.x = _sensor_combined.accelerometer_m_s2[0]; _ekf->accel.y = _sensor_combined.accelerometer_m_s2[1]; _ekf->accel.z = _sensor_combined.accelerometer_m_s2[2]; if (!_accel_valid) { lastAccel = _ekf->accel; } _accel_valid = true; } _ekf->dAngIMU = 0.5f * (_ekf->angRate + lastAngRate) * _ekf->dtIMU; lastAngRate = _ekf->angRate; _ekf->dVelIMU = 0.5f * (_ekf->accel + lastAccel) * _ekf->dtIMU; lastAccel = _ekf->accel; if (last_mag != _sensor_combined.magnetometer_timestamp) { mag_updated = true; newDataMag = true; } else { newDataMag = false; } last_mag = _sensor_combined.magnetometer_timestamp; #endif //warnx("dang: %8.4f %8.4f dvel: %8.4f %8.4f", _ekf->dAngIMU.x, _ekf->dAngIMU.z, _ekf->dVelIMU.x, _ekf->dVelIMU.z); bool airspeed_updated; orb_check(_airspeed_sub, &airspeed_updated); if (airspeed_updated) { orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); perf_count(_perf_airspeed); _ekf->VtasMeas = _airspeed.true_airspeed_m_s; newAdsData = true; } else { newAdsData = false; } bool gps_updated; orb_check(_gps_sub, &gps_updated); if (gps_updated) { last_gps = _gps.timestamp_position; orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps); perf_count(_perf_gps); if (_gps.fix_type < 3) { newDataGps = false; } else { /* store time of valid GPS measurement */ _gps_start_time = hrt_absolute_time(); /* check if we had a GPS outage for a long time */ if (hrt_elapsed_time(&last_gps) > 5 * 1000 * 1000) { _ekf->ResetPosition(); _ekf->ResetVelocity(); _ekf->ResetStoredStates(); } /* fuse GPS updates */ //_gps.timestamp / 1e3; _ekf->GPSstatus = _gps.fix_type; _ekf->velNED[0] = _gps.vel_n_m_s; _ekf->velNED[1] = _gps.vel_e_m_s; _ekf->velNED[2] = _gps.vel_d_m_s; // warnx("GPS updated: status: %d, vel: %8.4f %8.4f %8.4f", (int)GPSstatus, velNED[0], velNED[1], velNED[2]); _ekf->gpsLat = math::radians(_gps.lat / (double)1e7); _ekf->gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI; _ekf->gpsHgt = _gps.alt / 1e3f; // if (_gps.s_variance_m_s > 0.25f && _gps.s_variance_m_s < 100.0f * 100.0f) { // _ekf->vneSigma = sqrtf(_gps.s_variance_m_s); // } else { // _ekf->vneSigma = _parameters.velne_noise; // } // if (_gps.p_variance_m > 0.25f && _gps.p_variance_m < 100.0f * 100.0f) { // _ekf->posNeSigma = sqrtf(_gps.p_variance_m); // } else { // _ekf->posNeSigma = _parameters.posne_noise; // } // warnx("vel: %8.4f pos: %8.4f", _gps.s_variance_m_s, _gps.p_variance_m); newDataGps = true; } } bool baro_updated; orb_check(_baro_sub, &baro_updated); if (baro_updated) { orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); _ekf->baroHgt = _baro.altitude; if (!_baro_init) { _baro_ref = _baro.altitude; _baro_init = true; warnx("ALT REF INIT"); } perf_count(_perf_baro); newHgtData = true; } else { newHgtData = false; } #ifndef SENSOR_COMBINED_SUB orb_check(_mag_sub, &mag_updated); #endif if (mag_updated) { _mag_valid = true; perf_count(_perf_mag); #ifndef SENSOR_COMBINED_SUB orb_copy(ORB_ID(sensor_mag), _mag_sub, &_mag); // XXX we compensate the offsets upfront - should be close to zero. // 0.001f _ekf->magData.x = _mag.x; _ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset _ekf->magData.y = _mag.y; _ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset _ekf->magData.z = _mag.z; _ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset #else // XXX we compensate the offsets upfront - should be close to zero. // 0.001f _ekf->magData.x = _sensor_combined.magnetometer_ga[0]; _ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset _ekf->magData.y = _sensor_combined.magnetometer_ga[1]; _ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset _ekf->magData.z = _sensor_combined.magnetometer_ga[2]; _ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset #endif newDataMag = true; } else { newDataMag = false; } /* * CHECK IF ITS THE RIGHT TIME TO RUN THINGS ALREADY */ if (hrt_elapsed_time(&_filter_start_time) < FILTER_INIT_DELAY) { continue; } /** * PART TWO: EXECUTE THE FILTER * * We run the filter only once all data has been fetched **/ if (_baro_init && _gyro_valid && _accel_valid && _mag_valid) { float initVelNED[3]; /* Initialize the filter first */ if (!_gps_initialized && _gps.fix_type > 2 && _gps.eph < _parameters.pos_stddev_threshold && _gps.epv < _parameters.pos_stddev_threshold) { // GPS is in scaled integers, convert double lat = _gps.lat / 1.0e7; double lon = _gps.lon / 1.0e7; float gps_alt = _gps.alt / 1e3f; initVelNED[0] = _gps.vel_n_m_s; initVelNED[1] = _gps.vel_e_m_s; initVelNED[2] = _gps.vel_d_m_s; // Set up height correctly orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); _baro_ref_offset = _ekf->states[9]; // this should become zero in the local frame _baro_gps_offset = _baro.altitude - gps_alt; _ekf->baroHgt = _baro.altitude; _ekf->hgtMea = 1.0f * (_ekf->baroHgt - (_baro_ref)); // Set up position variables correctly _ekf->GPSstatus = _gps.fix_type; _ekf->gpsLat = math::radians(lat); _ekf->gpsLon = math::radians(lon) - M_PI; _ekf->gpsHgt = gps_alt; // Look up mag declination based on current position float declination = math::radians(get_mag_declination(lat, lon)); _ekf->InitialiseFilter(initVelNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt, declination); // Initialize projection _local_pos.ref_lat = lat; _local_pos.ref_lon = lon; _local_pos.ref_alt = gps_alt; _local_pos.ref_timestamp = _gps.timestamp_position; map_projection_init(&_pos_ref, lat, lon); mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt); #if 0 warnx("HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)gps_alt, (double)_ekf->velNED[0], (double)_ekf->velNED[1], (double)_ekf->velNED[2]); warnx("BARO: %8.4f m / ref: %8.4f m / gps offs: %8.4f m", (double)_ekf->baroHgt, (double)_baro_ref, (double)_baro_ref_offset); warnx("GPS: eph: %8.4f, epv: %8.4f, declination: %8.4f", (double)_gps.eph, (double)_gps.epv, (double)math::degrees(declination)); #endif _gps_initialized = true; } else if (!_ekf->statesInitialised) { initVelNED[0] = 0.0f; initVelNED[1] = 0.0f; initVelNED[2] = 0.0f; _ekf->posNE[0] = posNED[0]; _ekf->posNE[1] = posNED[1]; _local_pos.ref_alt = _baro_ref; _baro_ref_offset = 0.0f; _baro_gps_offset = 0.0f; _ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f); } else if (_ekf->statesInitialised) { // We're apparently initialized in this case now int check = check_filter_state(); if (check) { // Let the system re-initialize itself continue; } // Run the strapdown INS equations every IMU update _ekf->UpdateStrapdownEquationsNED(); #if 0 // debug code - could be tunred into a filter mnitoring/watchdog function float tempQuat[4]; for (uint8_t j = 0; j <= 3; j++) tempQuat[j] = states[j]; quat2eul(eulerEst, tempQuat); for (uint8_t j = 0; j <= 2; j++) eulerDif[j] = eulerEst[j] - ahrsEul[j]; if (eulerDif[2] > pi) eulerDif[2] -= 2 * pi; if (eulerDif[2] < -pi) eulerDif[2] += 2 * pi; #endif // store the predicted states for subsequent use by measurement fusion _ekf->StoreStates(IMUmsec); // Check if on ground - status is used by covariance prediction _ekf->OnGroundCheck(); // sum delta angles and time used by covariance prediction _ekf->summedDelAng = _ekf->summedDelAng + _ekf->correctedDelAng; _ekf->summedDelVel = _ekf->summedDelVel + _ekf->dVelIMU; dt += _ekf->dtIMU; // perform a covariance prediction if the total delta angle has exceeded the limit // or the time limit will be exceeded at the next IMU update if ((dt >= (_ekf->covTimeStepMax - _ekf->dtIMU)) || (_ekf->summedDelAng.length() > _ekf->covDelAngMax)) { _ekf->CovariancePrediction(dt); _ekf->summedDelAng.zero(); _ekf->summedDelVel.zero(); dt = 0.0f; } // Fuse GPS Measurements if (newDataGps && _gps_initialized) { // Convert GPS measurements to Pos NE, hgt and Vel NED float gps_dt = (_gps.timestamp_position - last_gps) / 1e6f; // Calculate acceleration predicted by GPS velocity change if (((fabsf(_ekf->velNED[0] - _gps.vel_n_m_s) > FLT_EPSILON) || (fabsf(_ekf->velNED[1] - _gps.vel_e_m_s) > FLT_EPSILON) || (fabsf(_ekf->velNED[2] - _gps.vel_d_m_s) > FLT_EPSILON)) && (gps_dt > 0.00001f)) { _ekf->accelGPSNED[0] = (_ekf->velNED[0] - _gps.vel_n_m_s) / gps_dt; _ekf->accelGPSNED[1] = (_ekf->velNED[1] - _gps.vel_e_m_s) / gps_dt; _ekf->accelGPSNED[2] = (_ekf->velNED[2] - _gps.vel_d_m_s) / gps_dt; } _ekf->velNED[0] = _gps.vel_n_m_s; _ekf->velNED[1] = _gps.vel_e_m_s; _ekf->velNED[2] = _gps.vel_d_m_s; _ekf->calcposNED(posNED, _ekf->gpsLat, _ekf->gpsLon, _ekf->gpsHgt, _ekf->latRef, _ekf->lonRef, _ekf->hgtRef); _ekf->posNE[0] = posNED[0]; _ekf->posNE[1] = posNED[1]; // set fusion flags _ekf->fuseVelData = true; _ekf->fusePosData = true; // recall states stored at time of measurement after adjusting for delays _ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms)); _ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms)); // run the fusion step _ekf->FuseVelposNED(); } else if (_ekf->statesInitialised) { // Convert GPS measurements to Pos NE, hgt and Vel NED _ekf->velNED[0] = 0.0f; _ekf->velNED[1] = 0.0f; _ekf->velNED[2] = 0.0f; _ekf->posNE[0] = 0.0f; _ekf->posNE[1] = 0.0f; // set fusion flags _ekf->fuseVelData = true; _ekf->fusePosData = true; // recall states stored at time of measurement after adjusting for delays _ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms)); _ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms)); // run the fusion step _ekf->FuseVelposNED(); } else { _ekf->fuseVelData = false; _ekf->fusePosData = false; } if (newHgtData && _ekf->statesInitialised) { // Could use a blend of GPS and baro alt data if desired _ekf->hgtMea = 1.0f * (_ekf->baroHgt - _baro_ref); _ekf->fuseHgtData = true; // recall states stored at time of measurement after adjusting for delays _ekf->RecallStates(_ekf->statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms)); // run the fusion step _ekf->FuseVelposNED(); } else { _ekf->fuseHgtData = false; } // Fuse Magnetometer Measurements if (newDataMag && _ekf->statesInitialised) { _ekf->fuseMagData = true; _ekf->RecallStates(_ekf->statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data _ekf->magstate.obsIndex = 0; _ekf->FuseMagnetometer(); _ekf->FuseMagnetometer(); _ekf->FuseMagnetometer(); } else { _ekf->fuseMagData = false; } // Fuse Airspeed Measurements if (newAdsData && _ekf->statesInitialised && _ekf->VtasMeas > 8.0f) { _ekf->fuseVtasData = true; _ekf->RecallStates(_ekf->statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data _ekf->FuseAirspeed(); } else { _ekf->fuseVtasData = false; } // Output results math::Quaternion q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]); math::Matrix<3, 3> R = q.to_dcm(); math::Vector<3> euler = R.to_euler(); for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) _att.R[i][j] = R(i, j); _att.timestamp = _last_sensor_timestamp; _att.q[0] = _ekf->states[0]; _att.q[1] = _ekf->states[1]; _att.q[2] = _ekf->states[2]; _att.q[3] = _ekf->states[3]; _att.q_valid = true; _att.R_valid = true; _att.timestamp = _last_sensor_timestamp; _att.roll = euler(0); _att.pitch = euler(1); _att.yaw = euler(2); _att.rollspeed = _ekf->angRate.x - _ekf->states[10]; _att.pitchspeed = _ekf->angRate.y - _ekf->states[11]; _att.yawspeed = _ekf->angRate.z - _ekf->states[12]; // gyro offsets _att.rate_offsets[0] = _ekf->states[10]; _att.rate_offsets[1] = _ekf->states[11]; _att.rate_offsets[2] = _ekf->states[12]; /* lazily publish the attitude only once available */ if (_att_pub > 0) { /* publish the attitude setpoint */ orb_publish(ORB_ID(vehicle_attitude), _att_pub, &_att); } else { /* advertise and publish */ _att_pub = orb_advertise(ORB_ID(vehicle_attitude), &_att); } if (_gps_initialized) { _local_pos.timestamp = _last_sensor_timestamp; _local_pos.x = _ekf->states[7]; _local_pos.y = _ekf->states[8]; // XXX need to announce change of Z reference somehow elegantly _local_pos.z = _ekf->states[9] - _baro_ref_offset; _local_pos.vx = _ekf->states[4]; _local_pos.vy = _ekf->states[5]; _local_pos.vz = _ekf->states[6]; _local_pos.xy_valid = _gps_initialized; _local_pos.z_valid = true; _local_pos.v_xy_valid = _gps_initialized; _local_pos.v_z_valid = true; _local_pos.xy_global = true; _velocity_xy_filtered = 0.95f*_velocity_xy_filtered + 0.05f*sqrtf(_local_pos.vx*_local_pos.vx + _local_pos.vy*_local_pos.vy); _velocity_z_filtered = 0.95f*_velocity_z_filtered + 0.05f*fabsf(_local_pos.vz); _airspeed_filtered = 0.95f*_airspeed_filtered + + 0.05f*_airspeed.true_airspeed_m_s; /* crude land detector for fixedwing only, * TODO: adapt so that it works for both, maybe move to another location */ if (_velocity_xy_filtered < 5 && _velocity_z_filtered < 10 && _airspeed_filtered < 10) { _local_pos.landed = true; } else { _local_pos.landed = false; } _local_pos.z_global = false; _local_pos.yaw = _att.yaw; /* lazily publish the local position only once available */ if (_local_pos_pub > 0) { /* publish the attitude setpoint */ orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &_local_pos); } else { /* advertise and publish */ _local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &_local_pos); } _global_pos.timestamp = _local_pos.timestamp; if (_local_pos.xy_global) { double est_lat, est_lon; map_projection_reproject(&_pos_ref, _local_pos.x, _local_pos.y, &est_lat, &est_lon); _global_pos.lat = est_lat; _global_pos.lon = est_lon; _global_pos.time_gps_usec = _gps.time_gps_usec; _global_pos.eph = _gps.eph; _global_pos.epv = _gps.epv; } if (_local_pos.v_xy_valid) { _global_pos.vel_n = _local_pos.vx; _global_pos.vel_e = _local_pos.vy; } else { _global_pos.vel_n = 0.0f; _global_pos.vel_e = 0.0f; } /* local pos alt is negative, change sign and add alt offsets */ _global_pos.alt = _baro_ref + (-_local_pos.z) - _baro_gps_offset; if (_local_pos.v_z_valid) { _global_pos.vel_d = _local_pos.vz; } _global_pos.yaw = _local_pos.yaw; _global_pos.eph = _gps.eph; _global_pos.epv = _gps.epv; _global_pos.timestamp = _local_pos.timestamp; /* lazily publish the global position only once available */ if (_global_pos_pub > 0) { /* publish the global position */ orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &_global_pos); } else { /* advertise and publish */ _global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &_global_pos); } if (hrt_elapsed_time(&_wind.timestamp) > 99000) { _wind.timestamp = _global_pos.timestamp; _wind.windspeed_north = _ekf->states[14]; _wind.windspeed_east = _ekf->states[15]; _wind.covariance_north = 0.0f; // XXX get form filter _wind.covariance_east = 0.0f; /* lazily publish the wind estimate only once available */ if (_wind_pub > 0) { /* publish the wind estimate */ orb_publish(ORB_ID(wind_estimate), _wind_pub, &_wind); } else { /* advertise and publish */ _wind_pub = orb_advertise(ORB_ID(wind_estimate), &_wind); } } } } if (hrt_elapsed_time(&_wind.timestamp) > 99000) { _wind.timestamp = _global_pos.timestamp; _wind.windspeed_north = _ekf->states[14]; _wind.windspeed_east = _ekf->states[15]; _wind.covariance_north = _ekf->P[14][14]; _wind.covariance_east = _ekf->P[15][15]; /* lazily publish the wind estimate only once available */ if (_wind_pub > 0) { /* publish the wind estimate */ orb_publish(ORB_ID(wind_estimate), _wind_pub, &_wind); } else { /* advertise and publish */ _wind_pub = orb_advertise(ORB_ID(wind_estimate), &_wind); } } } } perf_end(_loop_perf); } warnx("exiting.\n"); _estimator_task = -1; _exit(0); }
void Ekf2::task_main() { // subscribe to relevant topics _sensors_sub = orb_subscribe(ORB_ID(sensor_combined)); _gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _optical_flow_sub = orb_subscribe(ORB_ID(optical_flow)); _range_finder_sub = orb_subscribe(ORB_ID(distance_sensor)); _vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected)); px4_pollfd_struct_t fds[2] = {}; fds[0].fd = _sensors_sub; fds[0].events = POLLIN; fds[1].fd = _params_sub; fds[1].events = POLLIN; // initialise parameter cache updateParams(); // initialize data structures outside of loop // because they will else not always be // properly populated sensor_combined_s sensors = {}; vehicle_gps_position_s gps = {}; airspeed_s airspeed = {}; optical_flow_s optical_flow = {}; distance_sensor_s range_finder = {}; vehicle_land_detected_s vehicle_land_detected = {}; while (!_task_should_exit) { int ret = px4_poll(fds, sizeof(fds) / sizeof(fds[0]), 1000); if (ret < 0) { // Poll error, sleep and try again usleep(10000); continue; } else if (ret == 0) { // Poll timeout or no new data, do nothing continue; } if (fds[1].revents & POLLIN) { // read from param to clear updated flag struct parameter_update_s update; orb_copy(ORB_ID(parameter_update), _params_sub, &update); updateParams(); // fetch sensor data in next loop continue; } else if (!(fds[0].revents & POLLIN)) { // no new data continue; } bool gps_updated = false; bool airspeed_updated = false; bool optical_flow_updated = false; bool range_finder_updated = false; bool vehicle_land_detected_updated = false; orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors); // update all other topics if they have new data orb_check(_gps_sub, &gps_updated); if (gps_updated) { orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &gps); } orb_check(_airspeed_sub, &airspeed_updated); if (airspeed_updated) { orb_copy(ORB_ID(airspeed), _airspeed_sub, &airspeed); } orb_check(_optical_flow_sub, &optical_flow_updated); if (optical_flow_updated) { orb_copy(ORB_ID(optical_flow), _optical_flow_sub, &optical_flow); } orb_check(_range_finder_sub, &range_finder_updated); if (range_finder_updated) { orb_copy(ORB_ID(distance_sensor), _range_finder_sub, &range_finder); } // in replay mode we are getting the actual timestamp from the sensor topic hrt_abstime now = 0; if (_replay_mode) { now = sensors.timestamp; } else { now = hrt_absolute_time(); } // push imu data into estimator _ekf.setIMUData(now, sensors.gyro_integral_dt[0], sensors.accelerometer_integral_dt[0], &sensors.gyro_integral_rad[0], &sensors.accelerometer_integral_m_s[0]); // read mag data _ekf.setMagData(sensors.magnetometer_timestamp[0], &sensors.magnetometer_ga[0]); // read baro data _ekf.setBaroData(sensors.baro_timestamp[0], &sensors.baro_alt_meter[0]); // read gps data if available if (gps_updated) { struct gps_message gps_msg = {}; gps_msg.time_usec = gps.timestamp_position; gps_msg.lat = gps.lat; gps_msg.lon = gps.lon; gps_msg.alt = gps.alt; gps_msg.fix_type = gps.fix_type; gps_msg.eph = gps.eph; gps_msg.epv = gps.epv; gps_msg.sacc = gps.s_variance_m_s; gps_msg.time_usec_vel = gps.timestamp_velocity; gps_msg.vel_m_s = gps.vel_m_s; gps_msg.vel_ned[0] = gps.vel_n_m_s; gps_msg.vel_ned[1] = gps.vel_e_m_s; gps_msg.vel_ned[2] = gps.vel_d_m_s; gps_msg.vel_ned_valid = gps.vel_ned_valid; gps_msg.nsats = gps.satellites_used; //TODO add gdop to gps topic gps_msg.gdop = 0.0f; _ekf.setGpsData(gps.timestamp_position, &gps_msg); } // read airspeed data if available float eas2tas = airspeed.true_airspeed_m_s / airspeed.indicated_airspeed_m_s; if (airspeed_updated && airspeed.true_airspeed_m_s > 7.0f) { _ekf.setAirspeedData(airspeed.timestamp, &airspeed.true_airspeed_m_s, &eas2tas); } if (optical_flow_updated) { flow_message flow; flow.flowdata(0) = optical_flow.pixel_flow_x_integral; flow.flowdata(1) = optical_flow.pixel_flow_y_integral; flow.quality = optical_flow.quality; flow.gyrodata(0) = optical_flow.gyro_x_rate_integral; flow.gyrodata(1) = optical_flow.gyro_y_rate_integral; flow.gyrodata(2) = optical_flow.gyro_z_rate_integral; flow.dt = optical_flow.integration_timespan; if (PX4_ISFINITE(optical_flow.pixel_flow_y_integral) && PX4_ISFINITE(optical_flow.pixel_flow_x_integral)) { _ekf.setOpticalFlowData(optical_flow.timestamp, &flow); } } if (range_finder_updated) { _ekf.setRangeData(range_finder.timestamp, &range_finder.current_distance); } orb_check(_vehicle_land_detected_sub, &vehicle_land_detected_updated); if (vehicle_land_detected_updated) { orb_copy(ORB_ID(vehicle_land_detected), _vehicle_land_detected_sub, &vehicle_land_detected); _ekf.set_in_air_status(!vehicle_land_detected.landed); } // run the EKF update and output if (_ekf.update()) { // generate vehicle attitude quaternion data struct vehicle_attitude_s att = {}; _ekf.copy_quaternion(att.q); matrix::Quaternion<float> q(att.q[0], att.q[1], att.q[2], att.q[3]); // generate control state data control_state_s ctrl_state = {}; ctrl_state.timestamp = hrt_absolute_time(); ctrl_state.roll_rate = _lp_roll_rate.apply(sensors.gyro_rad_s[0]); ctrl_state.pitch_rate = _lp_pitch_rate.apply(sensors.gyro_rad_s[1]); ctrl_state.yaw_rate = _lp_yaw_rate.apply(sensors.gyro_rad_s[2]); // Velocity in body frame float velocity[3]; _ekf.get_velocity(velocity); Vector3f v_n(velocity); matrix::Dcm<float> R_to_body(q.inversed()); Vector3f v_b = R_to_body * v_n; ctrl_state.x_vel = v_b(0); ctrl_state.y_vel = v_b(1); ctrl_state.z_vel = v_b(2); // Local Position NED float position[3]; _ekf.get_position(position); ctrl_state.x_pos = position[0]; ctrl_state.y_pos = position[1]; ctrl_state.z_pos = position[2]; // Attitude quaternion ctrl_state.q[0] = q(0); ctrl_state.q[1] = q(1); ctrl_state.q[2] = q(2); ctrl_state.q[3] = q(3); // Acceleration data matrix::Vector<float, 3> acceleration = {&sensors.accelerometer_m_s2[0]}; float accel_bias[3]; _ekf.get_accel_bias(accel_bias); ctrl_state.x_acc = acceleration(0) - accel_bias[0]; ctrl_state.y_acc = acceleration(1) - accel_bias[1]; ctrl_state.z_acc = acceleration(2) - accel_bias[2]; // compute lowpass filtered horizontal acceleration acceleration = R_to_body.transpose() * acceleration; _acc_hor_filt = 0.95f * _acc_hor_filt + 0.05f * sqrtf(acceleration(0) * acceleration(0) + acceleration( 1) * acceleration(1)); ctrl_state.horz_acc_mag = _acc_hor_filt; // Airspeed - take airspeed measurement directly here as no wind is estimated if (PX4_ISFINITE(airspeed.indicated_airspeed_m_s) && hrt_absolute_time() - airspeed.timestamp < 1e6 && airspeed.timestamp > 0) { ctrl_state.airspeed = airspeed.indicated_airspeed_m_s; ctrl_state.airspeed_valid = true; } else { ctrl_state.airspeed_valid = false; } // publish control state data if (_control_state_pub == nullptr) { _control_state_pub = orb_advertise(ORB_ID(control_state), &ctrl_state); } else { orb_publish(ORB_ID(control_state), _control_state_pub, &ctrl_state); } // generate remaining vehicle attitude data att.timestamp = hrt_absolute_time(); matrix::Euler<float> euler(q); att.roll = euler(0); att.pitch = euler(1); att.yaw = euler(2); att.q[0] = q(0); att.q[1] = q(1); att.q[2] = q(2); att.q[3] = q(3); att.q_valid = true; att.rollspeed = sensors.gyro_rad_s[0]; att.pitchspeed = sensors.gyro_rad_s[1]; att.yawspeed = sensors.gyro_rad_s[2]; // publish vehicle attitude data if (_att_pub == nullptr) { _att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att); } else { orb_publish(ORB_ID(vehicle_attitude), _att_pub, &att); } // generate vehicle local position data struct vehicle_local_position_s lpos = {}; float pos[3] = {}; float vel[3] = {}; lpos.timestamp = hrt_absolute_time(); // Position of body origin in local NED frame _ekf.get_position(pos); lpos.x = pos[0]; lpos.y = pos[1]; lpos.z = pos[2]; // Velocity of body origin in local NED frame (m/s) _ekf.get_velocity(vel); lpos.vx = vel[0]; lpos.vy = vel[1]; lpos.vz = vel[2]; // TODO: better status reporting lpos.xy_valid = _ekf.local_position_is_valid(); lpos.z_valid = true; lpos.v_xy_valid = _ekf.local_position_is_valid(); lpos.v_z_valid = true; // Position of local NED origin in GPS / WGS84 frame struct map_projection_reference_s ekf_origin = {}; // true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon) _ekf.get_ekf_origin(&lpos.ref_timestamp, &ekf_origin, &lpos.ref_alt); lpos.xy_global = _ekf.global_position_is_valid(); lpos.z_global = true; // true if z is valid and has valid global reference (ref_alt) lpos.ref_lat = ekf_origin.lat_rad * 180.0 / M_PI; // Reference point latitude in degrees lpos.ref_lon = ekf_origin.lon_rad * 180.0 / M_PI; // Reference point longitude in degrees // The rotation of the tangent plane vs. geographical north lpos.yaw = att.yaw; float terrain_vpos; lpos.dist_bottom_valid = _ekf.get_terrain_vert_pos(&terrain_vpos); lpos.dist_bottom = terrain_vpos - pos[2]; // Distance to bottom surface (ground) in meters lpos.dist_bottom_rate = -vel[2]; // Distance to bottom surface (ground) change rate lpos.surface_bottom_timestamp = hrt_absolute_time(); // Time when new bottom surface found // TODO: uORB definition does not define what these variables are. We have assumed them to be horizontal and vertical 1-std dev accuracy in metres Vector3f pos_var, vel_var; _ekf.get_pos_var(pos_var); _ekf.get_vel_var(vel_var); lpos.eph = sqrt(pos_var(0) + pos_var(1)); lpos.epv = sqrt(pos_var(2)); // publish vehicle local position data if (_lpos_pub == nullptr) { _lpos_pub = orb_advertise(ORB_ID(vehicle_local_position), &lpos); } else { orb_publish(ORB_ID(vehicle_local_position), _lpos_pub, &lpos); } // generate and publish global position data struct vehicle_global_position_s global_pos = {}; if (_ekf.global_position_is_valid()) { global_pos.timestamp = hrt_absolute_time(); // Time of this estimate, in microseconds since system start global_pos.time_utc_usec = gps.time_utc_usec; // GPS UTC timestamp in microseconds double est_lat, est_lon; map_projection_reproject(&ekf_origin, lpos.x, lpos.y, &est_lat, &est_lon); global_pos.lat = est_lat; // Latitude in degrees global_pos.lon = est_lon; // Longitude in degrees global_pos.alt = -pos[2] + lpos.ref_alt; // Altitude AMSL in meters global_pos.vel_n = vel[0]; // Ground north velocity, m/s global_pos.vel_e = vel[1]; // Ground east velocity, m/s global_pos.vel_d = vel[2]; // Ground downside velocity, m/s global_pos.yaw = euler(2); // Yaw in radians -PI..+PI. global_pos.eph = sqrt(pos_var(0) + pos_var(1));; // Standard deviation of position estimate horizontally global_pos.epv = sqrt(pos_var(2)); // Standard deviation of position vertically // TODO: implement terrain estimator global_pos.terrain_alt = 0.0f; // Terrain altitude in m, WGS84 global_pos.terrain_alt_valid = false; // Terrain altitude estimate is valid // TODO use innovatun consistency check timouts to set this global_pos.dead_reckoning = false; // True if this position is estimated through dead-reckoning global_pos.pressure_alt = sensors.baro_alt_meter[0]; // Pressure altitude AMSL (m) if (_vehicle_global_position_pub == nullptr) { _vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos); } else { orb_publish(ORB_ID(vehicle_global_position), _vehicle_global_position_pub, &global_pos); } } } else if (_replay_mode) { // in replay mode we have to tell the replay module not to wait for an update // we do this by publishing an attitude with zero timestamp struct vehicle_attitude_s att = {}; att.timestamp = 0; if (_att_pub == nullptr) { _att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att); } else { orb_publish(ORB_ID(vehicle_attitude), _att_pub, &att); } } // publish estimator status struct estimator_status_s status = {}; status.timestamp = hrt_absolute_time(); _ekf.get_state_delayed(status.states); _ekf.get_covariances(status.covariances); _ekf.get_gps_check_status(&status.gps_check_fail_flags); _ekf.get_control_mode(&status.control_mode_flags); _ekf.get_filter_fault_status(&status.filter_fault_flags); if (_estimator_status_pub == nullptr) { _estimator_status_pub = orb_advertise(ORB_ID(estimator_status), &status); } else { orb_publish(ORB_ID(estimator_status), _estimator_status_pub, &status); } // Publish wind estimate struct wind_estimate_s wind_estimate = {}; wind_estimate.timestamp = hrt_absolute_time(); wind_estimate.windspeed_north = status.states[22]; wind_estimate.windspeed_east = status.states[23]; wind_estimate.covariance_north = status.covariances[22]; wind_estimate.covariance_east = status.covariances[23]; if (_wind_pub == nullptr) { _wind_pub = orb_advertise(ORB_ID(wind_estimate), &wind_estimate); } else { orb_publish(ORB_ID(wind_estimate), _wind_pub, &wind_estimate); } // publish estimator innovation data struct ekf2_innovations_s innovations = {}; innovations.timestamp = hrt_absolute_time(); _ekf.get_vel_pos_innov(&innovations.vel_pos_innov[0]); _ekf.get_mag_innov(&innovations.mag_innov[0]); _ekf.get_heading_innov(&innovations.heading_innov); _ekf.get_airspeed_innov(&innovations.airspeed_innov); _ekf.get_flow_innov(&innovations.flow_innov[0]); _ekf.get_hagl_innov(&innovations.hagl_innov); _ekf.get_vel_pos_innov_var(&innovations.vel_pos_innov_var[0]); _ekf.get_mag_innov_var(&innovations.mag_innov_var[0]); _ekf.get_heading_innov_var(&innovations.heading_innov_var); _ekf.get_airspeed_innov_var(&innovations.airspeed_innov_var); _ekf.get_flow_innov_var(&innovations.flow_innov_var[0]); _ekf.get_hagl_innov_var(&innovations.hagl_innov_var); if (_estimator_innovations_pub == nullptr) { _estimator_innovations_pub = orb_advertise(ORB_ID(ekf2_innovations), &innovations); } else { orb_publish(ORB_ID(ekf2_innovations), _estimator_innovations_pub, &innovations); } // save the declination to the EKF2_MAG_DECL parameter when a land event is detected if ((_params->mag_declination_source & (1 << 1)) && !_prev_landed && vehicle_land_detected.landed) { float decl_deg; _ekf.copy_mag_decl_deg(&decl_deg); _mag_declination_deg.set(decl_deg); } _prev_landed = vehicle_land_detected.landed; // publish replay message if in replay mode bool publish_replay_message = (bool)_param_record_replay_msg.get(); if (publish_replay_message) { struct ekf2_replay_s replay = {}; replay.time_ref = now; replay.gyro_integral_dt = sensors.gyro_integral_dt[0]; replay.accelerometer_integral_dt = sensors.accelerometer_integral_dt[0]; replay.magnetometer_timestamp = sensors.magnetometer_timestamp[0]; replay.baro_timestamp = sensors.baro_timestamp[0]; memcpy(&replay.gyro_integral_rad[0], &sensors.gyro_integral_rad[0], sizeof(replay.gyro_integral_rad)); memcpy(&replay.accelerometer_integral_m_s[0], &sensors.accelerometer_integral_m_s[0], sizeof(replay.accelerometer_integral_m_s)); memcpy(&replay.magnetometer_ga[0], &sensors.magnetometer_ga[0], sizeof(replay.magnetometer_ga)); replay.baro_alt_meter = sensors.baro_alt_meter[0]; // only write gps data if we had a gps update. if (gps_updated) { replay.time_usec = gps.timestamp_position; replay.time_usec_vel = gps.timestamp_velocity; replay.lat = gps.lat; replay.lon = gps.lon; replay.alt = gps.alt; replay.fix_type = gps.fix_type; replay.nsats = gps.satellites_used; replay.eph = gps.eph; replay.epv = gps.epv; replay.sacc = gps.s_variance_m_s; replay.vel_m_s = gps.vel_m_s; replay.vel_n_m_s = gps.vel_n_m_s; replay.vel_e_m_s = gps.vel_e_m_s; replay.vel_d_m_s = gps.vel_d_m_s; replay.vel_ned_valid = gps.vel_ned_valid; } else { // this will tell the logging app not to bother logging any gps replay data replay.time_usec = 0; } if (optical_flow_updated) { replay.flow_timestamp = optical_flow.timestamp; replay.flow_pixel_integral[0] = optical_flow.pixel_flow_x_integral; replay.flow_pixel_integral[1] = optical_flow.pixel_flow_y_integral; replay.flow_gyro_integral[0] = optical_flow.gyro_x_rate_integral; replay.flow_gyro_integral[1] = optical_flow.gyro_y_rate_integral; replay.flow_time_integral = optical_flow.integration_timespan; replay.flow_quality = optical_flow.quality; } else { replay.flow_timestamp = 0; } if (range_finder_updated) { replay.rng_timestamp = range_finder.timestamp; replay.range_to_ground = range_finder.current_distance; } else { replay.rng_timestamp = 0; } if (airspeed_updated) { replay.asp_timestamp = airspeed.timestamp; replay.indicated_airspeed_m_s = airspeed.indicated_airspeed_m_s; replay.true_airspeed_m_s = airspeed.true_airspeed_m_s; replay.true_airspeed_unfiltered_m_s = airspeed.true_airspeed_unfiltered_m_s; replay.air_temperature_celsius = airspeed.air_temperature_celsius; replay.confidence = airspeed.confidence; } else { replay.asp_timestamp = 0; } if (_replay_pub == nullptr) { _replay_pub = orb_advertise(ORB_ID(ekf2_replay), &replay); } else { orb_publish(ORB_ID(ekf2_replay), _replay_pub, &replay); } } } delete ekf2::instance; ekf2::instance = nullptr; }
void FollowTarget::on_active() { struct map_projection_reference_s target_ref; math::Vector<3> target_reported_velocity(0, 0, 0); follow_target_s target_motion_with_offset = {}; uint64_t current_time = hrt_absolute_time(); bool _radius_entered = false; bool _radius_exited = false; bool updated = false; float dt_ms = 0; orb_check(_follow_target_sub, &updated); if (updated) { follow_target_s target_motion; _target_updates++; // save last known motion topic _previous_target_motion = _current_target_motion; orb_copy(ORB_ID(follow_target), _follow_target_sub, &target_motion); if (_current_target_motion.timestamp == 0) { _current_target_motion = target_motion; } _current_target_motion.timestamp = target_motion.timestamp; _current_target_motion.lat = (_current_target_motion.lat * (double)_responsiveness) + target_motion.lat * (double)( 1 - _responsiveness); _current_target_motion.lon = (_current_target_motion.lon * (double)_responsiveness) + target_motion.lon * (double)( 1 - _responsiveness); target_reported_velocity(0) = _current_target_motion.vx; target_reported_velocity(1) = _current_target_motion.vy; } else if (((current_time - _current_target_motion.timestamp) / 1000) > TARGET_TIMEOUT_MS && target_velocity_valid()) { reset_target_validity(); } // update distance to target if (target_position_valid()) { // get distance to target map_projection_init(&target_ref, _navigator->get_global_position()->lat, _navigator->get_global_position()->lon); map_projection_project(&target_ref, _current_target_motion.lat, _current_target_motion.lon, &_target_distance(0), &_target_distance(1)); } // update target velocity if (target_velocity_valid() && updated) { dt_ms = ((_current_target_motion.timestamp - _previous_target_motion.timestamp) / 1000); // ignore a small dt if (dt_ms > 10.0F) { // get last gps known reference for target map_projection_init(&target_ref, _previous_target_motion.lat, _previous_target_motion.lon); // calculate distance the target has moved map_projection_project(&target_ref, _current_target_motion.lat, _current_target_motion.lon, &(_target_position_delta(0)), &(_target_position_delta(1))); // update the average velocity of the target based on the position _est_target_vel = _target_position_delta / (dt_ms / 1000.0f); // if the target is moving add an offset and rotation if (_est_target_vel.length() > .5F) { _target_position_offset = _rot_matrix * _est_target_vel.normalized() * _follow_offset; } // are we within the target acceptance radius? // give a buffer to exit/enter the radius to give the velocity controller // a chance to catch up _radius_exited = ((_target_position_offset + _target_distance).length() > (float) TARGET_ACCEPTANCE_RADIUS_M * 1.5f); _radius_entered = ((_target_position_offset + _target_distance).length() < (float) TARGET_ACCEPTANCE_RADIUS_M); // to keep the velocity increase/decrease smooth // calculate how many velocity increments/decrements // it will take to reach the targets velocity // with the given amount of steps also add a feed forward input that adjusts the // velocity as the position gap increases since // just traveling at the exact velocity of the target will not // get any closer or farther from the target _step_vel = (_est_target_vel - _current_vel) + (_target_position_offset + _target_distance) * FF_K; _step_vel /= (dt_ms / 1000.0F * (float) INTERPOLATION_PNTS); _step_time_in_ms = (dt_ms / (float) INTERPOLATION_PNTS); // if we are less than 1 meter from the target don't worry about trying to yaw // lock the yaw until we are at a distance that makes sense if ((_target_distance).length() > 1.0F) { // yaw rate smoothing // this really needs to control the yaw rate directly in the attitude pid controller // but seems to work ok for now since the yaw rate cannot be controlled directly in auto mode _yaw_angle = get_bearing_to_next_waypoint(_navigator->get_global_position()->lat, _navigator->get_global_position()->lon, _current_target_motion.lat, _current_target_motion.lon); _yaw_rate = (_yaw_angle - _navigator->get_global_position()->yaw) / (dt_ms / 1000.0F); _yaw_rate = _wrap_pi(_yaw_rate); _yaw_rate = math::constrain(_yaw_rate, -1.0F * _yaw_auto_max, _yaw_auto_max); } else { _yaw_angle = _yaw_rate = NAN; } } // warnx(" _step_vel x %3.6f y %3.6f cur vel %3.6f %3.6f tar vel %3.6f %3.6f dist = %3.6f (%3.6f) mode = %d con ratio = %3.6f yaw rate = %3.6f", // (double) _step_vel(0), // (double) _step_vel(1), // (double) _current_vel(0), // (double) _current_vel(1), // (double) _est_target_vel(0), // (double) _est_target_vel(1), // (double) (_target_distance).length(), // (double) (_target_position_offset + _target_distance).length(), // _follow_target_state, // (double)_avg_cos_ratio, (double) _yaw_rate); } if (target_position_valid()) { // get the target position using the calculated offset map_projection_init(&target_ref, _current_target_motion.lat, _current_target_motion.lon); map_projection_reproject(&target_ref, _target_position_offset(0), _target_position_offset(1), &target_motion_with_offset.lat, &target_motion_with_offset.lon); } // clamp yaw rate smoothing if we are with in // 3 degrees of facing target if (PX4_ISFINITE(_yaw_rate)) { if (fabsf(fabsf(_yaw_angle) - fabsf(_navigator->get_global_position()->yaw)) < math::radians(3.0F)) { _yaw_rate = NAN; } } // update state machine switch (_follow_target_state) { case TRACK_POSITION: { if (_radius_entered == true) { _follow_target_state = TRACK_VELOCITY; } else if (target_velocity_valid()) { set_follow_target_item(&_mission_item, _param_min_alt.get(), target_motion_with_offset, _yaw_angle); // keep the current velocity updated with the target velocity for when it's needed _current_vel = _est_target_vel; update_position_sp(true, true, _yaw_rate); } else { _follow_target_state = SET_WAIT_FOR_TARGET_POSITION; } break; } case TRACK_VELOCITY: { if (_radius_exited == true) { _follow_target_state = TRACK_POSITION; } else if (target_velocity_valid()) { if ((float)(current_time - _last_update_time) / 1000.0f >= _step_time_in_ms) { _current_vel += _step_vel; _last_update_time = current_time; } set_follow_target_item(&_mission_item, _param_min_alt.get(), target_motion_with_offset, _yaw_angle); update_position_sp(true, false, _yaw_rate); } else { _follow_target_state = SET_WAIT_FOR_TARGET_POSITION; } break; } case SET_WAIT_FOR_TARGET_POSITION: { // Climb to the minimum altitude // and wait until a position is received follow_target_s target = {}; // for now set the target at the minimum height above the uav target.lat = _navigator->get_global_position()->lat; target.lon = _navigator->get_global_position()->lon; target.alt = 0.0F; set_follow_target_item(&_mission_item, _param_min_alt.get(), target, _yaw_angle); update_position_sp(false, false, _yaw_rate); _follow_target_state = WAIT_FOR_TARGET_POSITION; } /* FALLTHROUGH */ case WAIT_FOR_TARGET_POSITION: { if (is_mission_item_reached() && target_velocity_valid()) { _target_position_offset(0) = _follow_offset; _follow_target_state = TRACK_POSITION; } break; } } }
/**************************************************************************** * main ****************************************************************************/ int position_estimator_inav_thread_main(int argc, char *argv[]) { orb_advert_t mavlink_log_pub = nullptr; float x_est[2] = { 0.0f, 0.0f }; // pos, vel float y_est[2] = { 0.0f, 0.0f }; // pos, vel float z_est[2] = { 0.0f, 0.0f }; // pos, vel float est_buf[EST_BUF_SIZE][3][2]; // estimated position buffer float R_buf[EST_BUF_SIZE][3][3]; // rotation matrix buffer float R_gps[3][3]; // rotation matrix for GPS correction moment memset(est_buf, 0, sizeof(est_buf)); memset(R_buf, 0, sizeof(R_buf)); memset(R_gps, 0, sizeof(R_gps)); int buf_ptr = 0; static const float min_eph_epv = 2.0f; // min EPH/EPV, used for weight calculation static const float max_eph_epv = 20.0f; // max EPH/EPV acceptable for estimation float eph = max_eph_epv; float epv = 1.0f; float eph_flow = 1.0f; float eph_vision = 0.2f; float epv_vision = 0.2f; float eph_mocap = 0.05f; float epv_mocap = 0.05f; float x_est_prev[2], y_est_prev[2], z_est_prev[2]; memset(x_est_prev, 0, sizeof(x_est_prev)); memset(y_est_prev, 0, sizeof(y_est_prev)); memset(z_est_prev, 0, sizeof(z_est_prev)); int baro_init_cnt = 0; int baro_init_num = 200; float baro_offset = 0.0f; // baro offset for reference altitude, initialized on start, then adjusted hrt_abstime accel_timestamp = 0; hrt_abstime baro_timestamp = 0; bool ref_inited = false; hrt_abstime ref_init_start = 0; const hrt_abstime ref_init_delay = 1000000; // wait for 1s after 3D fix struct map_projection_reference_s ref; memset(&ref, 0, sizeof(ref)); uint16_t accel_updates = 0; uint16_t baro_updates = 0; uint16_t gps_updates = 0; uint16_t attitude_updates = 0; uint16_t flow_updates = 0; uint16_t vision_updates = 0; uint16_t mocap_updates = 0; hrt_abstime updates_counter_start = hrt_absolute_time(); hrt_abstime pub_last = hrt_absolute_time(); hrt_abstime t_prev = 0; /* store error when sensor updates, but correct on each time step to avoid jumps in estimated value */ float acc[] = { 0.0f, 0.0f, 0.0f }; // N E D float acc_bias[] = { 0.0f, 0.0f, 0.0f }; // body frame float corr_baro = 0.0f; // D float corr_gps[3][2] = { { 0.0f, 0.0f }, // N (pos, vel) { 0.0f, 0.0f }, // E (pos, vel) { 0.0f, 0.0f }, // D (pos, vel) }; float w_gps_xy = 1.0f; float w_gps_z = 1.0f; float corr_vision[3][2] = { { 0.0f, 0.0f }, // N (pos, vel) { 0.0f, 0.0f }, // E (pos, vel) { 0.0f, 0.0f }, // D (pos, vel) }; float corr_mocap[3][1] = { { 0.0f }, // N (pos) { 0.0f }, // E (pos) { 0.0f }, // D (pos) }; const int mocap_heading = 2; float dist_ground = 0.0f; //variables for lidar altitude estimation float corr_lidar = 0.0f; float lidar_offset = 0.0f; int lidar_offset_count = 0; bool lidar_first = true; bool use_lidar = false; bool use_lidar_prev = false; float corr_flow[] = { 0.0f, 0.0f }; // N E float w_flow = 0.0f; hrt_abstime lidar_time = 0; // time of last lidar measurement (not filtered) hrt_abstime lidar_valid_time = 0; // time of last lidar measurement used for correction (filtered) int n_flow = 0; float gyro_offset_filtered[] = { 0.0f, 0.0f, 0.0f }; float flow_gyrospeed[] = { 0.0f, 0.0f, 0.0f }; float flow_gyrospeed_filtered[] = { 0.0f, 0.0f, 0.0f }; float att_gyrospeed_filtered[] = { 0.0f, 0.0f, 0.0f }; float yaw_comp[] = { 0.0f, 0.0f }; hrt_abstime flow_time = 0; float flow_min_dist = 0.2f; bool gps_valid = false; // GPS is valid bool lidar_valid = false; // lidar is valid bool flow_valid = false; // flow is valid bool flow_accurate = false; // flow should be accurate (this flag not updated if flow_valid == false) bool vision_valid = false; // vision is valid bool mocap_valid = false; // mocap is valid /* declare and safely initialize all structs */ struct actuator_controls_s actuator; memset(&actuator, 0, sizeof(actuator)); struct actuator_armed_s armed; memset(&armed, 0, sizeof(armed)); struct sensor_combined_s sensor; memset(&sensor, 0, sizeof(sensor)); struct vehicle_gps_position_s gps; memset(&gps, 0, sizeof(gps)); struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); struct vehicle_local_position_s local_pos; memset(&local_pos, 0, sizeof(local_pos)); struct optical_flow_s flow; memset(&flow, 0, sizeof(flow)); struct vision_position_estimate_s vision; memset(&vision, 0, sizeof(vision)); struct att_pos_mocap_s mocap; memset(&mocap, 0, sizeof(mocap)); struct vehicle_global_position_s global_pos; memset(&global_pos, 0, sizeof(global_pos)); struct distance_sensor_s lidar; memset(&lidar, 0, sizeof(lidar)); struct vehicle_rates_setpoint_s rates_setpoint; memset(&rates_setpoint, 0, sizeof(rates_setpoint)); /* subscribe */ int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); int actuator_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); int armed_sub = orb_subscribe(ORB_ID(actuator_armed)); int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow)); int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); int vision_position_estimate_sub = orb_subscribe(ORB_ID(vision_position_estimate)); int att_pos_mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap)); int distance_sensor_sub = orb_subscribe(ORB_ID(distance_sensor)); int vehicle_rate_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); /* advertise */ orb_advert_t vehicle_local_position_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos); orb_advert_t vehicle_global_position_pub = NULL; struct position_estimator_inav_params params; memset(¶ms, 0, sizeof(params)); struct position_estimator_inav_param_handles pos_inav_param_handles; /* initialize parameter handles */ inav_parameters_init(&pos_inav_param_handles); /* first parameters read at start up */ struct parameter_update_s param_update; orb_copy(ORB_ID(parameter_update), parameter_update_sub, ¶m_update); /* read from param topic to clear updated flag */ /* first parameters update */ inav_parameters_update(&pos_inav_param_handles, ¶ms); px4_pollfd_struct_t fds_init[1] = {}; fds_init[0].fd = sensor_combined_sub; fds_init[0].events = POLLIN; /* wait for initial baro value */ bool wait_baro = true; TerrainEstimator *terrain_estimator = new TerrainEstimator(); thread_running = true; hrt_abstime baro_wait_for_sample_time = hrt_absolute_time(); while (wait_baro && !thread_should_exit) { int ret = px4_poll(&fds_init[0], 1, 1000); if (ret < 0) { /* poll error */ mavlink_log_info(&mavlink_log_pub, "[inav] poll error on init"); } else if (hrt_absolute_time() - baro_wait_for_sample_time > MAX_WAIT_FOR_BARO_SAMPLE) { wait_baro = false; mavlink_log_info(&mavlink_log_pub, "[inav] timed out waiting for a baro sample"); } else if (ret > 0) { if (fds_init[0].revents & POLLIN) { orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); if (wait_baro && sensor.baro_timestamp[0] != baro_timestamp) { baro_timestamp = sensor.baro_timestamp[0]; baro_wait_for_sample_time = hrt_absolute_time(); /* mean calculation over several measurements */ if (baro_init_cnt < baro_init_num) { if (PX4_ISFINITE(sensor.baro_alt_meter[0])) { baro_offset += sensor.baro_alt_meter[0]; baro_init_cnt++; } } else { wait_baro = false; baro_offset /= (float) baro_init_cnt; local_pos.z_valid = true; local_pos.v_z_valid = true; } } } } else { PX4_WARN("INAV poll timeout"); } } /* main loop */ px4_pollfd_struct_t fds[1]; fds[0].fd = vehicle_attitude_sub; fds[0].events = POLLIN; while (!thread_should_exit) { int ret = px4_poll(fds, 1, 20); // wait maximal 20 ms = 50 Hz minimum rate hrt_abstime t = hrt_absolute_time(); if (ret < 0) { /* poll error */ mavlink_log_info(&mavlink_log_pub, "[inav] poll error on init"); continue; } else if (ret > 0) { /* act on attitude updates */ /* vehicle attitude */ orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); attitude_updates++; bool updated; /* parameter update */ orb_check(parameter_update_sub, &updated); if (updated) { struct parameter_update_s update; orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update); inav_parameters_update(&pos_inav_param_handles, ¶ms); } /* actuator */ orb_check(actuator_sub, &updated); if (updated) { orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_sub, &actuator); } /* armed */ orb_check(armed_sub, &updated); if (updated) { orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); } /* sensor combined */ orb_check(sensor_combined_sub, &updated); if (updated) { orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); if (sensor.accelerometer_timestamp[0] != accel_timestamp) { if (att.R_valid) { /* correct accel bias */ sensor.accelerometer_m_s2[0] -= acc_bias[0]; sensor.accelerometer_m_s2[1] -= acc_bias[1]; sensor.accelerometer_m_s2[2] -= acc_bias[2]; /* transform acceleration vector from body frame to NED frame */ for (int i = 0; i < 3; i++) { acc[i] = 0.0f; for (int j = 0; j < 3; j++) { acc[i] += PX4_R(att.R, i, j) * sensor.accelerometer_m_s2[j]; } } acc[2] += CONSTANTS_ONE_G; } else { memset(acc, 0, sizeof(acc)); } accel_timestamp = sensor.accelerometer_timestamp[0]; accel_updates++; } if (sensor.baro_timestamp[0] != baro_timestamp) { corr_baro = baro_offset - sensor.baro_alt_meter[0] - z_est[0]; baro_timestamp = sensor.baro_timestamp[0]; baro_updates++; } } /* lidar alt estimation */ orb_check(distance_sensor_sub, &updated); /* update lidar separately, needed by terrain estimator */ if (updated) { orb_copy(ORB_ID(distance_sensor), distance_sensor_sub, &lidar); lidar.current_distance += params.lidar_calibration_offset; } if (updated) { //check if altitude estimation for lidar is enabled and new sensor data if (params.enable_lidar_alt_est && lidar.current_distance > lidar.min_distance && lidar.current_distance < lidar.max_distance && (PX4_R(att.R, 2, 2) > 0.7f)) { if (!use_lidar_prev && use_lidar) { lidar_first = true; } use_lidar_prev = use_lidar; lidar_time = t; dist_ground = lidar.current_distance * PX4_R(att.R, 2, 2); //vertical distance if (lidar_first) { lidar_first = false; lidar_offset = dist_ground + z_est[0]; mavlink_log_info(&mavlink_log_pub, "[inav] LIDAR: new ground offset"); warnx("[inav] LIDAR: new ground offset"); } corr_lidar = lidar_offset - dist_ground - z_est[0]; if (fabsf(corr_lidar) > params.lidar_err) { //check for spike corr_lidar = 0; lidar_valid = false; lidar_offset_count++; if (lidar_offset_count > 3) { //if consecutive bigger/smaller measurements -> new ground offset -> reinit lidar_first = true; lidar_offset_count = 0; } } else { corr_lidar = lidar_offset - dist_ground - z_est[0]; lidar_valid = true; lidar_offset_count = 0; lidar_valid_time = t; } } else { lidar_valid = false; } } /* optical flow */ orb_check(optical_flow_sub, &updated); if (updated && lidar_valid) { orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow); flow_time = t; float flow_q = flow.quality / 255.0f; float dist_bottom = lidar.current_distance; if (dist_bottom > flow_min_dist && flow_q > params.flow_q_min && PX4_R(att.R, 2, 2) > 0.7f) { /* distance to surface */ //float flow_dist = dist_bottom / PX4_R(att.R, 2, 2); //use this if using sonar float flow_dist = dist_bottom; //use this if using lidar /* check if flow if too large for accurate measurements */ /* calculate estimated velocity in body frame */ float body_v_est[2] = { 0.0f, 0.0f }; for (int i = 0; i < 2; i++) { body_v_est[i] = PX4_R(att.R, 0, i) * x_est[1] + PX4_R(att.R, 1, i) * y_est[1] + PX4_R(att.R, 2, i) * z_est[1]; } /* set this flag if flow should be accurate according to current velocity and attitude rate estimate */ flow_accurate = fabsf(body_v_est[1] / flow_dist - att.rollspeed) < max_flow && fabsf(body_v_est[0] / flow_dist + att.pitchspeed) < max_flow; /*calculate offset of flow-gyro using already calibrated gyro from autopilot*/ flow_gyrospeed[0] = flow.gyro_x_rate_integral / (float)flow.integration_timespan * 1000000.0f; flow_gyrospeed[1] = flow.gyro_y_rate_integral / (float)flow.integration_timespan * 1000000.0f; flow_gyrospeed[2] = flow.gyro_z_rate_integral / (float)flow.integration_timespan * 1000000.0f; //moving average if (n_flow >= 100) { gyro_offset_filtered[0] = flow_gyrospeed_filtered[0] - att_gyrospeed_filtered[0]; gyro_offset_filtered[1] = flow_gyrospeed_filtered[1] - att_gyrospeed_filtered[1]; gyro_offset_filtered[2] = flow_gyrospeed_filtered[2] - att_gyrospeed_filtered[2]; n_flow = 0; flow_gyrospeed_filtered[0] = 0.0f; flow_gyrospeed_filtered[1] = 0.0f; flow_gyrospeed_filtered[2] = 0.0f; att_gyrospeed_filtered[0] = 0.0f; att_gyrospeed_filtered[1] = 0.0f; att_gyrospeed_filtered[2] = 0.0f; } else { flow_gyrospeed_filtered[0] = (flow_gyrospeed[0] + n_flow * flow_gyrospeed_filtered[0]) / (n_flow + 1); flow_gyrospeed_filtered[1] = (flow_gyrospeed[1] + n_flow * flow_gyrospeed_filtered[1]) / (n_flow + 1); flow_gyrospeed_filtered[2] = (flow_gyrospeed[2] + n_flow * flow_gyrospeed_filtered[2]) / (n_flow + 1); att_gyrospeed_filtered[0] = (att.pitchspeed + n_flow * att_gyrospeed_filtered[0]) / (n_flow + 1); att_gyrospeed_filtered[1] = (att.rollspeed + n_flow * att_gyrospeed_filtered[1]) / (n_flow + 1); att_gyrospeed_filtered[2] = (att.yawspeed + n_flow * att_gyrospeed_filtered[2]) / (n_flow + 1); n_flow++; } /*yaw compensation (flow sensor is not in center of rotation) -> params in QGC*/ yaw_comp[0] = - params.flow_module_offset_y * (flow_gyrospeed[2] - gyro_offset_filtered[2]); yaw_comp[1] = params.flow_module_offset_x * (flow_gyrospeed[2] - gyro_offset_filtered[2]); /* convert raw flow to angular flow (rad/s) */ float flow_ang[2]; /* check for vehicle rates setpoint - it below threshold -> dont subtract -> better hover */ orb_check(vehicle_rate_sp_sub, &updated); if (updated) orb_copy(ORB_ID(vehicle_rates_setpoint), vehicle_rate_sp_sub, &rates_setpoint); double rate_threshold = 0.15f; if (fabs(rates_setpoint.pitch) < rate_threshold) { //warnx("[inav] test ohne comp"); flow_ang[0] = (flow.pixel_flow_x_integral / (float)flow.integration_timespan * 1000000.0f) * params.flow_k;//for now the flow has to be scaled (to small) } else { //warnx("[inav] test mit comp"); //calculate flow [rad/s] and compensate for rotations (and offset of flow-gyro) flow_ang[0] = ((flow.pixel_flow_x_integral - flow.gyro_x_rate_integral) / (float)flow.integration_timespan * 1000000.0f + gyro_offset_filtered[0]) * params.flow_k;//for now the flow has to be scaled (to small) } if (fabs(rates_setpoint.roll) < rate_threshold) { flow_ang[1] = (flow.pixel_flow_y_integral / (float)flow.integration_timespan * 1000000.0f) * params.flow_k;//for now the flow has to be scaled (to small) } else { //calculate flow [rad/s] and compensate for rotations (and offset of flow-gyro) flow_ang[1] = ((flow.pixel_flow_y_integral - flow.gyro_y_rate_integral) / (float)flow.integration_timespan * 1000000.0f + gyro_offset_filtered[1]) * params.flow_k;//for now the flow has to be scaled (to small) } /* flow measurements vector */ float flow_m[3]; if (fabs(rates_setpoint.yaw) < rate_threshold) { flow_m[0] = -flow_ang[0] * flow_dist; flow_m[1] = -flow_ang[1] * flow_dist; } else { flow_m[0] = -flow_ang[0] * flow_dist - yaw_comp[0] * params.flow_k; flow_m[1] = -flow_ang[1] * flow_dist - yaw_comp[1] * params.flow_k; } flow_m[2] = z_est[1]; /* velocity in NED */ float flow_v[2] = { 0.0f, 0.0f }; /* project measurements vector to NED basis, skip Z component */ for (int i = 0; i < 2; i++) { for (int j = 0; j < 3; j++) { flow_v[i] += PX4_R(att.R, i, j) * flow_m[j]; } } /* velocity correction */ corr_flow[0] = flow_v[0] - x_est[1]; corr_flow[1] = flow_v[1] - y_est[1]; /* adjust correction weight */ float flow_q_weight = (flow_q - params.flow_q_min) / (1.0f - params.flow_q_min); w_flow = PX4_R(att.R, 2, 2) * flow_q_weight / fmaxf(1.0f, flow_dist); /* if flow is not accurate, reduce weight for it */ // TODO make this more fuzzy if (!flow_accurate) { w_flow *= 0.05f; } /* under ideal conditions, on 1m distance assume EPH = 10cm */ eph_flow = 0.1f / w_flow; flow_valid = true; } else { w_flow = 0.0f; flow_valid = false; } flow_updates++; } /* check no vision circuit breaker is set */ if (params.no_vision != CBRK_NO_VISION_KEY) { /* vehicle vision position */ orb_check(vision_position_estimate_sub, &updated); if (updated) { orb_copy(ORB_ID(vision_position_estimate), vision_position_estimate_sub, &vision); static float last_vision_x = 0.0f; static float last_vision_y = 0.0f; static float last_vision_z = 0.0f; /* reset position estimate on first vision update */ if (!vision_valid) { x_est[0] = vision.x; x_est[1] = vision.vx; y_est[0] = vision.y; y_est[1] = vision.vy; /* only reset the z estimate if the z weight parameter is not zero */ if (params.w_z_vision_p > MIN_VALID_W) { z_est[0] = vision.z; z_est[1] = vision.vz; } vision_valid = true; last_vision_x = vision.x; last_vision_y = vision.y; last_vision_z = vision.z; warnx("VISION estimate valid"); mavlink_log_info(&mavlink_log_pub, "[inav] VISION estimate valid"); } /* calculate correction for position */ corr_vision[0][0] = vision.x - x_est[0]; corr_vision[1][0] = vision.y - y_est[0]; corr_vision[2][0] = vision.z - z_est[0]; static hrt_abstime last_vision_time = 0; float vision_dt = (vision.timestamp_boot - last_vision_time) / 1e6f; last_vision_time = vision.timestamp_boot; if (vision_dt > 0.000001f && vision_dt < 0.2f) { vision.vx = (vision.x - last_vision_x) / vision_dt; vision.vy = (vision.y - last_vision_y) / vision_dt; vision.vz = (vision.z - last_vision_z) / vision_dt; last_vision_x = vision.x; last_vision_y = vision.y; last_vision_z = vision.z; /* calculate correction for velocity */ corr_vision[0][1] = vision.vx - x_est[1]; corr_vision[1][1] = vision.vy - y_est[1]; corr_vision[2][1] = vision.vz - z_est[1]; } else { /* assume zero motion */ corr_vision[0][1] = 0.0f - x_est[1]; corr_vision[1][1] = 0.0f - y_est[1]; corr_vision[2][1] = 0.0f - z_est[1]; } vision_updates++; } } /* vehicle mocap position */ orb_check(att_pos_mocap_sub, &updated); if (updated) { orb_copy(ORB_ID(att_pos_mocap), att_pos_mocap_sub, &mocap); if (!params.disable_mocap) { /* reset position estimate on first mocap update */ if (!mocap_valid) { x_est[0] = mocap.x; y_est[0] = mocap.y; z_est[0] = mocap.z; mocap_valid = true; warnx("MOCAP data valid"); mavlink_log_info(&mavlink_log_pub, "[inav] MOCAP data valid"); } /* calculate correction for position */ corr_mocap[0][0] = mocap.x - x_est[0]; corr_mocap[1][0] = mocap.y - y_est[0]; corr_mocap[2][0] = mocap.z - z_est[0]; mocap_updates++; } } /* vehicle GPS position */ orb_check(vehicle_gps_position_sub, &updated); if (updated) { orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); bool reset_est = false; /* hysteresis for GPS quality */ if (gps_valid) { if (gps.eph > max_eph_epv || gps.epv > max_eph_epv || gps.fix_type < 3) { gps_valid = false; mavlink_log_info(&mavlink_log_pub, "[inav] GPS signal lost"); warnx("[inav] GPS signal lost"); } } else { if (gps.eph < max_eph_epv * 0.7f && gps.epv < max_eph_epv * 0.7f && gps.fix_type >= 3) { gps_valid = true; reset_est = true; mavlink_log_info(&mavlink_log_pub, "[inav] GPS signal found"); warnx("[inav] GPS signal found"); } } if (gps_valid) { double lat = gps.lat * 1e-7; double lon = gps.lon * 1e-7; float alt = gps.alt * 1e-3; /* initialize reference position if needed */ if (!ref_inited) { if (ref_init_start == 0) { ref_init_start = t; } else if (t > ref_init_start + ref_init_delay) { ref_inited = true; /* set position estimate to (0, 0, 0), use GPS velocity for XY */ x_est[0] = 0.0f; x_est[1] = gps.vel_n_m_s; y_est[0] = 0.0f; y_est[1] = gps.vel_e_m_s; local_pos.ref_lat = lat; local_pos.ref_lon = lon; local_pos.ref_alt = alt + z_est[0]; local_pos.ref_timestamp = t; /* initialize projection */ map_projection_init(&ref, lat, lon); // XXX replace this print warnx("init ref: lat=%.7f, lon=%.7f, alt=%8.4f", (double)lat, (double)lon, (double)alt); mavlink_log_info(&mavlink_log_pub, "[inav] init ref: %.7f, %.7f, %8.4f", (double)lat, (double)lon, (double)alt); } } if (ref_inited) { /* project GPS lat lon to plane */ float gps_proj[2]; map_projection_project(&ref, lat, lon, &(gps_proj[0]), &(gps_proj[1])); /* reset position estimate when GPS becomes good */ if (reset_est) { x_est[0] = gps_proj[0]; x_est[1] = gps.vel_n_m_s; y_est[0] = gps_proj[1]; y_est[1] = gps.vel_e_m_s; } /* calculate index of estimated values in buffer */ int est_i = buf_ptr - 1 - min(EST_BUF_SIZE - 1, max(0, (int)(params.delay_gps * 1000000.0f / PUB_INTERVAL))); if (est_i < 0) { est_i += EST_BUF_SIZE; } /* calculate correction for position */ corr_gps[0][0] = gps_proj[0] - est_buf[est_i][0][0]; corr_gps[1][0] = gps_proj[1] - est_buf[est_i][1][0]; corr_gps[2][0] = local_pos.ref_alt - alt - est_buf[est_i][2][0]; /* calculate correction for velocity */ if (gps.vel_ned_valid) { corr_gps[0][1] = gps.vel_n_m_s - est_buf[est_i][0][1]; corr_gps[1][1] = gps.vel_e_m_s - est_buf[est_i][1][1]; corr_gps[2][1] = gps.vel_d_m_s - est_buf[est_i][2][1]; } else { corr_gps[0][1] = 0.0f; corr_gps[1][1] = 0.0f; corr_gps[2][1] = 0.0f; } /* save rotation matrix at this moment */ memcpy(R_gps, R_buf[est_i], sizeof(R_gps)); w_gps_xy = min_eph_epv / fmaxf(min_eph_epv, gps.eph); w_gps_z = min_eph_epv / fmaxf(min_eph_epv, gps.epv); } } else { /* no GPS lock */ memset(corr_gps, 0, sizeof(corr_gps)); ref_init_start = 0; } gps_updates++; } } /* check for timeout on FLOW topic */ if ((flow_valid || lidar_valid) && t > (flow_time + flow_topic_timeout)) { flow_valid = false; warnx("FLOW timeout"); mavlink_log_info(&mavlink_log_pub, "[inav] FLOW timeout"); } /* check for timeout on GPS topic */ if (gps_valid && (t > (gps.timestamp_position + gps_topic_timeout))) { gps_valid = false; warnx("GPS timeout"); mavlink_log_info(&mavlink_log_pub, "[inav] GPS timeout"); } /* check for timeout on vision topic */ if (vision_valid && (t > (vision.timestamp_boot + vision_topic_timeout))) { vision_valid = false; warnx("VISION timeout"); mavlink_log_info(&mavlink_log_pub, "[inav] VISION timeout"); } /* check for timeout on mocap topic */ if (mocap_valid && (t > (mocap.timestamp_boot + mocap_topic_timeout))) { mocap_valid = false; warnx("MOCAP timeout"); mavlink_log_info(&mavlink_log_pub, "[inav] MOCAP timeout"); } /* check for lidar measurement timeout */ if (lidar_valid && (t > (lidar_time + lidar_timeout))) { lidar_valid = false; warnx("LIDAR timeout"); mavlink_log_info(&mavlink_log_pub, "[inav] LIDAR timeout"); } float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f; dt = fmaxf(fminf(0.02, dt), 0.0002); // constrain dt from 0.2 to 20 ms t_prev = t; /* increase EPH/EPV on each step */ if (eph < 0.000001f) { //get case where eph is 0 -> would stay 0 eph = 0.001; } if (eph < max_eph_epv) { eph *= 1.0f + dt; } if (epv < 0.000001f) { //get case where epv is 0 -> would stay 0 epv = 0.001; } if (epv < max_eph_epv) { epv += 0.005f * dt; // add 1m to EPV each 200s (baro drift) } /* use GPS if it's valid and reference position initialized */ //bool use_gps_xy = ref_inited && gps_valid && params.w_xy_gps_p > MIN_VALID_W; //bool use_gps_z = ref_inited && gps_valid && params.w_z_gps_p > MIN_VALID_W; bool use_gps_xy = false; bool use_gps_z = false; /* use VISION if it's valid and has a valid weight parameter */ bool use_vision_xy = vision_valid && params.w_xy_vision_p > MIN_VALID_W; bool use_vision_z = vision_valid && params.w_z_vision_p > MIN_VALID_W; /* use MOCAP if it's valid and has a valid weight parameter */ bool use_mocap = mocap_valid && params.w_mocap_p > MIN_VALID_W && params.att_ext_hdg_m == mocap_heading; //check if external heading is mocap if (params.disable_mocap) { //disable mocap if fake gps is used use_mocap = false; } /* use flow if it's valid and (accurate or no GPS available) */ bool use_flow = flow_valid && (flow_accurate || !use_gps_xy); /* use LIDAR if it's valid and lidar altitude estimation is enabled */ use_lidar = lidar_valid && params.enable_lidar_alt_est; bool can_estimate_xy = (eph < max_eph_epv) || use_gps_xy || use_flow || use_vision_xy || use_mocap; bool dist_bottom_valid = (t < lidar_valid_time + lidar_valid_timeout); float w_xy_gps_p = params.w_xy_gps_p * w_gps_xy; float w_xy_gps_v = params.w_xy_gps_v * w_gps_xy; float w_z_gps_p = params.w_z_gps_p * w_gps_z; float w_z_gps_v = params.w_z_gps_v * w_gps_z; float w_xy_vision_p = params.w_xy_vision_p; float w_xy_vision_v = params.w_xy_vision_v; float w_z_vision_p = params.w_z_vision_p; float w_mocap_p = params.w_mocap_p; /* reduce GPS weight if optical flow is good */ if (use_flow && flow_accurate) { w_xy_gps_p *= params.w_gps_flow; w_xy_gps_v *= params.w_gps_flow; } /* baro offset correction */ if (use_gps_z) { float offs_corr = corr_gps[2][0] * w_z_gps_p * dt; baro_offset += offs_corr; corr_baro += offs_corr; } /* accelerometer bias correction for GPS (use buffered rotation matrix) */ float accel_bias_corr[3] = { 0.0f, 0.0f, 0.0f }; if (use_gps_xy) { accel_bias_corr[0] -= corr_gps[0][0] * w_xy_gps_p * w_xy_gps_p; accel_bias_corr[0] -= corr_gps[0][1] * w_xy_gps_v; accel_bias_corr[1] -= corr_gps[1][0] * w_xy_gps_p * w_xy_gps_p; accel_bias_corr[1] -= corr_gps[1][1] * w_xy_gps_v; } if (use_gps_z) { accel_bias_corr[2] -= corr_gps[2][0] * w_z_gps_p * w_z_gps_p; accel_bias_corr[2] -= corr_gps[2][1] * w_z_gps_v; } /* transform error vector from NED frame to body frame */ for (int i = 0; i < 3; i++) { float c = 0.0f; for (int j = 0; j < 3; j++) { c += R_gps[j][i] * accel_bias_corr[j]; } if (PX4_ISFINITE(c)) { acc_bias[i] += c * params.w_acc_bias * dt; } } /* accelerometer bias correction for VISION (use buffered rotation matrix) */ accel_bias_corr[0] = 0.0f; accel_bias_corr[1] = 0.0f; accel_bias_corr[2] = 0.0f; if (use_vision_xy) { accel_bias_corr[0] -= corr_vision[0][0] * w_xy_vision_p * w_xy_vision_p; accel_bias_corr[0] -= corr_vision[0][1] * w_xy_vision_v; accel_bias_corr[1] -= corr_vision[1][0] * w_xy_vision_p * w_xy_vision_p; accel_bias_corr[1] -= corr_vision[1][1] * w_xy_vision_v; } if (use_vision_z) { accel_bias_corr[2] -= corr_vision[2][0] * w_z_vision_p * w_z_vision_p; } /* accelerometer bias correction for MOCAP (use buffered rotation matrix) */ accel_bias_corr[0] = 0.0f; accel_bias_corr[1] = 0.0f; accel_bias_corr[2] = 0.0f; if (use_mocap) { accel_bias_corr[0] -= corr_mocap[0][0] * w_mocap_p * w_mocap_p; accel_bias_corr[1] -= corr_mocap[1][0] * w_mocap_p * w_mocap_p; accel_bias_corr[2] -= corr_mocap[2][0] * w_mocap_p * w_mocap_p; } /* transform error vector from NED frame to body frame */ for (int i = 0; i < 3; i++) { float c = 0.0f; for (int j = 0; j < 3; j++) { c += PX4_R(att.R, j, i) * accel_bias_corr[j]; } if (PX4_ISFINITE(c)) { acc_bias[i] += c * params.w_acc_bias * dt; } } /* accelerometer bias correction for flow and baro (assume that there is no delay) */ accel_bias_corr[0] = 0.0f; accel_bias_corr[1] = 0.0f; accel_bias_corr[2] = 0.0f; if (use_flow) { accel_bias_corr[0] -= corr_flow[0] * params.w_xy_flow; accel_bias_corr[1] -= corr_flow[1] * params.w_xy_flow; } if (use_lidar) { accel_bias_corr[2] -= corr_lidar * params.w_z_lidar * params.w_z_lidar; } else { accel_bias_corr[2] -= corr_baro * params.w_z_baro * params.w_z_baro; } /* transform error vector from NED frame to body frame */ for (int i = 0; i < 3; i++) { float c = 0.0f; for (int j = 0; j < 3; j++) { c += PX4_R(att.R, j, i) * accel_bias_corr[j]; } if (PX4_ISFINITE(c)) { acc_bias[i] += c * params.w_acc_bias * dt; } } /* inertial filter prediction for altitude */ inertial_filter_predict(dt, z_est, acc[2]); if (!(PX4_ISFINITE(z_est[0]) && PX4_ISFINITE(z_est[1]))) { write_debug_log("BAD ESTIMATE AFTER Z PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p, corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v); memcpy(z_est, z_est_prev, sizeof(z_est)); } /* inertial filter correction for altitude */ if (use_lidar) { inertial_filter_correct(corr_lidar, dt, z_est, 0, params.w_z_lidar); } else { inertial_filter_correct(corr_baro, dt, z_est, 0, params.w_z_baro); } if (use_gps_z) { epv = fminf(epv, gps.epv); inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p); inertial_filter_correct(corr_gps[2][1], dt, z_est, 1, w_z_gps_v); } if (use_vision_z) { epv = fminf(epv, epv_vision); inertial_filter_correct(corr_vision[2][0], dt, z_est, 0, w_z_vision_p); } if (use_mocap) { epv = fminf(epv, epv_mocap); inertial_filter_correct(corr_mocap[2][0], dt, z_est, 0, w_mocap_p); } if (!(PX4_ISFINITE(z_est[0]) && PX4_ISFINITE(z_est[1]))) { write_debug_log("BAD ESTIMATE AFTER Z CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p, corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v); memcpy(z_est, z_est_prev, sizeof(z_est)); memset(corr_gps, 0, sizeof(corr_gps)); memset(corr_vision, 0, sizeof(corr_vision)); memset(corr_mocap, 0, sizeof(corr_mocap)); corr_baro = 0; } else { memcpy(z_est_prev, z_est, sizeof(z_est)); } if (can_estimate_xy) { /* inertial filter prediction for position */ inertial_filter_predict(dt, x_est, acc[0]); inertial_filter_predict(dt, y_est, acc[1]); if (!(PX4_ISFINITE(x_est[0]) && PX4_ISFINITE(x_est[1]) && PX4_ISFINITE(y_est[0]) && PX4_ISFINITE(y_est[1]))) { write_debug_log("BAD ESTIMATE AFTER PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p, corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v); memcpy(x_est, x_est_prev, sizeof(x_est)); memcpy(y_est, y_est_prev, sizeof(y_est)); } /* inertial filter correction for position */ if (use_flow) { eph = fminf(eph, eph_flow); inertial_filter_correct(corr_flow[0], dt, x_est, 1, params.w_xy_flow * w_flow); inertial_filter_correct(corr_flow[1], dt, y_est, 1, params.w_xy_flow * w_flow); } if (use_gps_xy) { eph = fminf(eph, gps.eph); inertial_filter_correct(corr_gps[0][0], dt, x_est, 0, w_xy_gps_p); inertial_filter_correct(corr_gps[1][0], dt, y_est, 0, w_xy_gps_p); if (gps.vel_ned_valid && t < gps.timestamp_velocity + gps_topic_timeout) { inertial_filter_correct(corr_gps[0][1], dt, x_est, 1, w_xy_gps_v); inertial_filter_correct(corr_gps[1][1], dt, y_est, 1, w_xy_gps_v); } } if (use_vision_xy) { eph = fminf(eph, eph_vision); inertial_filter_correct(corr_vision[0][0], dt, x_est, 0, w_xy_vision_p); inertial_filter_correct(corr_vision[1][0], dt, y_est, 0, w_xy_vision_p); if (w_xy_vision_v > MIN_VALID_W) { inertial_filter_correct(corr_vision[0][1], dt, x_est, 1, w_xy_vision_v); inertial_filter_correct(corr_vision[1][1], dt, y_est, 1, w_xy_vision_v); } } if (use_mocap) { eph = fminf(eph, eph_mocap); inertial_filter_correct(corr_mocap[0][0], dt, x_est, 0, w_mocap_p); inertial_filter_correct(corr_mocap[1][0], dt, y_est, 0, w_mocap_p); } if (!(PX4_ISFINITE(x_est[0]) && PX4_ISFINITE(x_est[1]) && PX4_ISFINITE(y_est[0]) && PX4_ISFINITE(y_est[1]))) { write_debug_log("BAD ESTIMATE AFTER CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p, corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v); memcpy(x_est, x_est_prev, sizeof(x_est)); memcpy(y_est, y_est_prev, sizeof(y_est)); memset(corr_gps, 0, sizeof(corr_gps)); memset(corr_vision, 0, sizeof(corr_vision)); memset(corr_mocap, 0, sizeof(corr_mocap)); memset(corr_flow, 0, sizeof(corr_flow)); } else { memcpy(x_est_prev, x_est, sizeof(x_est)); memcpy(y_est_prev, y_est, sizeof(y_est)); } } else { /* gradually reset xy velocity estimates */ inertial_filter_correct(-x_est[1], dt, x_est, 1, params.w_xy_res_v); inertial_filter_correct(-y_est[1], dt, y_est, 1, params.w_xy_res_v); } /* run terrain estimator */ terrain_estimator->predict(dt, &att, &sensor, &lidar); terrain_estimator->measurement_update(hrt_absolute_time(), &gps, &lidar, &att); if (inav_verbose_mode) { /* print updates rate */ if (t > updates_counter_start + updates_counter_len) { float updates_dt = (t - updates_counter_start) * 0.000001f; warnx( "updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s, flow = %.1f/s, vision = %.1f/s, mocap = %.1f/s", (double)(accel_updates / updates_dt), (double)(baro_updates / updates_dt), (double)(gps_updates / updates_dt), (double)(attitude_updates / updates_dt), (double)(flow_updates / updates_dt), (double)(vision_updates / updates_dt), (double)(mocap_updates / updates_dt)); updates_counter_start = t; accel_updates = 0; baro_updates = 0; gps_updates = 0; attitude_updates = 0; flow_updates = 0; vision_updates = 0; mocap_updates = 0; } } if (t > pub_last + PUB_INTERVAL) { pub_last = t; /* push current estimate to buffer */ est_buf[buf_ptr][0][0] = x_est[0]; est_buf[buf_ptr][0][1] = x_est[1]; est_buf[buf_ptr][1][0] = y_est[0]; est_buf[buf_ptr][1][1] = y_est[1]; est_buf[buf_ptr][2][0] = z_est[0]; est_buf[buf_ptr][2][1] = z_est[1]; /* push current rotation matrix to buffer */ memcpy(R_buf[buf_ptr], att.R, sizeof(att.R)); buf_ptr++; if (buf_ptr >= EST_BUF_SIZE) { buf_ptr = 0; } /* publish local position */ local_pos.xy_valid = can_estimate_xy; local_pos.v_xy_valid = can_estimate_xy; //local_pos.xy_global = local_pos.xy_valid && use_gps_xy; //local_pos.z_global = local_pos.z_valid && use_gps_z; local_pos.xy_global = true; local_pos.z_global = true; local_pos.x = x_est[0]; local_pos.vx = x_est[1]; local_pos.y = y_est[0]; local_pos.vy = y_est[1]; local_pos.z = z_est[0]; local_pos.vz = z_est[1]; local_pos.yaw = att.yaw; local_pos.dist_bottom_valid = dist_bottom_valid; local_pos.eph = eph; local_pos.epv = epv; if (local_pos.dist_bottom_valid) { local_pos.dist_bottom = dist_ground; local_pos.dist_bottom_rate = - z_est[1]; } local_pos.timestamp = t; orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos); if (local_pos.xy_global && local_pos.z_global) { /* publish global position */ global_pos.timestamp = t; global_pos.time_utc_usec = gps.time_utc_usec; double est_lat, est_lon; map_projection_reproject(&ref, local_pos.x, local_pos.y, &est_lat, &est_lon); global_pos.lat = est_lat; global_pos.lon = est_lon; global_pos.alt = local_pos.ref_alt - local_pos.z; global_pos.vel_n = local_pos.vx; global_pos.vel_e = local_pos.vy; global_pos.vel_d = local_pos.vz; global_pos.yaw = local_pos.yaw; global_pos.eph = eph; global_pos.epv = epv; if (terrain_estimator->is_valid()) { global_pos.terrain_alt = global_pos.alt - terrain_estimator->get_distance_to_ground(); global_pos.terrain_alt_valid = true; } else { global_pos.terrain_alt_valid = false; } global_pos.pressure_alt = sensor.baro_alt_meter[0]; if (vehicle_global_position_pub == NULL) { vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos); } else { orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos); } } } } warnx("stopped"); mavlink_log_info(&mavlink_log_pub, "[inav] stopped"); thread_running = false; return 0; }