dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge remote-tracking branch 'upstream/navigator_rewrite' into navigator_rewrite_estimator

This commit is contained in:
Thomas Gubler 2014-06-13 23:38:02 +02:00
parent 214a8cab49 a702a350ce
commit 719779a07b
14 changed files with 65 additions and 64 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
src/modules/commander/commander.cpp
View File

@ -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 we have a global position, we can switch to RTL, if not, we can try to land */
if (status.condition_global_position_valid) { if (status.condition_global_position_valid) {
status.failsafe_state = FAILSAFE_STATE_RTL_RC; status.failsafe_state = FAILSAFE_STATE_RC_LOSS;
} else { } else {
status.failsafe_state = FAILSAFE_STATE_LAND; 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 */ /* 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 && 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 we have a global position, we can switch to RTL, if not, we can try to land */
if (status.condition_global_position_valid) { 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"); mavlink_log_info(mavlink_fd, "#audio: RTL after Mission is finished");
} else { } else {
/* this probably doesn't make sense since we are in mission and have global position */ /* 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_MISSION:
case NAVIGATION_STATE_AUTO_LOITER: case NAVIGATION_STATE_AUTO_LOITER:
case NAVIGATION_STATE_AUTO_RTL: case NAVIGATION_STATE_AUTO_RTL:
case NAVIGATION_STATE_AUTO_RTL_RC: case NAVIGATION_STATE_AUTO_FAILSAFE_RC_LOSS:
case NAVIGATION_STATE_AUTO_RTL_DL: case NAVIGATION_STATE_AUTO_FAILSAFE_DL_LOSS:
control_mode.flag_control_manual_enabled = false; control_mode.flag_control_manual_enabled = false;
control_mode.flag_control_auto_enabled = true; control_mode.flag_control_auto_enabled = true;
control_mode.flag_control_rates_enabled = true; control_mode.flag_control_rates_enabled = true;

8
src/modules/commander/state_machine_helper.cpp
View File

@ -406,12 +406,12 @@ void set_nav_state(struct vehicle_status_s *status)
} }
break; break;
case FAILSAFE_STATE_RTL_RC: case FAILSAFE_STATE_RC_LOSS:
status->set_nav_state = NAVIGATION_STATE_AUTO_RTL_RC; status->set_nav_state = NAVIGATION_STATE_AUTO_FAILSAFE_RC_LOSS;
break; break;
case FAILSAFE_STATE_RTL_DL: case FAILSAFE_STATE_DL_LOSS:
status->set_nav_state = NAVIGATION_STATE_AUTO_RTL_DL; status->set_nav_state = NAVIGATION_STATE_AUTO_FAILSAFE_DL_LOSS;
break; break;
case FAILSAFE_STATE_LAND: case FAILSAFE_STATE_LAND:

14
src/modules/fw_pos_control_l1/mtecs/mTecs_params.c
View File

@ -65,7 +65,7 @@ PARAM_DEFINE_INT32(MT_ENABLED, 1);
* @max 10.0 * @max 10.0
* @group mTECS * @group mTECS
*/ */
PARAM_DEFINE_FLOAT(MT_THR_FF, 0.2f); PARAM_DEFINE_FLOAT(MT_THR_FF, 0.7f);
/** /**
* Total Energy Rate Control P * Total Energy Rate Control P
@ -75,7 +75,7 @@ PARAM_DEFINE_FLOAT(MT_THR_FF, 0.2f);
* @max 10.0 * @max 10.0
* @group mTECS * @group mTECS
*/ */
PARAM_DEFINE_FLOAT(MT_THR_P, 0.03f); PARAM_DEFINE_FLOAT(MT_THR_P, 0.1f);
/** /**
* Total Energy Rate Control I * Total Energy Rate Control I
@ -85,7 +85,7 @@ PARAM_DEFINE_FLOAT(MT_THR_P, 0.03f);
* @max 10.0 * @max 10.0
* @group mTECS * @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) * Total Energy Rate Control Offset (Cruise throttle sp)
@ -104,7 +104,7 @@ PARAM_DEFINE_FLOAT(MT_THR_OFF, 0.7f);
* @max 10.0 * @max 10.0
* @group mTECS * @group mTECS
*/ */
PARAM_DEFINE_FLOAT(MT_PIT_FF, 0.1f); PARAM_DEFINE_FLOAT(MT_PIT_FF, 0.4f);
/** /**
* Energy Distribution Rate Control P * Energy Distribution Rate Control P
@ -182,7 +182,7 @@ PARAM_DEFINE_FLOAT(MT_PIT_MAX, 20.0f);
* @max 10.0f * @max 10.0f
* @group mTECS * @group mTECS
*/ */
PARAM_DEFINE_FLOAT(MT_FPA_P, 0.2f); PARAM_DEFINE_FLOAT(MT_FPA_P, 0.3f);
/** /**
* D gain for the altitude control * D gain for the altitude control
@ -210,7 +210,7 @@ PARAM_DEFINE_FLOAT(MT_FPA_D_LP, 1.0f);
* @unit deg * @unit deg
* @group mTECS * @group mTECS
*/ */
PARAM_DEFINE_FLOAT(MT_FPA_MIN, -10.0f); PARAM_DEFINE_FLOAT(MT_FPA_MIN, -20.0f);
/** /**
* Maximal flight path angle setpoint * Maximal flight path angle setpoint
@ -237,7 +237,7 @@ PARAM_DEFINE_FLOAT(MT_A_LP, 1.0f);
* @max 10.0f * @max 10.0f
* @group mTECS * @group mTECS
*/ */
PARAM_DEFINE_FLOAT(MT_ACC_P, 1.5f); PARAM_DEFINE_FLOAT(MT_ACC_P, 0.3f);
/** /**
* D gain for the airspeed control * D gain for the airspeed control

4
src/modules/mavlink/mavlink_messages.cpp
View File

@ -161,8 +161,8 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
break; break;
case NAVIGATION_STATE_AUTO_RTL: case NAVIGATION_STATE_AUTO_RTL:
case NAVIGATION_STATE_AUTO_RTL_RC: case NAVIGATION_STATE_AUTO_FAILSAFE_RC_LOSS:
case NAVIGATION_STATE_AUTO_RTL_DL: case NAVIGATION_STATE_AUTO_FAILSAFE_DL_LOSS:
*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED
| MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED
| MAV_MODE_FLAG_GUIDED_ENABLED; | MAV_MODE_FLAG_GUIDED_ENABLED;

6
src/modules/navigator/loiter.cpp
View File

@ -59,7 +59,7 @@ Loiter::Loiter(Navigator *navigator, const char *name) :
/* load initial params */ /* load initial params */
updateParams(); updateParams();
/* initial reset */ /* initial reset */
reset(); on_inactive();
} }
Loiter::~Loiter() Loiter::~Loiter()
@ -67,14 +67,14 @@ Loiter::~Loiter()
} }
bool 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 */ /* set loiter item, don't reuse an existing position setpoint */
return set_loiter_item(false, pos_sp_triplet);; return set_loiter_item(false, pos_sp_triplet);;
} }
void void
Loiter::reset() Loiter::on_inactive()
{ {
} }

