diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 756287dd81..2158af5bfc 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -310,7 +310,7 @@ GPS::task_main() _report_gps_pos.cog_rad = 0.0f; _report_gps_pos.vel_ned_valid = true; - //no time and satellite information simulated + /* no time and satellite information simulated */ if (!(_pub_blocked)) { @@ -351,28 +351,46 @@ GPS::task_main() unlock(); + /* the Ashtech driver lies about successful configuration and the + * MTK driver is not well tested, so we really only trust the UBX + * driver for an advance publication + */ if (_Helper->configure(_baudrate) == 0) { unlock(); - //Publish initial report that we have access to a GPS - //Make sure to clear any stale data in case driver is reset + /* reset report */ memset(&_report_gps_pos, 0, sizeof(_report_gps_pos)); - _report_gps_pos.timestamp_position = hrt_absolute_time(); - _report_gps_pos.timestamp_variance = hrt_absolute_time(); - _report_gps_pos.timestamp_velocity = hrt_absolute_time(); - _report_gps_pos.timestamp_time = hrt_absolute_time(); - if (!(_pub_blocked)) { - if (_report_gps_pos_pub != nullptr) { - orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); + if (_mode == GPS_DRIVER_MODE_UBX) { + /* Publish initial report that we have access to a GPS, + * but set all critical state fields to indicate we have + * no valid position lock + */ - } else { - _report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos); + _report_gps_pos.timestamp_time = hrt_absolute_time(); + + /* reset the timestamps for data, because we have no data yet */ + _report_gps_pos.timestamp_position = 0; + _report_gps_pos.timestamp_variance = 0; + _report_gps_pos.timestamp_velocity = 0; + + /* set a massive variance */ + _report_gps_pos.eph = 10000.0f; + _report_gps_pos.epv = 10000.0f; + _report_gps_pos.fix_type = 0; + + if (!(_pub_blocked)) { + if (_report_gps_pos_pub != nullptr) { + orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); + + } else { + _report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos); + } } - } - // GPS is obviously detected successfully, reset statistics - _Helper->reset_update_rates(); + /* GPS is obviously detected successfully, reset statistics */ + _Helper->reset_update_rates(); + } int helper_ret; while ((helper_ret = _Helper->receive(TIMEOUT_5HZ)) > 0 && !_task_should_exit) { diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 1a6977eb1e..c5dab56151 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -226,7 +226,7 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta * for a good result, so we're not constraining the user more than we have to. */ - hrt_abstime detection_deadline = hrt_absolute_time() + worker_data->calibration_interval_perside_useconds; + hrt_abstime detection_deadline = hrt_absolute_time() + worker_data->calibration_interval_perside_useconds * 2; hrt_abstime last_gyro = 0; float gyro_x_integral = 0.0f; float gyro_y_integral = 0.0f; diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index d94978147b..e95b22b7e4 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1606,7 +1606,7 @@ Mavlink::task_main(int argc, char *argv[]) case MAVLINK_MODE_ONBOARD: configure_stream("SYS_STATUS", 1.0f); - configure_stream("ATTITUDE", 50.0f); + configure_stream("ATTITUDE", 250.0f); configure_stream("HIGHRES_IMU", 50.0f); configure_stream("GPS_RAW_INT", 5.0f); configure_stream("GLOBAL_POSITION_INT", 50.0f); diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index ea515e62ad..de34797aa3 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -70,8 +70,8 @@ Mission::Mission(Navigator *navigator, const char *name) : _param_dist_1wp(this, "MIS_DIST_1WP", false), _param_altmode(this, "MIS_ALTMODE", false), _param_yawmode(this, "MIS_YAWMODE", false), - _onboard_mission({0}), - _offboard_mission({0}), + _onboard_mission{}, + _offboard_mission{}, _current_onboard_mission_index(-1), _current_offboard_mission_index(-1), _need_takeoff(true), @@ -110,32 +110,29 @@ Mission::on_inactive() update_offboard_mission(); } - /* check if the home position became valid in the meantime */ - if ((_mission_type == MISSION_TYPE_NONE || _mission_type == MISSION_TYPE_OFFBOARD) && - !_home_inited && _navigator->home_position_valid()) { + } else { - dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id); + /* load missions from storage */ + mission_s mission_state; - _navigator->get_mission_result()->valid = _missionFeasiblityChecker.checkMissionFeasible(_navigator->get_mavlink_fd(), _navigator->get_vstatus()->is_rotary_wing, - dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(), - _navigator->get_home_position()->alt, _navigator->home_position_valid(), - _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, - _param_dist_1wp.get(), _navigator->get_mission_result()->warning); + dm_lock(DM_KEY_MISSION_STATE); - _navigator->increment_mission_instance_count(); - _navigator->set_mission_result_updated(); + /* read current state */ + int read_res = dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s)); - _home_inited = true; + dm_unlock(DM_KEY_MISSION_STATE); + + if (read_res == sizeof(mission_s)) { + _offboard_mission.dataman_id = mission_state.dataman_id; + _offboard_mission.count = mission_state.count; + _current_offboard_mission_index = mission_state.current_seq; } - } else { - /* read mission topics on initialization */ _inited = true; - - update_onboard_mission(); - update_offboard_mission(); } + check_mission_valid(); + /* require takeoff after non-loiter or landing */ if (!_navigator->get_can_loiter_at_sp() || _navigator->get_vstatus()->condition_landed) { _need_takeoff = true; @@ -151,6 +148,8 @@ Mission::on_activation() void Mission::on_active() { + check_mission_valid(); + /* check if anything has changed */ bool onboard_updated = false; orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated); @@ -272,6 +271,8 @@ Mission::update_offboard_mission() _offboard_mission.count = 0; _offboard_mission.current_seq = 0; _current_offboard_mission_index = 0; + + warnx("mission check failed"); } set_current_offboard_mission_item(); @@ -652,6 +653,7 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)mission->count) { /* mission item index out of bounds */ + mavlink_log_critical(_navigator->get_mavlink_fd(), "[wpm] err: index: %d, max: %d", *mission_index_ptr, (int)mission->count); return false; } @@ -795,3 +797,26 @@ Mission::set_mission_finished() _navigator->get_mission_result()->finished = true; _navigator->set_mission_result_updated(); } + +bool +Mission::check_mission_valid() +{ + /* check if the home position became valid in the meantime */ + if (!_home_inited && _navigator->home_position_valid()) { + + dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id); + + _navigator->get_mission_result()->valid = _missionFeasiblityChecker.checkMissionFeasible(_navigator->get_mavlink_fd(), _navigator->get_vstatus()->is_rotary_wing, + dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(), + _navigator->get_home_position()->alt, _navigator->home_position_valid(), + _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, + _param_dist_1wp.get(), _navigator->get_mission_result()->warning); + + _navigator->increment_mission_instance_count(); + _navigator->set_mission_result_updated(); + + _home_inited = true; + } + + return _navigator->get_mission_result()->valid; +} diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index d77f461574..f367da1c7d 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -39,6 +39,7 @@ * @author Thomas Gubler * @author Anton Babushkin * @author Ban Siesta + * @author Lorenz Meier */ #ifndef NAVIGATOR_MISSION_H @@ -165,6 +166,11 @@ private: */ void set_mission_finished(); + /** + * Check wether a mission is ready to go + */ + bool check_mission_valid(); + control::BlockParamInt _param_onboard_enabled; control::BlockParamFloat _param_takeoff_alt; control::BlockParamFloat _param_dist_1wp;