forked from Archive/PX4-Autopilot
Merge remote-tracking branch 'upstream/navigator_rewrite' into navigator_rewrite_estimator
This commit is contained in:
commit
719779a07b
|
@ -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;
|
||||
|
|
|
@ -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:
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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()
|
||||
{
|
||||
}
|
||||
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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;
|
||||
|
||||
|
|
|
@ -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:
|
||||
/**
|
||||
|
|
|
@ -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:
|
||||
|
@ -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 */
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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;
|
||||
|
||||
|
|
|
@ -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:
|
||||
|
|
|
@ -1,10 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Author: @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
* @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
|
||||
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||
* @author Julian Oes <joes@student.ethz.ch>
|
||||
* 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 <lm@inf.ethz.ch>
|
||||
* @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
|
||||
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||
* @author Julian Oes <julian@oes.ch>
|
||||
*/
|
||||
|
||||
#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 {
|
||||
|
|
Loading…
Reference in New Issue