4
src/modules/navigator/loiter.h
View File

@ -63,12 +63,12 @@ public:
/** /**
* This function is called while the mode is inactive * 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 * This function is called while the mode is active
*/ */
void reset(); virtual bool on_active(struct position_setpoint_triplet_s *pos_sp_triplet);
}; };
#endif #endif

6
src/modules/navigator/mission.cpp
View File

@ -72,7 +72,7 @@ Mission::Mission(Navigator *navigator, const char *name) :
/* load initial params */ /* load initial params */
updateParams(); updateParams();
/* set initial mission items */ /* set initial mission items */
reset(); on_inactive();
} }
@ -81,7 +81,7 @@ Mission::~Mission()
} }
void void
Mission::reset() Mission::on_inactive()
{ {
_first_run = true; _first_run = true;
@ -100,7 +100,7 @@ Mission::reset()
} }
bool 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; bool updated = false;

4
src/modules/navigator/mission.h
View File

@ -78,12 +78,12 @@ public:
/** /**
* This function is called while the mode is inactive * 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 * 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: private:
/** /**

16
src/modules/navigator/navigator_main.cpp
View File

@ -351,8 +351,8 @@ Navigator::task_main()
_navigation_mode = &_loiter; _navigation_mode = &_loiter;
break; break;
case NAVIGATION_STATE_AUTO_RTL: case NAVIGATION_STATE_AUTO_RTL:
case NAVIGATION_STATE_AUTO_RTL_RC: case NAVIGATION_STATE_AUTO_FAILSAFE_RC_LOSS:
case NAVIGATION_STATE_AUTO_RTL_DL: case NAVIGATION_STATE_AUTO_FAILSAFE_DL_LOSS:
_navigation_mode = &_rtl; _navigation_mode = &_rtl;
break; break;
case NAVIGATION_STATE_LAND: case NAVIGATION_STATE_LAND:
@ -365,21 +365,21 @@ Navigator::task_main()
/* TODO: make list of modes and loop through it */ /* TODO: make list of modes and loop through it */
if (_navigation_mode == &_mission) { if (_navigation_mode == &_mission) {
_update_triplet = _mission.update(&_pos_sp_triplet); _update_triplet = _mission.on_active(&_pos_sp_triplet);
} else { } else {
_mission.reset(); _mission.on_inactive();
} }
if (_navigation_mode == &_rtl) { if (_navigation_mode == &_rtl) {
_update_triplet = _rtl.update(&_pos_sp_triplet); _update_triplet = _rtl.on_active(&_pos_sp_triplet);
} else { } else {
_rtl.reset(); _rtl.on_inactive();
} }
if (_navigation_mode == &_loiter) { if (_navigation_mode == &_loiter) {
_update_triplet = _loiter.update(&_pos_sp_triplet); _update_triplet = _loiter.on_active(&_pos_sp_triplet);
} else { } else {
_loiter.reset(); _loiter.on_inactive();
} }
/* if nothing is running, set position setpoint triplet invalid */ /* if nothing is running, set position setpoint triplet invalid */

