From d48a8bc073a3aa5f515c582a1c2c3cae58a8d783 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 12 Jun 2014 19:09:18 +0200 Subject: [PATCH 1/4] navigator: renamed the different RTL states --- src/modules/commander/commander.cpp | 10 ++--- .../commander/state_machine_helper.cpp | 8 ++-- src/modules/mavlink/mavlink_messages.cpp | 4 +- src/modules/navigator/navigator_main.cpp | 4 +- src/modules/uORB/topics/vehicle_status.h | 37 ++++++++++--------- 5 files changed, 32 insertions(+), 31 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index bb75b2af07..e992706ac3 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1300,7 +1300,7 @@ int commander_thread_main(int argc, char *argv[]) /* if we have a global position, we can switch to RTL, if not, we can try to land */ if (status.condition_global_position_valid) { - status.failsafe_state = FAILSAFE_STATE_RTL_RC; + status.failsafe_state = FAILSAFE_STATE_RC_LOSS; } else { status.failsafe_state = FAILSAFE_STATE_LAND; } @@ -1313,10 +1313,10 @@ int commander_thread_main(int argc, char *argv[]) /* hack to detect if we finished a mission after we lost RC, so that we can trigger RTL now */ if (status.rc_signal_lost && status.set_nav_state == NAVIGATION_STATE_AUTO_MISSION && - mission_result.mission_finished && status.failsafe_state != FAILSAFE_STATE_RTL_RC) { + mission_result.mission_finished && status.failsafe_state != FAILSAFE_STATE_RC_LOSS) { /* if we have a global position, we can switch to RTL, if not, we can try to land */ if (status.condition_global_position_valid) { - status.failsafe_state = FAILSAFE_STATE_RTL_RC; + status.failsafe_state = FAILSAFE_STATE_RC_LOSS; mavlink_log_info(mavlink_fd, "#audio: RTL after Mission is finished"); } else { /* this probably doesn't make sense since we are in mission and have global position */ @@ -1719,8 +1719,8 @@ set_control_mode() case NAVIGATION_STATE_AUTO_MISSION: case NAVIGATION_STATE_AUTO_LOITER: case NAVIGATION_STATE_AUTO_RTL: - case NAVIGATION_STATE_AUTO_RTL_RC: - case NAVIGATION_STATE_AUTO_RTL_DL: + case NAVIGATION_STATE_AUTO_FAILSAFE_RC_LOSS: + case NAVIGATION_STATE_AUTO_FAILSAFE_DL_LOSS: control_mode.flag_control_manual_enabled = false; control_mode.flag_control_auto_enabled = true; control_mode.flag_control_rates_enabled = true; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 2144800790..c52e618ef1 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -406,12 +406,12 @@ void set_nav_state(struct vehicle_status_s *status) } break; - case FAILSAFE_STATE_RTL_RC: - status->set_nav_state = NAVIGATION_STATE_AUTO_RTL_RC; + case FAILSAFE_STATE_RC_LOSS: + status->set_nav_state = NAVIGATION_STATE_AUTO_FAILSAFE_RC_LOSS; break; - case FAILSAFE_STATE_RTL_DL: - status->set_nav_state = NAVIGATION_STATE_AUTO_RTL_DL; + case FAILSAFE_STATE_DL_LOSS: + status->set_nav_state = NAVIGATION_STATE_AUTO_FAILSAFE_DL_LOSS; break; case FAILSAFE_STATE_LAND: diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index e7d2ba515a..b58fb4cb8a 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -161,8 +161,8 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set break; case NAVIGATION_STATE_AUTO_RTL: - case NAVIGATION_STATE_AUTO_RTL_RC: - case NAVIGATION_STATE_AUTO_RTL_DL: + case NAVIGATION_STATE_AUTO_FAILSAFE_RC_LOSS: + case NAVIGATION_STATE_AUTO_FAILSAFE_DL_LOSS: *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index dc85738134..20cff5c282 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -351,8 +351,8 @@ Navigator::task_main() _navigation_mode = &_loiter; break; case NAVIGATION_STATE_AUTO_RTL: - case NAVIGATION_STATE_AUTO_RTL_RC: - case NAVIGATION_STATE_AUTO_RTL_DL: + case NAVIGATION_STATE_AUTO_FAILSAFE_RC_LOSS: + case NAVIGATION_STATE_AUTO_FAILSAFE_DL_LOSS: _navigation_mode = &_rtl; break; case NAVIGATION_STATE_LAND: diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 259d7329e8..9390ff717f 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -1,10 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier - * @author Petri Tanskanen - * @author Thomas Gubler - * @author Julian Oes + * Copyright (C) 2012 - 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 @@ -45,6 +41,11 @@ * All apps should write to subsystem_info: * * (any app) --> subsystem_info (published) --> (commander app state machine) --> vehicle_status --> (mavlink app) + * + * @author Lorenz Meier + * @author Petri Tanskanen + * @author Thomas Gubler + * @author Julian Oes */ #ifndef VEHICLE_STATUS_H_ @@ -90,25 +91,25 @@ typedef enum { typedef enum { FAILSAFE_STATE_NORMAL = 0, /**< Normal operation */ - FAILSAFE_STATE_RTL_RC, /**< Return To Launch on remote control loss */ - FAILSAFE_STATE_RTL_DL, /**< Return To Launch on datalink loss */ + FAILSAFE_STATE_RC_LOSS, /**< Return To Launch on remote control loss */ + FAILSAFE_STATE_DL_LOSS, /**< Return To Launch on datalink loss */ FAILSAFE_STATE_LAND, /**< Land as safe as possible */ FAILSAFE_STATE_TERMINATION, /**< Disable motors and use parachute, can't be recovered */ FAILSAFE_STATE_MAX, } failsafe_state_t; typedef enum { - NAVIGATION_STATE_MANUAL = 0, - NAVIGATION_STATE_ACRO, - NAVIGATION_STATE_ALTCTL, - NAVIGATION_STATE_POSCTL, - NAVIGATION_STATE_AUTO_MISSION, - NAVIGATION_STATE_AUTO_LOITER, - NAVIGATION_STATE_AUTO_RTL, - NAVIGATION_STATE_AUTO_RTL_RC, - NAVIGATION_STATE_AUTO_RTL_DL, - NAVIGATION_STATE_LAND, - NAVIGATION_STATE_TERMINATION, + NAVIGATION_STATE_MANUAL = 0, /**< Manual mode */ + NAVIGATION_STATE_ACRO, /**< Acro mode */ + NAVIGATION_STATE_ALTCTL, /**< Altitude control mode */ + NAVIGATION_STATE_POSCTL, /**< Position control mode */ + NAVIGATION_STATE_AUTO_MISSION, /**< Auto mission mode */ + NAVIGATION_STATE_AUTO_LOITER, /**< Auto loiter mode */ + NAVIGATION_STATE_AUTO_RTL, /**< Auto RTL mode */ + NAVIGATION_STATE_AUTO_FAILSAFE_RC_LOSS, /**< Auto failsafe on RC loss mode */ + NAVIGATION_STATE_AUTO_FAILSAFE_DL_LOSS, /**< Auto failsafe on datalink loss mode */ + NAVIGATION_STATE_LAND, /**< Land mode */ + NAVIGATION_STATE_TERMINATION, /**< Termination mode */ } navigation_state_t; enum VEHICLE_MODE_FLAG { From 3b39a8a789e347e318375ebe18c583eabec0501c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 12 Jun 2014 19:37:25 +0200 Subject: [PATCH 2/4] navigator: rename update and reset calls to on_active and on_inactive --- src/modules/navigator/mission.cpp | 6 +++--- src/modules/navigator/mission.h | 4 ++-- src/modules/navigator/navigator_main.cpp | 12 ++++++------ src/modules/navigator/navigator_mode.cpp | 6 +++--- src/modules/navigator/navigator_mode.h | 4 ++-- src/modules/navigator/rtl.cpp | 6 +++--- src/modules/navigator/rtl.h | 4 ++-- 7 files changed, 21 insertions(+), 21 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index a551df9a24..9244063b19 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -72,7 +72,7 @@ Mission::Mission(Navigator *navigator, const char *name) : /* load initial params */ updateParams(); /* set initial mission items */ - reset(); + on_inactive(); } @@ -81,7 +81,7 @@ Mission::~Mission() } void -Mission::reset() +Mission::on_inactive() { _first_run = true; @@ -100,7 +100,7 @@ Mission::reset() } bool -Mission::update(struct position_setpoint_triplet_s *pos_sp_triplet) +Mission::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) { bool updated = false; diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index a3dd09ecd8..322aaf96af 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -78,12 +78,12 @@ public: /** * This function is called while the mode is inactive */ - virtual void reset(); + virtual void on_inactive(); /** * This function is called while the mode is active */ - virtual bool update(struct position_setpoint_triplet_s *pos_sp_triplet); + virtual bool on_active(struct position_setpoint_triplet_s *pos_sp_triplet); private: /** diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 20cff5c282..a3c190c7f2 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -365,21 +365,21 @@ Navigator::task_main() /* TODO: make list of modes and loop through it */ if (_navigation_mode == &_mission) { - _update_triplet = _mission.update(&_pos_sp_triplet); + _update_triplet = _mission.on_active(&_pos_sp_triplet); } else { - _mission.reset(); + _mission.on_inactive(); } if (_navigation_mode == &_rtl) { - _update_triplet = _rtl.update(&_pos_sp_triplet); + _update_triplet = _rtl.on_active(&_pos_sp_triplet); } else { - _rtl.reset(); + _rtl.on_inactive(); } if (_navigation_mode == &_loiter) { - _update_triplet = _loiter.update(&_pos_sp_triplet); + _update_triplet = _loiter.on_active(&_pos_sp_triplet); } else { - _loiter.reset(); + _loiter.on_inactive(); } /* if nothing is running, set position setpoint triplet invalid */ diff --git a/src/modules/navigator/navigator_mode.cpp b/src/modules/navigator/navigator_mode.cpp index c968291901..25e767c2b3 100644 --- a/src/modules/navigator/navigator_mode.cpp +++ b/src/modules/navigator/navigator_mode.cpp @@ -48,7 +48,7 @@ NavigatorMode::NavigatorMode(Navigator *navigator, const char *name) : /* load initial params */ updateParams(); /* set initial mission items */ - reset(); + on_inactive(); } NavigatorMode::~NavigatorMode() @@ -56,13 +56,13 @@ NavigatorMode::~NavigatorMode() } void -NavigatorMode::reset() +NavigatorMode::on_inactive() { _first_run = true; } bool -NavigatorMode::update(struct position_setpoint_triplet_s *pos_sp_triplet) +NavigatorMode::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) { pos_sp_triplet->current.valid = false; _first_run = false; diff --git a/src/modules/navigator/navigator_mode.h b/src/modules/navigator/navigator_mode.h index 0844bb94d4..cbb53d91b9 100644 --- a/src/modules/navigator/navigator_mode.h +++ b/src/modules/navigator/navigator_mode.h @@ -68,7 +68,7 @@ public: /** * This function is called while the mode is inactive */ - virtual void reset(); + virtual void on_inactive(); /** * This function is called while the mode is active @@ -76,7 +76,7 @@ public: * @param position setpoint triplet to set * @return true if position setpoint triplet has been changed */ - virtual bool update(struct position_setpoint_triplet_s *pos_sp_triplet); + virtual bool on_active(struct position_setpoint_triplet_s *pos_sp_triplet); protected: Navigator *_navigator; diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 8888c6b625..c1b1d3f097 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -64,7 +64,7 @@ RTL::RTL(Navigator *navigator, const char *name) : /* load initial params */ updateParams(); /* initial reset */ - reset(); + on_inactive(); } RTL::~RTL() @@ -72,14 +72,14 @@ RTL::~RTL() } void -RTL::reset() +RTL::on_inactive() { _first_run = true; _rtl_state = RTL_STATE_NONE; } bool -RTL::update(struct position_setpoint_triplet_s *pos_sp_triplet) +RTL::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) { bool updated = false; diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index 9836f5064e..bcccf74548 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -70,7 +70,7 @@ public: /** * This function is called while the mode is inactive */ - void reset(); + void on_inactive(); /** * This function is called while the mode is active @@ -78,7 +78,7 @@ public: * @param position setpoint triplet that needs to be set * @return true if updated */ - bool update(position_setpoint_triplet_s *pos_sp_triplet); + bool on_active(position_setpoint_triplet_s *pos_sp_triplet); private: From e14d4751423e21a9ce052e1f6760f929d3694515 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 13 Jun 2014 14:31:19 +0200 Subject: [PATCH 3/4] navigator: update Loiter navigator mode to use new method names --- src/modules/navigator/loiter.cpp | 6 +++--- src/modules/navigator/loiter.h | 4 ++-- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp index 035d035e16..8bbb79d5ee 100644 --- a/src/modules/navigator/loiter.cpp +++ b/src/modules/navigator/loiter.cpp @@ -59,7 +59,7 @@ Loiter::Loiter(Navigator *navigator, const char *name) : /* load initial params */ updateParams(); /* initial reset */ - reset(); + on_inactive(); } Loiter::~Loiter() @@ -67,14 +67,14 @@ Loiter::~Loiter() } bool -Loiter::update(struct position_setpoint_triplet_s *pos_sp_triplet) +Loiter::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) { /* set loiter item, don't reuse an existing position setpoint */ return set_loiter_item(false, pos_sp_triplet);; } void -Loiter::reset() +Loiter::on_inactive() { } diff --git a/src/modules/navigator/loiter.h b/src/modules/navigator/loiter.h index a83b53f438..685ae61410 100644 --- a/src/modules/navigator/loiter.h +++ b/src/modules/navigator/loiter.h @@ -63,12 +63,12 @@ public: /** * This function is called while the mode is inactive */ - bool update(struct position_setpoint_triplet_s *pos_sp_triplet); + virtual void on_inactive(); /** * This function is called while the mode is active */ - void reset(); + virtual bool on_active(struct position_setpoint_triplet_s *pos_sp_triplet); }; #endif From 195c4005e1bae41fcead6ff609f05a1058e6256d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 13 Jun 2014 23:36:26 +0200 Subject: [PATCH 4/4] mtecs: update default params --- src/modules/fw_pos_control_l1/mtecs/mTecs_params.c | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c index c95bf1dc9d..bbbb8f9dbe 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c @@ -65,7 +65,7 @@ PARAM_DEFINE_INT32(MT_ENABLED, 1); * @max 10.0 * @group mTECS */ -PARAM_DEFINE_FLOAT(MT_THR_FF, 0.2f); +PARAM_DEFINE_FLOAT(MT_THR_FF, 0.7f); /** * Total Energy Rate Control P @@ -75,7 +75,7 @@ PARAM_DEFINE_FLOAT(MT_THR_FF, 0.2f); * @max 10.0 * @group mTECS */ -PARAM_DEFINE_FLOAT(MT_THR_P, 0.03f); +PARAM_DEFINE_FLOAT(MT_THR_P, 0.1f); /** * Total Energy Rate Control I @@ -85,7 +85,7 @@ PARAM_DEFINE_FLOAT(MT_THR_P, 0.03f); * @max 10.0 * @group mTECS */ -PARAM_DEFINE_FLOAT(MT_THR_I, 0.1f); +PARAM_DEFINE_FLOAT(MT_THR_I, 0.25f); /** * Total Energy Rate Control Offset (Cruise throttle sp) @@ -104,7 +104,7 @@ PARAM_DEFINE_FLOAT(MT_THR_OFF, 0.7f); * @max 10.0 * @group mTECS */ -PARAM_DEFINE_FLOAT(MT_PIT_FF, 0.1f); +PARAM_DEFINE_FLOAT(MT_PIT_FF, 0.4f); /** * Energy Distribution Rate Control P @@ -182,7 +182,7 @@ PARAM_DEFINE_FLOAT(MT_PIT_MAX, 20.0f); * @max 10.0f * @group mTECS */ -PARAM_DEFINE_FLOAT(MT_FPA_P, 0.2f); +PARAM_DEFINE_FLOAT(MT_FPA_P, 0.3f); /** * D gain for the altitude control @@ -210,7 +210,7 @@ PARAM_DEFINE_FLOAT(MT_FPA_D_LP, 1.0f); * @unit deg * @group mTECS */ -PARAM_DEFINE_FLOAT(MT_FPA_MIN, -10.0f); +PARAM_DEFINE_FLOAT(MT_FPA_MIN, -20.0f); /** * Maximal flight path angle setpoint @@ -237,7 +237,7 @@ PARAM_DEFINE_FLOAT(MT_A_LP, 1.0f); * @max 10.0f * @group mTECS */ -PARAM_DEFINE_FLOAT(MT_ACC_P, 1.5f); +PARAM_DEFINE_FLOAT(MT_ACC_P, 0.3f); /** * D gain for the airspeed control