diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 7b92942b33..64fd972ba4 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -143,7 +143,9 @@ static constexpr uint8_t COMMANDER_MAX_GPS_NOISE = 60; /**< Maximum percentage #define DIFFPRESS_TIMEOUT 2000000 #define PRINT_INTERVAL 5000000 -#define PRINT_MODE_REJECT_INTERVAL 2000000 +#define PRINT_MODE_REJECT_INTERVAL 10000000 + +#define INAIR_RESTART_HOLDOFF_INTERVAL 2000000 enum MAV_MODE_FLAG { MAV_MODE_FLAG_CUSTOM_MODE_ENABLED = 1, /* 0b00000001 Reserved for future use. | */ @@ -169,6 +171,7 @@ static volatile bool thread_should_exit = false; /**< daemon exit flag */ static volatile bool thread_running = false; /**< daemon status flag */ static int daemon_task; /**< Handle of daemon task / thread */ static bool need_param_autosave = false; /**< Flag set to true if parameters should be autosaved in next iteration (happens on param update and if functionality is enabled) */ +static hrt_abstime commander_boot_timestamp = 0; static unsigned int leds_counter; /* To remember when last notification was sent */ @@ -208,7 +211,7 @@ void usage(const char *reason); */ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos, - orb_advert_t *home_pub); + struct vehicle_local_position_s *local_pos, orb_advert_t *home_pub); /** * Mainloop of commander. @@ -347,6 +350,7 @@ int commander_main(int argc, char *argv[]) if (!strcmp(argv[1], "arm")) { int mavlink_fd_local = open(MAVLINK_LOG_DEVICE, 0); arm_disarm(true, mavlink_fd_local, "command line"); + warnx("note: not updating home position on commandline arming!"); close(mavlink_fd_local); exit(0); } @@ -450,7 +454,8 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char bool handle_command(struct vehicle_status_s *status_local, const struct safety_s *safety_local, struct vehicle_command_s *cmd, struct actuator_armed_s *armed_local, - struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub) + struct home_position_s *home, struct vehicle_global_position_s *global_pos, + struct vehicle_local_position_s *local_pos, orb_advert_t *home_pub) { /* only handle commands that are meant to be handled by this system and component */ if (cmd->target_system != status_local->system_id || ((cmd->target_component != status_local->component_id) @@ -478,6 +483,11 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s // Transition the arming state arming_ret = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command"); + /* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */ + if (arming_ret == TRANSITION_CHANGED && hrt_absolute_time() > commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL) { + commander_set_home_position(*home_pub, *home, *local_pos, *global_pos); + } + if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) { /* use autopilot-specific mode */ if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) { @@ -1161,7 +1171,7 @@ int commander_thread_main(int argc, char *argv[]) set_tune_override(TONE_STARTUP_TUNE); //normal boot tune } - const hrt_abstime commander_boot_timestamp = hrt_absolute_time(); + commander_boot_timestamp = hrt_absolute_time(); transition_result_t arming_ret; @@ -1957,7 +1967,7 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); /* handle it */ - if (handle_command(&status, &safety, &cmd, &armed, &_home, &global_position, &home_pub)) { + if (handle_command(&status, &safety, &cmd, &armed, &_home, &global_position, &local_position, &home_pub)) { status_changed = true; } } @@ -2023,7 +2033,7 @@ int commander_thread_main(int argc, char *argv[]) } /* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */ - else if (arming_state_changed && armed.armed && !was_armed && now > commander_boot_timestamp + 2000000) { + else if (arming_state_changed && armed.armed && !was_armed && now > commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL) { commander_set_home_position(home_pub, _home, local_position, global_position); } diff --git a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h index 4d5e56a961..0dd2b72d92 100644 --- a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h +++ b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h @@ -174,7 +174,7 @@ private: struct map_projection_reference_s _pos_ref; - float _baro_ref_offset; /**< offset between initial baro reference and GPS init baro altitude */ + float _filter_ref_offset; /**< offset between initial baro reference and GPS init baro altitude */ float _baro_gps_offset; /**< offset between baro altitude (at GPS init time) and GPS altitude */ hrt_abstime _last_debug_print = 0; @@ -193,7 +193,6 @@ private: bool _gpsIsGood; ///< True if the current GPS fix is good enough for us to use uint64_t _previousGPSTimestamp; ///< Timestamp of last good GPS fix we have received bool _baro_init; - float _baroAltRef; bool _gps_initialized; hrt_abstime _filter_start_time; hrt_abstime _last_sensor_timestamp; @@ -333,6 +332,12 @@ private: **/ void initializeGPS(); + /** + * Initialize the reference position for the local coordinate frame + */ + void initReferencePosition(hrt_abstime timestamp, + double lat, double lon, float gps_alt, float baro_alt); + /** * @brief * Polls all uORB subscriptions if any new sensor data has been publish and sets the appropriate diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index f3fe048e78..451e14d202 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -153,7 +153,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : _sensor_combined {}, _pos_ref{}, - _baro_ref_offset(0.0f), + _filter_ref_offset(0.0f), _baro_gps_offset(0.0f), /* performance counters */ @@ -173,7 +173,6 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : _gpsIsGood(false), _previousGPSTimestamp(0), _baro_init(false), - _baroAltRef(0.0f), _gps_initialized(false), _filter_start_time(0), _last_sensor_timestamp(0), @@ -404,6 +403,15 @@ int AttitudePositionEstimatorEKF::check_filter_state() // Count the reset condition perf_count(_perf_reset); + // GPS is in scaled integers, convert + double lat = _gps.lat / 1.0e7; + double lon = _gps.lon / 1.0e7; + float gps_alt = _gps.alt / 1e3f; + + // Set up height correctly + orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); + + initReferencePosition(_gps.timestamp_position, lat, lon, gps_alt, _baro.altitude); } else if (_ekf_logging) { _ekf->GetFilterState(&ekf_report); @@ -585,6 +593,7 @@ void AttitudePositionEstimatorEKF::task_main() _baro_init = false; _gps_initialized = false; + _last_sensor_timestamp = hrt_absolute_time(); _last_run = _last_sensor_timestamp; @@ -643,12 +652,13 @@ void AttitudePositionEstimatorEKF::task_main() _ekf->posNE[1] = 0.0f; _local_pos.ref_alt = 0.0f; - _baro_ref_offset = 0.0f; _baro_gps_offset = 0.0f; _baro_alt_filt = _baro.altitude; _ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f); + warnx("filter ref off: baro_alt: %8.4f", (double)_filter_ref_offset); + } else { if (!_gps_initialized && _gpsIsGood) { @@ -702,6 +712,32 @@ void AttitudePositionEstimatorEKF::task_main() _exit(0); } +void AttitudePositionEstimatorEKF::initReferencePosition(hrt_abstime timestamp, + double lat, double lon, float gps_alt, float baro_alt) +{ + // Reference altitude + if (isfinite(_ekf->states[9])) { + _filter_ref_offset = _ekf->states[9]; + } else if (isfinite(-_ekf->hgtMea)) { + _filter_ref_offset = -_ekf->hgtMea; + } else { + _filter_ref_offset = -_baro.altitude; + } + + // init filtered gps and baro altitudes + _gps_alt_filt = gps_alt; + _baro_alt_filt = baro_alt; + + // Initialize projection + _local_pos.ref_lat = lat; + _local_pos.ref_lon = lon; + _local_pos.ref_alt = gps_alt; + _local_pos.ref_timestamp = timestamp; + + 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); +} + void AttitudePositionEstimatorEKF::initializeGPS() { // GPS is in scaled integers, convert @@ -711,11 +747,6 @@ void AttitudePositionEstimatorEKF::initializeGPS() // 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 - - // init filtered gps and baro altitudes - _gps_alt_filt = gps_alt; - _baro_alt_filt = _baro.altitude; _ekf->baroHgt = _baro.altitude; _ekf->hgtMea = _ekf->baroHgt; @@ -737,20 +768,13 @@ void AttitudePositionEstimatorEKF::initializeGPS() _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); + initReferencePosition(_gps.timestamp_position, lat, lon, gps_alt, _baro.altitude); #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); + (double)_filter_ref_offset); warnx("GPS: eph: %8.4f, epv: %8.4f, declination: %8.4f", (double)_gps.eph, (double)_gps.epv, (double)math::degrees(declination)); #endif @@ -811,7 +835,8 @@ void AttitudePositionEstimatorEKF::publishLocalPosition() _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 - _baroAltRef; + _local_pos.z = _ekf->states[9] - _filter_ref_offset; + //_local_pos.z_stable = _ekf->states[9]; _local_pos.vx = _ekf->states[4]; _local_pos.vy = _ekf->states[5]; @@ -826,6 +851,17 @@ void AttitudePositionEstimatorEKF::publishLocalPosition() _local_pos.z_global = false; _local_pos.yaw = _att.yaw; + if (!isfinite(_local_pos.x) || + !isfinite(_local_pos.y) || + !isfinite(_local_pos.z) || + !isfinite(_local_pos.vx) || + !isfinite(_local_pos.vy) || + !isfinite(_local_pos.vz)) + { + // bad data, abort publication + return; + } + /* lazily publish the local position only once available */ if (_local_pos_pub > 0) { /* publish the attitude setpoint */ @@ -859,7 +895,7 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition() } /* local pos alt is negative, change sign and add alt offsets */ - _global_pos.alt = (-_local_pos.z) - _baro_gps_offset; + _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; @@ -874,6 +910,17 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition() _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 */ @@ -1070,8 +1117,9 @@ void AttitudePositionEstimatorEKF::print_status() // 19-21: Body Magnetic Field Vector - gauss (X,Y,Z) printf("dtIMU: %8.6f filt: %8.6f IMUmsec: %d\n", (double)_ekf->dtIMU, (double)_ekf->dtIMUfilt, (int)IMUmsec); - printf("baro alt: %8.4f GPS alt: %8.4f\n", (double)_baro.altitude, (double)(_gps.alt / 1e3f)); - printf("baro ref offset: %8.4f baro GPS offset: %8.4f\n", (double)_baro_ref_offset, + printf("alt RAW: baro alt: %8.4f GPS alt: %8.4f\n", (double)_baro.altitude, (double)_ekf->gpsHgt); + printf("alt EST: local alt: %8.4f (NED), AMSL alt: %8.4f (ENU)\n", (double)(_local_pos.z), (double)_global_pos.alt); + printf("filter ref offset: %8.4f baro GPS offset: %8.4f\n", (double)_filter_ref_offset, (double)_baro_gps_offset); printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z, (double)_ekf->accel.x, (double)_ekf->accel.y, (double)_ekf->accel.z); @@ -1317,7 +1365,11 @@ void AttitudePositionEstimatorEKF::pollData() _ekf->updateDtGpsFilt(math::constrain(dtLastGoodGPS, 0.01f, POS_RESET_THRESHOLD)); // update LPF - _gps_alt_filt += (dtLastGoodGPS / (rc + dtLastGoodGPS)) * (_ekf->gpsHgt - _gps_alt_filt); + float filter_step = (dtLastGoodGPS / (rc + dtLastGoodGPS)) * (_ekf->gpsHgt - _gps_alt_filt); + + if (isfinite(filter_step)) { + _gps_alt_filt += filter_step; + } } //check if we had a GPS outage for a long time @@ -1400,15 +1452,19 @@ void AttitudePositionEstimatorEKF::pollData() } baro_last = _baro.timestamp; - if(!_baro_init) { + if (!_baro_init) { _baro_init = true; - _baroAltRef = _baro.altitude; + _baro_alt_filt = _baro.altitude; } _ekf->updateDtHgtFilt(math::constrain(baro_elapsed, 0.001f, 0.1f)); _ekf->baroHgt = _baro.altitude; - _baro_alt_filt += (baro_elapsed / (rc + baro_elapsed)) * (_baro.altitude - _baro_alt_filt); + float filter_step = (baro_elapsed / (rc + baro_elapsed)) * (_baro.altitude - _baro_alt_filt); + + if (isfinite(filter_step)) { + _baro_alt_filt += filter_step; + } perf_count(_perf_baro); } @@ -1494,30 +1550,34 @@ int AttitudePositionEstimatorEKF::trip_nan() float nan_val = 0.0f / 0.0f; - warnx("system not armed, tripping state vector with NaN values"); + warnx("system not armed, tripping state vector with NaN"); _ekf->states[5] = nan_val; usleep(100000); - warnx("tripping covariance #1 with NaN values"); + warnx("tripping covariance #1 with NaN"); _ekf->KH[2][2] = nan_val; // intermediate result used for covariance updates usleep(100000); - warnx("tripping covariance #2 with NaN values"); + warnx("tripping covariance #2 with NaN"); _ekf->KHP[5][5] = nan_val; // intermediate result used for covariance updates usleep(100000); - warnx("tripping covariance #3 with NaN values"); + warnx("tripping covariance #3 with NaN"); _ekf->P[3][3] = nan_val; // covariance matrix usleep(100000); - warnx("tripping Kalman gains with NaN values"); + warnx("tripping Kalman gains with NaN"); _ekf->Kfusion[0] = nan_val; // Kalman gains usleep(100000); - warnx("tripping stored states[0] with NaN values"); + warnx("tripping stored states[0] with NaN"); _ekf->storedStates[0][0] = nan_val; usleep(100000); + warnx("tripping states[9] with NaN"); + _ekf->states[9] = nan_val; + usleep(100000); + warnx("\nDONE - FILTER STATE:"); print_status(); } diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 1b4d7a2e15..350706f263 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -900,6 +900,8 @@ FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> &c void FixedwingPositionControl::navigation_capabilities_publish() { + _nav_capabilities.timestamp = hrt_absolute_time(); + if (_nav_capabilities_pub > 0) { orb_publish(ORB_ID(navigation_capabilities), _nav_capabilities_pub, &_nav_capabilities); } else { @@ -1674,7 +1676,8 @@ FixedwingPositionControl::task_main() float turn_distance = _l1_control.switch_distance(100.0f); /* lazily publish navigation capabilities */ - if (fabsf(turn_distance - _nav_capabilities.turn_distance) > FLT_EPSILON && turn_distance > 0) { + if ((hrt_elapsed_time(&_nav_capabilities.timestamp) > 1000000) || (fabsf(turn_distance - _nav_capabilities.turn_distance) > FLT_EPSILON + && turn_distance > 0)) { /* set new turn distance */ _nav_capabilities.turn_distance = turn_distance; diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index e36e3fa386..98b01c7578 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -586,7 +586,7 @@ Mission::altitude_sp_foh_update() } /* Do not try to find a solution if the last waypoint is inside the acceptance radius of the current one */ - if (_distance_current_previous - _mission_item.acceptance_radius < 0.0f) { + if (_distance_current_previous - _navigator->get_acceptance_radius(_mission_item.acceptance_radius) < 0.0f) { return; } @@ -608,7 +608,7 @@ Mission::altitude_sp_foh_update() /* if the minimal distance is smaller then the acceptance radius, we should be at waypoint alt * navigator will soon switch to the next waypoint item (if there is one) as soon as we reach this altitude */ - if (_min_current_sp_distance_xy < _mission_item.acceptance_radius) { + if (_min_current_sp_distance_xy < _navigator->get_acceptance_radius(_mission_item.acceptance_radius)) { pos_sp_triplet->current.alt = _mission_item.altitude; } else { /* update the altitude sp of the 'current' item in the sp triplet, but do not update the altitude sp @@ -617,7 +617,7 @@ Mission::altitude_sp_foh_update() * radius around the current waypoint **/ float delta_alt = (_mission_item.altitude - _mission_item_previous_alt); - float grad = -delta_alt/(_distance_current_previous - _mission_item.acceptance_radius); + float grad = -delta_alt/(_distance_current_previous - _navigator->get_acceptance_radius(_mission_item.acceptance_radius)); float a = _mission_item_previous_alt - grad * _distance_current_previous; pos_sp_triplet->current.alt = a + grad * _min_current_sp_distance_xy; diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index dce7d45171..42c74428ad 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -123,12 +123,12 @@ MissionBlock::is_mission_item_reached() * Therefore the item is marked as reached once the system reaches the loiter * radius (+ some margin). Time inside and turn count is handled elsewhere. */ - if (dist >= 0.0f && dist <= _mission_item.loiter_radius * 1.2f) { + if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(_mission_item.loiter_radius * 1.2f)) { _waypoint_position_reached = true; } } else { /* for normal mission items used their acceptance radius */ - if (dist >= 0.0f && dist <= _mission_item.acceptance_radius) { + if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(_mission_item.acceptance_radius)) { _waypoint_position_reached = true; } } diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index d2acce7899..95e3f9f964 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -144,7 +144,22 @@ public: Geofence& get_geofence() { return _geofence; } bool get_can_loiter_at_sp() { return _can_loiter_at_sp; } float get_loiter_radius() { return _param_loiter_radius.get(); } - float get_acceptance_radius() { return _param_acceptance_radius.get(); } + + /** + * Get the acceptance radius + * + * @return the distance at which the next waypoint should be used + */ + float get_acceptance_radius(); + + /** + * Get the acceptance radius given the mission item preset radius + * + * @param mission_item_radius the radius to use in case the controller-derived radius is smaller + * + * @return the distance at which the next waypoint should be used + */ + float get_acceptance_radius(float mission_item_radius); int get_mavlink_fd() { return _mavlink_fd; } private: diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 295172ebb2..97baa1235f 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -571,6 +571,26 @@ Navigator::publish_position_setpoint_triplet() } } +float +Navigator::get_acceptance_radius() +{ + return get_acceptance_radius(_param_acceptance_radius.get()); +} + +float +Navigator::get_acceptance_radius(float mission_item_radius) +{ + float radius = mission_item_radius; + + if (hrt_elapsed_time(&_nav_caps.timestamp) < 5000000) { + if (_nav_caps.turn_distance > radius) { + radius = _nav_caps.turn_distance; + } + } + + return radius; +} + void Navigator::add_fence_point(int argc, char *argv[]) { _geofence.addPoint(argc, argv);