6
src/modules/navigator/navigator_mode.cpp
View File

@ -48,7 +48,7 @@ NavigatorMode::NavigatorMode(Navigator *navigator, const char *name) :
/* load initial params */ /* load initial params */
updateParams(); updateParams();
/* set initial mission items */ /* set initial mission items */
reset(); on_inactive();
} }
NavigatorMode::~NavigatorMode() NavigatorMode::~NavigatorMode()
@ -56,13 +56,13 @@ NavigatorMode::~NavigatorMode()
} }
void void
NavigatorMode::reset() NavigatorMode::on_inactive()
{ {
_first_run = true; _first_run = true;
} }
bool 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; pos_sp_triplet->current.valid = false;
_first_run = false; _first_run = false;

4
src/modules/navigator/navigator_mode.h
View File

@ -68,7 +68,7 @@ public:
/** /**
* This function is called while the mode is inactive * 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 * This function is called while the mode is active
@ -76,7 +76,7 @@ public:
* @param position setpoint triplet to set * @param position setpoint triplet to set
* @return true if position setpoint triplet has been changed * @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: protected:
Navigator *_navigator; Navigator *_navigator;

6
src/modules/navigator/rtl.cpp
View File

@ -64,7 +64,7 @@ RTL::RTL(Navigator *navigator, const char *name) :
/* load initial params */ /* load initial params */
updateParams(); updateParams();
/* initial reset */ /* initial reset */
reset(); on_inactive();
} }
RTL::~RTL() RTL::~RTL()
@ -72,14 +72,14 @@ RTL::~RTL()
} }
void void
RTL::reset() RTL::on_inactive()
{ {
_first_run = true; _first_run = true;
_rtl_state = RTL_STATE_NONE; _rtl_state = RTL_STATE_NONE;
} }
bool 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; bool updated = false;

