From 253b8f50ce4df336df49b24df84430456ef7aa0a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 17 Jul 2015 20:35:07 +0200 Subject: [PATCH 01/14] Load missions correctly after restart. Fixes #2564 --- src/modules/navigator/mission.cpp | 65 ++++++++++++++++++++++--------- src/modules/navigator/mission.h | 8 +++- 2 files changed, 53 insertions(+), 20 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 4944ebe789..bedfc35d32 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,31 @@ 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; + + warnx("LOADED MISSION STATE FROM STORAGE"); } - } 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 +150,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 +273,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 +655,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 */ + warnx("err: index: %d, max: %d", *mission_index_ptr, (int)mission->count); return false; } @@ -795,3 +799,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; From d8a54f5018d3f282e8e083a637258ece64a69bd5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 17 Jul 2015 20:59:39 +0200 Subject: [PATCH 02/14] Navigator: fix feedback on success / fail --- src/modules/navigator/mission.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index bedfc35d32..b9e6e69439 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -126,8 +126,6 @@ Mission::on_inactive() _offboard_mission.dataman_id = mission_state.dataman_id; _offboard_mission.count = mission_state.count; _current_offboard_mission_index = mission_state.current_seq; - - warnx("LOADED MISSION STATE FROM STORAGE"); } _inited = true; @@ -655,7 +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 */ - warnx("err: index: %d, max: %d", *mission_index_ptr, (int)mission->count); + mavlink_log_critical(_navigator->get_mavlink_fd(), "[wpm] err: index: %d, max: %d", *mission_index_ptr, (int)mission->count); return false; } From d9f0baf4dcde62a143d2a9b111a30cac14a1382a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 17 Jul 2015 21:19:32 +0200 Subject: [PATCH 03/14] GPS driver: Only let the driver publish reports if it really found a GPS receiver --- src/drivers/gps/gps.cpp | 48 ++++++++++++++++++++++++++++------------- 1 file changed, 33 insertions(+), 15 deletions(-) diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 30fbdfd8d2..79719361a6 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 > 0) { - 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 > 0) { + 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) { From ec21a71b369ff609fa74ccb5b71a4d275e9e5068 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 17 Jul 2015 23:41:00 +0200 Subject: [PATCH 04/14] Commander: increase mag cal timeout --- src/modules/commander/mag_calibration.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 36cc0cdd5d..7570b8b0f4 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -223,7 +223,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; From 116bd9a03e007c938263781b305507f035884f16 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 18 Jul 2015 11:36:42 +0200 Subject: [PATCH 05/14] MC pos control: Code style fixes --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 54c7608632..59caa27ef7 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -119,15 +119,15 @@ private: int _control_task; /**< task handle for task */ int _mavlink_fd; /**< mavlink fd */ - int _vehicle_status_sub; /**< vehicle status subscription */ - int _att_sub; /**< vehicle attitude subscription */ + int _vehicle_status_sub; /**< vehicle status subscription */ + int _att_sub; /**< vehicle attitude subscription */ int _att_sp_sub; /**< vehicle attitude setpoint */ int _control_mode_sub; /**< vehicle control mode subscription */ int _params_sub; /**< notification of parameter updates */ int _manual_sub; /**< notification of manual control updates */ int _arming_sub; /**< arming status of outputs */ int _local_pos_sub; /**< vehicle local position */ - int _pos_sp_triplet_sub; /**< position setpoint triplet */ + int _pos_sp_triplet_sub; /**< position setpoint triplet */ int _local_pos_sp_sub; /**< offboard local position setpoint */ int _global_vel_sp_sub; /**< offboard global velocity setpoint */ @@ -135,16 +135,16 @@ private: orb_advert_t _local_pos_sp_pub; /**< vehicle local position setpoint publication */ orb_advert_t _global_vel_sp_pub; /**< vehicle global velocity setpoint publication */ - struct vehicle_status_s _vehicle_status; /**< vehicle status */ + struct vehicle_status_s _vehicle_status; /**< vehicle status */ struct vehicle_attitude_s _att; /**< vehicle attitude */ struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ struct manual_control_setpoint_s _manual; /**< r/c channel data */ - struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */ + struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */ struct actuator_armed_s _arming; /**< actuator arming status */ struct vehicle_local_position_s _local_pos; /**< vehicle local position */ struct position_setpoint_triplet_s _pos_sp_triplet; /**< vehicle global position setpoint triplet */ struct vehicle_local_position_setpoint_s _local_pos_sp; /**< vehicle local position setpoint */ - struct vehicle_global_velocity_setpoint_s _global_vel_sp; /**< vehicle global velocity setpoint */ + struct vehicle_global_velocity_setpoint_s _global_vel_sp; /**< vehicle global velocity setpoint */ struct { @@ -210,7 +210,7 @@ private: /** * Update our local parameter cache. */ - int parameters_update(bool force); + int parameters_update(bool force); /** * Update control outputs From fb8236e6b99eee6f8517798d1fcb6105a7a14ffd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 19 Jul 2015 16:00:45 +0200 Subject: [PATCH 06/14] sdlog: Default to require GPS for time stamping --- src/modules/sdlog2/sdlog2.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 76fd47cc15..bedd854af6 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -156,7 +156,7 @@ PARAM_DEFINE_INT32(SDLOG_EXT, -1); * @max 1 * @group SD Logging */ -PARAM_DEFINE_INT32(SDLOG_GPSTIME, 0); +PARAM_DEFINE_INT32(SDLOG_GPSTIME, 1); #define LOGBUFFER_WRITE_AND_COUNT(_msg) if (logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(_msg))) { \ log_msgs_written++; \ From 06c45aadfbca55d88ff643a1ca526065a1d357e7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 19 Jul 2015 16:03:08 +0200 Subject: [PATCH 07/14] FW attitude control: Increase default integrator gains to compensate common airframe trim issues --- src/modules/fw_att_control/fw_att_control_params.c | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index 234ff0c3fb..aa8787b623 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -84,11 +84,11 @@ PARAM_DEFINE_FLOAT(FW_PR_P, 0.08f); * This gain defines how much control response will result out of a steady * state error. It trims any constant error. * - * @min 0.0 - * @max 50.0 + * @min 0.005 + * @max 0.5 * @group FW Attitude Control */ -PARAM_DEFINE_FLOAT(FW_PR_I, 0.005f); +PARAM_DEFINE_FLOAT(FW_PR_I, 0.01f); /** * Maximum positive / up pitch rate. @@ -157,11 +157,11 @@ PARAM_DEFINE_FLOAT(FW_RR_P, 0.05f); * This gain defines how much control response will result out of a steady * state error. It trims any constant error. * - * @min 0.0 - * @max 100.0 + * @min 0.005 + * @max 0.2 * @group FW Attitude Control */ -PARAM_DEFINE_FLOAT(FW_RR_I, 0.005f); +PARAM_DEFINE_FLOAT(FW_RR_I, 0.01f); /** * Roll Integrator Anti-Windup From 3ccc6823a2e1d9dca4952049878605005f096377 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 19 Jul 2015 16:03:48 +0200 Subject: [PATCH 08/14] ROMFS: Remove integral gains from default configs as they are fine with the default value --- ROMFS/px4fmu_common/init.d/3030_io_camflyer | 2 -- ROMFS/px4fmu_common/init.d/3031_phantom | 2 -- ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha | 2 -- 3 files changed, 6 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/3030_io_camflyer b/ROMFS/px4fmu_common/init.d/3030_io_camflyer index 040b78dc77..9fe5002de9 100644 --- a/ROMFS/px4fmu_common/init.d/3030_io_camflyer +++ b/ROMFS/px4fmu_common/init.d/3030_io_camflyer @@ -17,11 +17,9 @@ then param set FW_LND_TLALT 5 param set FW_THR_LND_MAX 0 param set FW_PR_FF 0.35 - param set FW_PR_I 0.005 param set FW_PR_IMAX 0.4 param set FW_PR_P 0.08 param set FW_RR_FF 0.6 - param set FW_RR_I 0.005 param set FW_RR_IMAX 0.2 param set FW_RR_P 0.04 fi diff --git a/ROMFS/px4fmu_common/init.d/3031_phantom b/ROMFS/px4fmu_common/init.d/3031_phantom index c7dd1dfc5c..43db1ed6de 100644 --- a/ROMFS/px4fmu_common/init.d/3031_phantom +++ b/ROMFS/px4fmu_common/init.d/3031_phantom @@ -16,14 +16,12 @@ then param set FW_L1_DAMPING 0.75 param set FW_L1_PERIOD 15 param set FW_PR_FF 0.2 - param set FW_PR_I 0.005 param set FW_PR_IMAX 0.2 param set FW_PR_P 0.03 param set FW_P_LIM_MAX 50 param set FW_P_LIM_MIN -50 param set FW_P_ROLLFF 1 param set FW_RR_FF 0.5 - param set FW_RR_I 0.02 param set FW_RR_IMAX 0.2 param set FW_RR_P 0.08 param set FW_R_LIM 50 diff --git a/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha b/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha index 8a661f25e2..5fce4ed226 100644 --- a/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha +++ b/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha @@ -22,11 +22,9 @@ then param set FW_LND_TLALT 5 param set FW_THR_LND_MAX 0 param set FW_PR_FF 0.35 - param set FW_PR_I 0.01 param set FW_PR_IMAX 0.4 param set FW_PR_P 0.08 param set FW_RR_FF 0.6 - param set FW_RR_I 0.01 param set FW_RR_IMAX 0.2 param set FW_RR_P 0.04 param set SYS_COMPANION 157600 From d0d46c97138edbc2583866a0da9e12df2ca1cd0b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 20 Jul 2015 10:16:05 +0200 Subject: [PATCH 09/14] MAVLink app: Use better default rates --- src/modules/mavlink/mavlink_main.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 91c9ce7db3..3a2acd07c3 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1440,15 +1440,15 @@ Mavlink::task_main(int argc, char *argv[]) case MAVLINK_MODE_NORMAL: configure_stream("SYS_STATUS", 1.0f); configure_stream("GPS_GLOBAL_ORIGIN", 0.5f); - configure_stream("HIGHRES_IMU", 1.0f); - configure_stream("ATTITUDE", 10.0f); + configure_stream("HIGHRES_IMU", 2.0f); + configure_stream("ATTITUDE", 20.0f); configure_stream("VFR_HUD", 8.0f); configure_stream("GPS_RAW_INT", 1.0f); configure_stream("GLOBAL_POSITION_INT", 3.0f); configure_stream("LOCAL_POSITION_NED", 3.0f); - configure_stream("RC_CHANNELS_RAW", 1.0f); + configure_stream("RC_CHANNELS_RAW", 4.0f); configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f); - configure_stream("ATTITUDE_TARGET", 3.0f); + configure_stream("ATTITUDE_TARGET", 8.0f); configure_stream("DISTANCE_SENSOR", 0.5f); configure_stream("OPTICAL_FLOW_RAD", 5.0f); break; From 006dfbb14f658b23c0a760229bac05e861db48dd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 20 Jul 2015 10:43:12 +0200 Subject: [PATCH 10/14] Commander: Speed up airspeed calibration --- src/modules/commander/airspeed_calibration.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 837a3f1e83..32077d5f3b 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -72,12 +72,12 @@ int do_airspeed_calibration(int mavlink_fd) { int result = OK; unsigned calibration_counter = 0; - const unsigned maxcount = 3000; + const unsigned maxcount = 2400; /* give directions */ mavlink_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name); - const unsigned calibration_count = 2000; + const unsigned calibration_count = (maxcount * 2) / 3; int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); struct differential_pressure_s diff_pres; @@ -201,10 +201,10 @@ int do_airspeed_calibration(int mavlink_fd) /* just take a few samples and make sure pitot tubes are not reversed, timeout after ~30 seconds */ while (calibration_counter < maxcount) { - if (calibrate_cancel_check(mavlink_fd, cancel_sub)) { - goto error_return; - } - + if (calibrate_cancel_check(mavlink_fd, cancel_sub)) { + goto error_return; + } + /* wait blocking for new data */ struct pollfd fds[1]; fds[0].fd = diff_pres_sub; From 6d096401b57b3c0a1c6ae9898c792140d84785f5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 20 Jul 2015 10:44:09 +0200 Subject: [PATCH 11/14] Commander: Turn calibration warnings into critical (voice) output --- src/modules/commander/calibration_routines.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp index 045dbb032d..44ab9054d2 100644 --- a/src/modules/commander/calibration_routines.cpp +++ b/src/modules/commander/calibration_routines.cpp @@ -469,8 +469,8 @@ calibrate_return calibrate_from_orientation(int mavlink_fd, /* inform user about already handled side */ if (side_data_collected[orient]) { orientation_failures++; - mavlink_and_console_log_info(mavlink_fd, "[cal] %s side completed or not needed", detect_orientation_str(orient)); - mavlink_and_console_log_info(mavlink_fd, "[cal] rotate to a pending side"); + mavlink_and_console_log_critical(mavlink_fd, "%s side already completed", detect_orientation_str(orient)); + mavlink_and_console_log_critical(mavlink_fd, "rotate to a pending side"); continue; } From 0d90cf19dd80586934b4710b80ed28b166fe04e6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 20 Jul 2015 10:45:15 +0200 Subject: [PATCH 12/14] Mag calibration: Reduce time required to complete calibration significantly --- src/modules/commander/mag_calibration.cpp | 30 +++++++++++++++++------ 1 file changed, 22 insertions(+), 8 deletions(-) diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 7570b8b0f4..8f8891bc0c 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -64,9 +64,11 @@ static const int ERROR = -1; static const char *sensor_name = "mag"; -static const unsigned max_mags = 3; +static constexpr unsigned max_mags = 3; static constexpr float mag_sphere_radius = 0.2f; -static const unsigned int calibration_sides = 6; +static constexpr unsigned int calibration_sides = 6; ///< The total number of sides +static constexpr unsigned int calibration_total_points = 240; ///< The total points per magnetometer +static constexpr unsigned int calibraton_duration_seconds = 42; ///< The total duration the routine is allowed to take calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]); @@ -204,6 +206,10 @@ static bool reject_sample(float sx, float sy, float sz, float x[], float y[], fl return false; } +static unsigned progress_percentage(mag_worker_data_t* worker_data) { + return 100 * ((float)worker_data->done_count) / calibration_sides; +} + static calibrate_return mag_calibration_worker(detect_orientation_return orientation, int cancel_sub, void* data) { calibrate_return result = calibrate_return_ok; @@ -223,7 +229,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 * 2; + hrt_abstime detection_deadline = hrt_absolute_time() + worker_data->calibration_interval_perside_useconds * 5; hrt_abstime last_gyro = 0; float gyro_x_integral = 0.0f; float gyro_y_integral = 0.0f; @@ -344,8 +350,8 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta // Progress indicator for side mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] %s side calibration: progress <%u>", - detect_orientation_str(orientation), - (unsigned)(100 * ((float)calibration_counter_side / (float)worker_data->calibration_points_perside))); + detect_orientation_str(orientation), progress_percentage(worker_data) + + (unsigned)((100 / calibration_sides) * ((float)calibration_counter_side / (float)worker_data->calibration_points_perside))); } } else { poll_errcount++; @@ -362,7 +368,7 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] %s side done, rotate to a different side", detect_orientation_str(orientation)); worker_data->done_count++; - mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_QGC_PROGRESS_MSG, 34 * worker_data->done_count); + mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_QGC_PROGRESS_MSG, progress_percentage(worker_data)); } return result; @@ -376,8 +382,8 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag worker_data.mavlink_fd = mavlink_fd; worker_data.done_count = 0; - worker_data.calibration_points_perside = 40; - worker_data.calibration_interval_perside_seconds = 20; + worker_data.calibration_points_perside = calibration_total_points / calibration_sides; + worker_data.calibration_interval_perside_seconds = calibraton_duration_seconds / calibration_sides; worker_data.calibration_interval_perside_useconds = worker_data.calibration_interval_perside_seconds * 1000 * 1000; // Collect: Right-side up, Left Side, Nose down @@ -496,6 +502,10 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag printf("RAW DATA:\n--------------------\n"); for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) { + if (worker_data.calibration_counter_total[cur_mag] == 0) { + continue; + } + printf("RAW: MAG %u with %u samples:\n", cur_mag, worker_data.calibration_counter_total[cur_mag]); for (size_t i = 0; i < worker_data.calibration_counter_total[cur_mag]; i++) { @@ -511,6 +521,10 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag printf("CALIBRATED DATA:\n--------------------\n"); for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) { + if (worker_data.calibration_counter_total[cur_mag] == 0) { + continue; + } + printf("Calibrated: MAG %u with %u samples:\n", cur_mag, worker_data.calibration_counter_total[cur_mag]); for (size_t i = 0; i < worker_data.calibration_counter_total[cur_mag]; i++) { From fbb68443d946fc385e514b8cbd93896b45b4b4c0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 20 Jul 2015 10:50:23 +0200 Subject: [PATCH 13/14] F330 config: Allow the use of PWM_ params --- ROMFS/px4fmu_common/init.d/4010_dji_f330 | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/4010_dji_f330 b/ROMFS/px4fmu_common/init.d/4010_dji_f330 index 512ad132be..f297e270a0 100644 --- a/ROMFS/px4fmu_common/init.d/4010_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/4010_dji_f330 @@ -21,7 +21,11 @@ then param set MC_YAWRATE_P 0.2 param set MC_YAWRATE_I 0.1 param set MC_YAWRATE_D 0.0 + param set PWM_DISARMED 900 + param set PWM_MIN 1230 + param set PWM_MAX 1950 fi -set PWM_MIN 1200 -set PWM_MAX 1950 +set PWM_DISARMED p:PWM_DISARMED +set PWM_MIN p:PWM_MIN +set PWM_MAX p:PWM_MAX From 08032179fe694221b98b4b50c5efc4b3a6f4ca2b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 20 Jul 2015 10:50:46 +0200 Subject: [PATCH 14/14] F450 config: Allow the use of PWM_ params --- ROMFS/px4fmu_common/init.d/4011_dji_f450 | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/4011_dji_f450 b/ROMFS/px4fmu_common/init.d/4011_dji_f450 index 2a77f13866..3682381a19 100644 --- a/ROMFS/px4fmu_common/init.d/4011_dji_f450 +++ b/ROMFS/px4fmu_common/init.d/4011_dji_f450 @@ -22,7 +22,11 @@ then param set MC_YAWRATE_P 0.3 param set MC_YAWRATE_I 0.1 param set MC_YAWRATE_D 0.0 + param set PWM_DISARMED 900 + param set PWM_MIN 1230 + param set PWM_MAX 1950 fi -set PWM_MIN 1230 -set PWM_MAX 1950 +set PWM_DISARMED p:PWM_DISARMED +set PWM_MIN p:PWM_MIN +set PWM_MAX p:PWM_MAX