From 9ea62e74025316071651898037a400cdac564181 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 20 May 2015 21:01:38 +0200 Subject: [PATCH 1/6] mavlink: Enable a few helpful streams on companion link --- src/modules/mavlink/mavlink_main.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index e5cddfafb6..b6050fa56b 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1406,6 +1406,9 @@ Mavlink::task_main(int argc, char *argv[]) case MAVLINK_MODE_ONBOARD: configure_stream("SYS_STATUS", 1.0f); configure_stream("ATTITUDE", 50.0f); + configure_stream("HIGHRES_IMU", 50.0f); + configure_stream("VFR_HUD", 5.0f); + configure_stream("GPS_RAW_INT", 5.0f); configure_stream("GLOBAL_POSITION_INT", 50.0f); configure_stream("LOCAL_POSITION_NED", 30.0f); configure_stream("CAMERA_CAPTURE", 2.0f); @@ -1414,6 +1417,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("POSITION_TARGET_LOCAL_NED", 10.0f); configure_stream("DISTANCE_SENSOR", 10.0f); configure_stream("OPTICAL_FLOW_RAD", 10.0f); + configure_stream("RC_CHANNELS_RAW", 20.0f); configure_stream("VFR_HUD", 10.0f); configure_stream("SYSTEM_TIME", 1.0f); configure_stream("TIMESYNC", 10.0f); From bc75814d500c673fa8699f8d242c88e610ecded2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 20 May 2015 21:02:28 +0200 Subject: [PATCH 2/6] Increase buffer sizes on companion link --- nuttx-configs/px4fmu-v2/nsh/defconfig | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index ab8f9eef62..19e0f7c632 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -560,8 +560,8 @@ CONFIG_USART1_2STOP=0 # # USART2 Configuration # -CONFIG_USART2_RXBUFSIZE=512 -CONFIG_USART2_TXBUFSIZE=512 +CONFIG_USART2_RXBUFSIZE=600 +CONFIG_USART2_TXBUFSIZE=2200 CONFIG_USART2_BAUD=57600 CONFIG_USART2_BITS=8 CONFIG_USART2_PARITY=0 From fb4dc27bc9c604e71733b38ea06045c041cd2bc6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 21 May 2015 10:26:36 +0200 Subject: [PATCH 3/6] commander: Improve user feedback on sensor health, in particular during calibration --- src/modules/commander/state_machine_helper.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index cdcc12043e..2d631305dd 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -219,6 +219,7 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s // Sensors need to be initialized for STANDBY state, except for HIL if ((status->hil_state != vehicle_status_s::HIL_STATE_ON) && (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) && + (status->arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR) && (!status->condition_system_sensors_initialized)) { mavlink_and_console_log_critical(mavlink_fd, "Not ready to fly: Sensors need inspection"); feedback_provided = true; @@ -232,7 +233,7 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s if (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) { if (status->condition_system_sensors_initialized) { - mavlink_and_console_log_critical(mavlink_fd, "Preflight check now OK, power cycle before arming"); + mavlink_and_console_log_critical(mavlink_fd, "Preflight check resolved, reboot before arming"); } else { mavlink_and_console_log_critical(mavlink_fd, "Preflight check failed, refusing to arm"); } @@ -240,7 +241,7 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s } else if ((new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) && status->condition_system_sensors_initialized) { - mavlink_and_console_log_critical(mavlink_fd, "Preflight check resolved, power cycle to complete"); + mavlink_and_console_log_critical(mavlink_fd, "Preflight check resolved, reboot to complete"); feedback_provided = true; } else { From 9179fcefc9ce138f8fdbbe9e194ca11ecb116b33 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 21 May 2015 17:25:37 +0200 Subject: [PATCH 4/6] Calibration state machine fixes, generates less bogus error messages during calibration --- msg/vehicle_status.msg | 1 + src/modules/commander/commander.cpp | 10 ++++--- .../commander/state_machine_helper.cpp | 26 ++++++++++--------- 3 files changed, 22 insertions(+), 15 deletions(-) diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index 430f7dd768..c5d5ee9a16 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -91,6 +91,7 @@ uint8 nav_state # set navigation state machine to specified value uint8 arming_state # current arming state uint8 hil_state # current hil state bool failsafe # true if system is in failsafe state +bool calibration_enabled # true if current calibrating parts of the system. Also sets the system to ARMING_STATE_INIT. int32 system_type # system type, inspired by MAVLink's VEHICLE_TYPE enum int32 system_id # system id, inspired by MAVLink's system ID field diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 7e29dbd94d..3e9b1de6f7 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1604,7 +1604,7 @@ int commander_thread_main(int argc, char *argv[]) /* End battery voltage check */ /* If in INIT state, try to proceed to STANDBY state */ - if (status.arming_state == vehicle_status_s::ARMING_STATE_INIT) { + if (!status.calibration_enabled && status.arming_state == vehicle_status_s::ARMING_STATE_INIT) { arming_ret = arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd); @@ -2689,6 +2689,8 @@ void *commander_low_prio_loop(void *arg) false /* fRunPreArmChecks */, mavlink_fd)) { answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); break; + } else { + status.calibration_enabled = true; } if ((int)(cmd.param1) == 1) { @@ -2748,12 +2750,14 @@ void *commander_low_prio_loop(void *arg) /* enable RC control input */ status.rc_input_blocked = false; mavlink_log_info(mavlink_fd, "CAL: Re-enabling RC IN"); - calib_ret = OK; + calib_ret = OK; } /* this always succeeds */ calib_ret = OK; } + status.calibration_enabled = false; + if (calib_ret == OK) { tune_positive(true); @@ -2771,7 +2775,7 @@ void *commander_low_prio_loop(void *arg) status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true, !status.circuit_breaker_engaged_gpsfailure_check); - arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd); + arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, false /* fRunPreArmChecks */, mavlink_fd); } else { tune_negative(true); diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 2d631305dd..b53eeebeb6 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -216,17 +216,6 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s valid_transition = true; } - // Sensors need to be initialized for STANDBY state, except for HIL - if ((status->hil_state != vehicle_status_s::HIL_STATE_ON) && - (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) && - (status->arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR) && - (!status->condition_system_sensors_initialized)) { - mavlink_and_console_log_critical(mavlink_fd, "Not ready to fly: Sensors need inspection"); - feedback_provided = true; - valid_transition = false; - status->arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR; - } - // Check if we are trying to arm, checks look good but we are in STANDBY_ERROR if (status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) { @@ -244,8 +233,21 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s mavlink_and_console_log_critical(mavlink_fd, "Preflight check resolved, reboot to complete"); feedback_provided = true; } else { - + // Silent ignore + feedback_provided = true; } + + // Sensors need to be initialized for STANDBY state, except for HIL + } else if ((status->hil_state != vehicle_status_s::HIL_STATE_ON) && + (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) && + (status->arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR) && + (!status->condition_system_sensors_initialized)) { + if (!fRunPreArmChecks) { + mavlink_and_console_log_critical(mavlink_fd, "Not ready to fly: Sensors need inspection"); + } + feedback_provided = true; + valid_transition = false; + status->arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR; } // Finish up the state transition From afa8266255eeec47dbc6617aa172723121b1da63 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Thu, 21 May 2015 15:30:22 +0200 Subject: [PATCH 5/6] do not update mission if it has unsupported mission items --- src/modules/navigator/mission.cpp | 7 +++- .../navigator/mission_feasibility_checker.cpp | 42 ++++++++++++++++++- .../navigator/mission_feasibility_checker.h | 1 + 3 files changed, 47 insertions(+), 3 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index c1763ba93d..a68f4de012 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -207,6 +207,8 @@ Mission::update_onboard_mission() void Mission::update_offboard_mission() { + bool failed = true; + if (orb_copy(ORB_ID(offboard_mission), _navigator->get_offboard_mission_sub(), &_offboard_mission) == OK) { warnx("offboard mission updated: dataman_id=%d, count=%d, current_seq=%d", _offboard_mission.dataman_id, _offboard_mission.count, _offboard_mission.current_seq); /* determine current index */ @@ -228,12 +230,15 @@ Mission::update_offboard_mission() * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */ dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id); - _missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing, + failed = !_missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing, dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(), _navigator->get_home_position()->alt); } else { warnx("offboard mission update failed"); + } + + if (failed) { _offboard_mission.count = 0; _offboard_mission.current_seq = 0; _current_offboard_mission_index = 0; diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 949231b159..8f1d6f8e85 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -65,6 +65,7 @@ MissionFeasibilityChecker::MissionFeasibilityChecker() : _mavlink_fd(-1), _capab bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt) { + bool failed = false; /* Init if not done yet */ init(); @@ -74,11 +75,16 @@ bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_ _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); } + // check if all mission item commands are supported + failed |= !checkMissionItemValidity(dm_current, nMissionItems); + if (isRotarywing) - return checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt); + failed |= !checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt); else - return checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt); + failed |= !checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt); + + return !failed; } bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt) @@ -163,6 +169,38 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, return true; } +bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, size_t nMissionItems) { + // do not allow mission if we find unsupported item + for (size_t i = 0; i < nMissionItems; i++) { + struct mission_item_s missionitem; + const ssize_t len = sizeof(struct mission_item_s); + + if (dm_read(dm_current, i, &missionitem, len) != len) { + // not supposed to happen unless the datamanager can't access the SD card, etc. + mavlink_log_critical(_mavlink_fd, "Rejecting Mission: Cannot access mission from SD card"); + return false; + } + + // check if we find unsupported item and reject mission if so + if (missionitem.nav_cmd != NAV_CMD_IDLE && + missionitem.nav_cmd != NAV_CMD_WAYPOINT && + missionitem.nav_cmd != NAV_CMD_LOITER_UNLIMITED && + missionitem.nav_cmd != NAV_CMD_LOITER_TURN_COUNT && + missionitem.nav_cmd != NAV_CMD_LOITER_TIME_LIMIT && + missionitem.nav_cmd != NAV_CMD_LAND && + missionitem.nav_cmd != NAV_CMD_TAKEOFF && + missionitem.nav_cmd != NAV_CMD_ROI && + missionitem.nav_cmd != NAV_CMD_PATHPLANNING && + missionitem.nav_cmd != NAV_CMD_DO_JUMP) { + + mavlink_log_critical(_mavlink_fd, "Rejecting mission item %i: unsupported action.", (int)(i+1)); + return false; + } + } + mavlink_log_critical(_mavlink_fd, "Mission is valid!"); + return true; +} + bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems) { /* Go through all mission items and search for a landing waypoint diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h index 9a77a6dc25..94b6b29226 100644 --- a/src/modules/navigator/mission_feasibility_checker.h +++ b/src/modules/navigator/mission_feasibility_checker.h @@ -62,6 +62,7 @@ private: /* Checks for all airframes */ bool checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence); bool checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool throw_error = false); + bool checkMissionItemValidity(dm_item_t dm_current, size_t nMissionItems); /* Checks specific to fixedwing airframes */ bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt); From 323759bb5279af62dcc4a0454d3ec06debae36ea Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 21 May 2015 22:06:54 +0200 Subject: [PATCH 6/6] commander: Fix error checking and handling of level routine --- src/modules/commander/accelerometer_calibration.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 32c0ffd8cc..f250eef475 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -557,7 +557,7 @@ calibrate_return calculate_calibration_values(unsigned sensor, float (&accel_ref int do_level_calibration(int mavlink_fd) { const unsigned cal_time = 5; - const unsigned cal_hz = 250; + const unsigned cal_hz = 100; const unsigned settle_time = 30; int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); struct vehicle_attitude_s att; @@ -612,6 +612,8 @@ int do_level_calibration(int mavlink_fd) { pitch_mean /= counter; } else { mavlink_and_console_log_info(mavlink_fd, "not enough measurements taken"); + success = false; + goto out; } if (fabsf(roll_mean) > 0.8f ) { @@ -626,6 +628,7 @@ int do_level_calibration(int mavlink_fd) { success = true; } +out: if (success) { mavlink_and_console_log_info(mavlink_fd, CAL_QGC_DONE_MSG, "level"); return 0;