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 (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;

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

@ -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:

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
* @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

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;
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;

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

@ -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()
{
}

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

@ -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

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

@ -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;

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

@ -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:
/**

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

@ -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 */

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

@ -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;

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

@ -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;

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

@ -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;

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

@ -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:

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

@ -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 {