4
src/modules/navigator/rtl.h
View File

@ -70,7 +70,7 @@ public:
/** /**
* This function is called while the mode is inactive * This function is called while the mode is inactive
*/ */
void reset(); void on_inactive();
/** /**
* This function is called while the mode is active * This function is called while the mode is active
@ -78,7 +78,7 @@ public:
* @param position setpoint triplet that needs to be set * @param position setpoint triplet that needs to be set
* @return true if updated * @return true if updated
*/ */
bool update(position_setpoint_triplet_s *pos_sp_triplet); bool on_active(position_setpoint_triplet_s *pos_sp_triplet);
private: private:

37
src/modules/uORB/topics/vehicle_status.h
View File

@ -1,10 +1,6 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (C) 2012 PX4 Development Team. All rights reserved. * Copyright (C) 2012 - 2014 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>
* *
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@ -45,6 +41,11 @@
* All apps should write to subsystem_info: * All apps should write to subsystem_info:
* *
* (any app) --> subsystem_info (published) --> (commander app state machine) --> vehicle_status --> (mavlink app) * (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_ #ifndef VEHICLE_STATUS_H_
@ -90,25 +91,25 @@ typedef enum {
typedef enum { typedef enum {
FAILSAFE_STATE_NORMAL = 0, /**< Normal operation */ FAILSAFE_STATE_NORMAL = 0, /**< Normal operation */
FAILSAFE_STATE_RTL_RC, /**< Return To Launch on remote control loss */ FAILSAFE_STATE_RC_LOSS, /**< Return To Launch on remote control loss */
FAILSAFE_STATE_RTL_DL, /**< Return To Launch on datalink loss */ FAILSAFE_STATE_DL_LOSS, /**< Return To Launch on datalink loss */
FAILSAFE_STATE_LAND, /**< Land as safe as possible */ FAILSAFE_STATE_LAND, /**< Land as safe as possible */
FAILSAFE_STATE_TERMINATION, /**< Disable motors and use parachute, can't be recovered */ FAILSAFE_STATE_TERMINATION, /**< Disable motors and use parachute, can't be recovered */
FAILSAFE_STATE_MAX, FAILSAFE_STATE_MAX,
} failsafe_state_t; } failsafe_state_t;
typedef enum { typedef enum {
NAVIGATION_STATE_MANUAL = 0, NAVIGATION_STATE_MANUAL = 0, /**< Manual mode */
NAVIGATION_STATE_ACRO, NAVIGATION_STATE_ACRO, /**< Acro mode */
NAVIGATION_STATE_ALTCTL, NAVIGATION_STATE_ALTCTL, /**< Altitude control mode */
NAVIGATION_STATE_POSCTL, NAVIGATION_STATE_POSCTL, /**< Position control mode */
NAVIGATION_STATE_AUTO_MISSION, NAVIGATION_STATE_AUTO_MISSION, /**< Auto mission mode */
NAVIGATION_STATE_AUTO_LOITER, NAVIGATION_STATE_AUTO_LOITER, /**< Auto loiter mode */
NAVIGATION_STATE_AUTO_RTL, NAVIGATION_STATE_AUTO_RTL, /**< Auto RTL mode */
NAVIGATION_STATE_AUTO_RTL_RC, NAVIGATION_STATE_AUTO_FAILSAFE_RC_LOSS, /**< Auto failsafe on RC loss mode */
NAVIGATION_STATE_AUTO_RTL_DL, NAVIGATION_STATE_AUTO_FAILSAFE_DL_LOSS, /**< Auto failsafe on datalink loss mode */
NAVIGATION_STATE_LAND, NAVIGATION_STATE_LAND, /**< Land mode */
NAVIGATION_STATE_TERMINATION, NAVIGATION_STATE_TERMINATION, /**< Termination mode */
} navigation_state_t; } navigation_state_t;
enum VEHICLE_MODE_FLAG { enum VEHICLE_MODE_FLAG {