diff --git a/ROMFS/px4fmu_common/init.d/4030_solo b/ROMFS/px4fmu_common/init.d/4030_solo index 185b261325..e07fd9b1de 100644 --- a/ROMFS/px4fmu_common/init.d/4030_solo +++ b/ROMFS/px4fmu_common/init.d/4030_solo @@ -15,10 +15,16 @@ sh /etc/init.d/rc.mc_defaults if [ $AUTOCNF == yes ] then + # tuning param set MC_PITCHRATE_P 0.13 param set MC_ROLLRATE_P 0.13 param set MPC_MANTHR_MIN 0.08 + # takeoff. land and RTL settings + param set MIS_TAKEOFF_ALT 5.0 + param set COM_DISARM_LAND 1 + param set RTL_LAND_DELAY 1 + # enable high bandwidth mavlink by default param set SYS_COMPANION 921601 fi diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index f3a1767dde..8a67f928e5 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -10,7 +10,8 @@ uint8 MAIN_STATE_OFFBOARD = 7 uint8 MAIN_STATE_STAB = 8 uint8 MAIN_STATE_RATTITUDE = 9 uint8 MAIN_STATE_AUTO_TAKEOFF = 10 -uint8 MAIN_STATE_MAX = 11 +uint8 MAIN_STATE_AUTO_LAND = 11 +uint8 MAIN_STATE_MAX = 12 # If you change the order, add or remove arming_state_t states make sure to update the arrays # in state_machine_helper.cpp as well. @@ -104,6 +105,7 @@ uint16 counter # incremented by the writing thread everytime new data is store uint64 timestamp # in microseconds since system start, is set whenever the writing thread stores new data uint8 main_state # main state machine +uint8 main_state_prev # previous main state uint8 nav_state # set navigation state machine to specified value uint8 arming_state # current arming state uint8 hil_state # current hil state diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 499b11b6c1..12fa023a0e 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1083,6 +1083,7 @@ int commander_thread_main(int argc, char *argv[]) status.rc_input_blocked = false; status.rc_input_mode = vehicle_status_s::RC_IN_MODE_DEFAULT; status.main_state =vehicle_status_s::MAIN_STATE_MANUAL; + status.main_state_prev = vehicle_status_s::MAIN_STATE_MAX; status.nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL; status.arming_state = vehicle_status_s::ARMING_STATE_INIT; @@ -2348,6 +2349,25 @@ int commander_thread_main(int argc, char *argv[]) } } + /* handle main state after takeoff and land */ + if (status.main_state == vehicle_status_s::MAIN_STATE_AUTO_TAKEOFF + && mission_result.finished) { + + // transition back to state we had before takeoff + if (status.main_state_prev < vehicle_status_s::MAIN_STATE_MAX + && status.main_state_prev != vehicle_status_s::MAIN_STATE_AUTO_LAND) { + main_state_transition(&status, status.main_state_prev); + } + + } else if (status.main_state == vehicle_status_s::MAIN_STATE_AUTO_LAND + && status.condition_landed) { + + // transition back to state we had before takeoff + if (status.main_state_prev < vehicle_status_s::MAIN_STATE_MAX + && status.main_state_prev != vehicle_status_s::MAIN_STATE_AUTO_TAKEOFF) { + main_state_transition(&status, status.main_state_prev); + } + } /* handle commands last, as the system needs to be updated to handle them */ orb_check(cmd_sub, &updated); diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 1bde387c63..5163087830 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -352,6 +352,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta case vehicle_status_s::MAIN_STATE_AUTO_MISSION: case vehicle_status_s::MAIN_STATE_AUTO_RTL: case vehicle_status_s::MAIN_STATE_AUTO_TAKEOFF: + case vehicle_status_s::MAIN_STATE_AUTO_LAND: /* need global position and home position */ if (status->condition_global_position_valid && status->condition_home_position_valid) { ret = TRANSITION_CHANGED; @@ -373,6 +374,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta } if (ret == TRANSITION_CHANGED) { if (status->main_state != new_main_state) { + status->main_state_prev = status->main_state; status->main_state = new_main_state; } else { ret = TRANSITION_NOT_CHANGED; @@ -766,6 +768,25 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en } break; + case vehicle_status_s::MAIN_STATE_AUTO_LAND: + /* require global position and home */ + + if (status->engine_failure) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; + } else if ((!status->condition_global_position_valid || + !status->condition_home_position_valid)) { + status->failsafe = true; + + if (status->condition_local_altitude_valid) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; + } else { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; + } + } else { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND; + } + break; + case vehicle_status_s::MAIN_STATE_OFFBOARD: /* require offboard control, otherwise stay where you are */ if (status->offboard_control_signal_lost && !status->rc_signal_lost) { diff --git a/src/modules/navigator/CMakeLists.txt b/src/modules/navigator/CMakeLists.txt index 6a5c4c15d9..0ce9399a27 100644 --- a/src/modules/navigator/CMakeLists.txt +++ b/src/modules/navigator/CMakeLists.txt @@ -45,6 +45,7 @@ px4_add_module( loiter.cpp rtl.cpp takeoff.cpp + land.cpp mission_feasibility_checker.cpp geofence.cpp datalinkloss.cpp diff --git a/src/modules/navigator/land.cpp b/src/modules/navigator/land.cpp new file mode 100644 index 0000000000..25da635622 --- /dev/null +++ b/src/modules/navigator/land.cpp @@ -0,0 +1,96 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2014 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file land.cpp + * + * Helper class to land at the current position + * + * @author Andreas Antener + */ + +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include "land.h" +#include "navigator.h" + +Land::Land(Navigator *navigator, const char *name) : + MissionBlock(navigator, name) +{ + /* load initial params */ + updateParams(); +} + +Land::~Land() +{ +} + +void +Land::on_inactive() +{ +} + +void +Land::on_activation() +{ + /* set current mission item to Land */ + set_land_item(&_mission_item, true); + + /* convert mission item to current setpoint */ + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + pos_sp_triplet->previous.valid = false; + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); + pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_LAND; + pos_sp_triplet->next.valid = false; + + _navigator->set_can_loiter_at_sp(false); + + _navigator->set_position_setpoint_triplet_updated(); +} + +void +Land::on_active() +{ + if (is_mission_item_reached()) { + set_idle_item(&_mission_item); + } +} diff --git a/src/modules/navigator/land.h b/src/modules/navigator/land.h new file mode 100644 index 0000000000..c97feb7046 --- /dev/null +++ b/src/modules/navigator/land.h @@ -0,0 +1,65 @@ +/*************************************************************************** + * + * Copyright (c) 2014 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file land.h + * + * Helper class to land at the current position + * + * @author Andreas Antener + */ + +#ifndef NAVIGATOR_LAND_H +#define NAVIGATOR_LAND_H + +#include +#include + +#include "navigator_mode.h" +#include "mission_block.h" + +class Land : public MissionBlock +{ +public: + Land(Navigator *navigator, const char *name); + + ~Land(); + + virtual void on_inactive(); + + virtual void on_activation(); + + virtual void on_active(); + +}; + +#endif diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index e2f454d4cf..7c55eccf57 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -323,3 +323,51 @@ MissionBlock::set_takeoff_item(struct mission_item_s *item, float min_clearance, item->autocontinue = false; item->origin = ORIGIN_ONBOARD; } + +void +MissionBlock::set_land_item(struct mission_item_s *item, bool at_current_location) +{ + item->nav_cmd = NAV_CMD_LAND; + + /* use current position */ + if (at_current_location) { + item->lat = _navigator->get_global_position()->lat; + item->lon = _navigator->get_global_position()->lon; + + /* use home position */ + } else { + item->lat = _navigator->get_home_position()->lat; + item->lon = _navigator->get_home_position()->lon; + } + + item->altitude = 0; + item->altitude_is_relative = false; + item->yaw = NAN; + item->loiter_radius = _navigator->get_loiter_radius(); + item->loiter_direction = 1; + item->acceptance_radius = _navigator->get_acceptance_radius(); + item->time_inside = 0.0f; + item->pitch_min = 0.0f; + item->autocontinue = true; + item->origin = ORIGIN_ONBOARD; +} + +void +MissionBlock::set_idle_item(struct mission_item_s *item) +{ + item->nav_cmd = NAV_CMD_IDLE; + item->lat = _navigator->get_home_position()->lat; + item->lon = _navigator->get_home_position()->lon; + item->altitude_is_relative = false; + item->altitude = _navigator->get_home_position()->alt; + item->yaw = NAN; + item->loiter_radius = _navigator->get_loiter_radius(); + item->loiter_direction = 1; + item->acceptance_radius = _navigator->get_acceptance_radius(); + item->time_inside = 0.0f; + item->pitch_min = 0.0f; + item->autocontinue = true; + item->origin = ORIGIN_ONBOARD; +} + + diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index 5490b4c6b8..36a1ecaba1 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -101,6 +101,16 @@ protected: */ void set_takeoff_item(struct mission_item_s *item, float min_clearance = -1.0f, float min_pitch = 0.0f); + /** + * Set a land mission item + */ + void set_land_item(struct mission_item_s *item, bool at_current_location); + + /** + * Set idle mission item + */ + void set_idle_item(struct mission_item_s *item); + mission_item_s _mission_item; bool _waypoint_position_reached; bool _waypoint_yaw_reached; diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index b154d21769..514705a68a 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -62,6 +62,7 @@ #include "mission.h" #include "loiter.h" #include "takeoff.h" +#include "land.h" #include "rtl.h" #include "datalinkloss.h" #include "enginefailure.h" @@ -72,7 +73,7 @@ /** * Number of navigation modes that need on_active/on_inactive calls */ -#define NAVIGATOR_MODE_ARRAY_SIZE 8 +#define NAVIGATOR_MODE_ARRAY_SIZE 9 class Navigator : public control::SuperBlock { @@ -226,6 +227,7 @@ private: Mission _mission; /**< class that handles the missions */ Loiter _loiter; /**< class that handles loiter */ Takeoff _takeoff; /**< class for handling takeoff commands */ + Land _land; /**< class for handling land commands */ RTL _rtl; /**< class that handles RTL */ RCLoss _rcLoss; /**< class that handles RTL according to OBC rules (rc loss mode) */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 7d4736449f..b16f9b9b91 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -142,6 +142,7 @@ Navigator::Navigator() : _mission(this, "MIS"), _loiter(this, "LOI"), _takeoff(this, "TKF"), + _land(this, "LND"), _rtl(this, "RTL"), _rcLoss(this, "RCL"), _dataLinkLoss(this, "DLL"), @@ -161,6 +162,7 @@ Navigator::Navigator() : _navigation_mode_array[5] = &_gpsFailure; _navigation_mode_array[6] = &_rcLoss; _navigation_mode_array[7] = &_takeoff; + _navigation_mode_array[8] = &_land; updateParams(); } @@ -444,7 +446,6 @@ Navigator::task_main() case vehicle_status_s::NAVIGATION_STATE_ACRO: case vehicle_status_s::NAVIGATION_STATE_ALTCTL: case vehicle_status_s::NAVIGATION_STATE_POSCTL: - case vehicle_status_s::NAVIGATION_STATE_LAND: case vehicle_status_s::NAVIGATION_STATE_TERMINATION: case vehicle_status_s::NAVIGATION_STATE_OFFBOARD: _navigation_mode = nullptr; @@ -481,6 +482,10 @@ Navigator::task_main() _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_takeoff; break; + case vehicle_status_s::NAVIGATION_STATE_LAND: + _pos_sp_triplet_published_invalid_once = false; + _navigation_mode = &_land; + break; case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS: /* Use complex data link loss mode only when enabled via param * otherwise use rtl */ diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index a77f5895e1..0fc377874d 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -243,39 +243,14 @@ RTL::set_rtl_item() } case RTL_STATE_LAND: { - _mission_item.lat = _navigator->get_home_position()->lat; - _mission_item.lon = _navigator->get_home_position()->lon; - _mission_item.altitude_is_relative = false; - _mission_item.altitude = _navigator->get_home_position()->alt; - _mission_item.yaw = _navigator->get_home_position()->yaw; - _mission_item.loiter_radius = _navigator->get_loiter_radius(); - _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = NAV_CMD_LAND; - _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - _mission_item.time_inside = 0.0f; - _mission_item.pitch_min = 0.0f; - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; + set_land_item(&_mission_item, false); mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: land at home"); break; } case RTL_STATE_LANDED: { - _mission_item.lat = _navigator->get_home_position()->lat; - _mission_item.lon = _navigator->get_home_position()->lon; - _mission_item.altitude_is_relative = false; - _mission_item.altitude = _navigator->get_home_position()->alt; - // Do not change / control yaw in landed - _mission_item.yaw = NAN; - _mission_item.loiter_radius = _navigator->get_loiter_radius(); - _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = NAV_CMD_IDLE; - _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - _mission_item.time_inside = 0.0f; - _mission_item.pitch_min = 0.0f; - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; + set_idle_item(&_mission_item); mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: completed, landed"); break; diff --git a/src/modules/navigator/takeoff.cpp b/src/modules/navigator/takeoff.cpp index 5ea96f8b3f..8d5040eac2 100644 --- a/src/modules/navigator/takeoff.cpp +++ b/src/modules/navigator/takeoff.cpp @@ -75,6 +75,8 @@ Takeoff::on_activation() { /* set current mission item to Takeoff */ set_takeoff_item(&_mission_item, _param_min_alt.get()); + _navigator->get_mission_result()->reached = false; + _navigator->get_mission_result()->finished = false; /* convert mission item to current setpoint */ struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); @@ -93,4 +95,8 @@ Takeoff::on_activation() void Takeoff::on_active() { + if (is_mission_item_reached()) { + _navigator->get_mission_result()->finished = true; + _navigator->set_mission_result_updated(); + } }