fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Commander: Support single channeel flight mode selection

Browse Source
This commit is contained in:
Lorenz Meier 2016-03-04 11:47:33 +01:00
parent 1c21a63101
commit ffd24e8cf0
2 changed files with 121 additions and 88 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -202,6 +202,7 @@ static struct safety_s safety;
static struct vehicle_control_mode_s control_mode;
static struct offboard_control_mode_s offboard_control_mode;
static struct home_position_s _home;
static int32_t _flight_mode_slots[manual_control_setpoint_s::MODE_SLOT_MAX - 1];
static unsigned _last_mission_instance = 0;
static manual_control_setpoint_s _last_sp_man;
@ -1091,6 +1092,13 @@ int commander_thread_main(int argc, char *argv[])
param_t _param_disarm_land = param_find("COM_DISARM_LAND");
param_t _param_map_mode_sw = param_find("RC_MAP_MODE_SW");
param_t _param_fmode_1 = param_find("COM_FLTMODE1");
param_t _param_fmode_2 = param_find("COM_FLTMODE2");
param_t _param_fmode_3 = param_find("COM_FLTMODE3");
param_t _param_fmode_4 = param_find("COM_FLTMODE4");
param_t _param_fmode_5 = param_find("COM_FLTMODE5");
param_t _param_fmode_6 = param_find("COM_FLTMODE6");
// These are too verbose, but we will retain them a little longer
// until we are sure we really don't need them.
@ -1553,6 +1561,14 @@ int commander_thread_main(int argc, char *argv[])
param_get(_param_eph, &eph_threshold);
param_get(_param_epv, &epv_threshold);
/* flight mode slots */
param_get(_param_fmode_1, &_flight_mode_slots[0]);
param_get(_param_fmode_2, &_flight_mode_slots[1]);
param_get(_param_fmode_3, &_flight_mode_slots[2]);
param_get(_param_fmode_4, &_flight_mode_slots[3]);
param_get(_param_fmode_5, &_flight_mode_slots[4]);
param_get(_param_fmode_6, &_flight_mode_slots[5]);
/* Set flag to autosave parameters if necessary */
if (updated && autosave_params != 0 && param_changed.saved == false) {
/* trigger an autosave */
@ -2840,7 +2856,8 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
(_last_sp_man.acro_switch == sp_man->acro_switch) &&
(_last_sp_man.rattitude_switch == sp_man->rattitude_switch) &&
(_last_sp_man.posctl_switch == sp_man->posctl_switch) &&
(_last_sp_man.loiter_switch == sp_man->loiter_switch))) {
(_last_sp_man.loiter_switch == sp_man->loiter_switch) &&
(_last_sp_man.mode_slot == sp_man->mode_slot))) {
// update these fields for the geofence system
_last_sp_man.timestamp = sp_man->timestamp;
@ -2888,6 +2905,25 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
/* if we get here mode was rejected, continue to evaluate the main system mode */
}
/* we know something has changed - check if we are in mode slot operation */
if (_last_sp_man.mode_slot > 0) {
if (_last_sp_man.mode_slot >= sizeof(_flight_mode_slots) / sizeof(_flight_mode_slots[0])) {
return TRANSITION_DENIED;
}
int new_mode = _flight_mode_slots[_last_sp_man.mode_slot];
if (new_mode < 0) {
/* slot is unused */
res = TRANSITION_NOT_CHANGED;
} else {
res = main_state_transition(status_local, new_mode);
}
return res;
}
/* offboard and RTL switches off or denied, check main mode switch */
switch (sp_man->mode_switch) {
case manual_control_setpoint_s::SWITCH_POS_NONE:

171
src/modules/commander/commander_params.c
View File

@ -346,30 +346,27 @@ PARAM_DEFINE_INT32(COM_RC_IN_MODE, 0);
*/
PARAM_DEFINE_INT32(COM_DISARM_LAND, 0);
PARAM_DEFINE_INT32(COM_FLTMODE_CH, 0);
/**
* First flightmode slot (1000-1160)
*
* If the main switch channel is in this range the
* selected flight mode will be applied.
*
* @value 0 Unassigned
* @value 1 MANUAL
* @value 2 ALTITUDE CONTROL
* @value 3 POSITION CONTROL
* @value 4 AUTO / MISSION
* @value 5 AUTO / PAUSE
* @value 11 AUTO / TAKEOFF
* @value 12 AUTO / LAND
* @value 6 RETURN TO LAUNCH
* @value 7 ACRO
* @value 8 OFFBOARD
* @value 9 STABILIZED
* @value 10 RATTITUDE
* @value -1 Unassigned
* @value 0 MANUAL
* @value 1 ALTITUDE CONTROL
* @value 2 POSITION CONTROL
* @value 3 AUTO / MISSION
* @value 4 AUTO / PAUSE
* @value 10 AUTO / TAKEOFF
* @value 11 AUTO / LAND
* @value 5 RETURN TO LAUNCH
* @value 6 ACRO
* @value 7 OFFBOARD
* @value 8 STABILIZED
* @value 9 RATTITUDE
*/
PARAM_DEFINE_INT32(COM_FLTMODE1, 0);
PARAM_DEFINE_INT32(COM_FLTMODE1, -1);
/**
* Second flightmode slot (1160-1320)
@ -377,21 +374,21 @@ PARAM_DEFINE_INT32(COM_FLTMODE1, 0);
* If the main switch channel is in this range the
* selected flight mode will be applied.
*
* @value 0 Unassigned
* @value 1 MANUAL
* @value 2 ALTITUDE CONTROL
* @value 3 POSITION CONTROL
* @value 4 AUTO / MISSION
* @value 5 AUTO / PAUSE
* @value 11 AUTO / TAKEOFF
* @value 12 AUTO / LAND
* @value 6 RETURN TO LAUNCH
* @value 7 ACRO
* @value 8 OFFBOARD
* @value 9 STABILIZED
* @value 10 RATTITUDE
* @value -1 Unassigned
* @value 0 MANUAL
* @value 1 ALTITUDE CONTROL
* @value 2 POSITION CONTROL
* @value 3 AUTO / MISSION
* @value 4 AUTO / PAUSE
* @value 10 AUTO / TAKEOFF
* @value 11 AUTO / LAND
* @value 5 RETURN TO LAUNCH
* @value 6 ACRO
* @value 7 OFFBOARD
* @value 8 STABILIZED
* @value 9 RATTITUDE
*/
PARAM_DEFINE_INT32(COM_FLTMODE2, 0);
PARAM_DEFINE_INT32(COM_FLTMODE2, -1);
/**
* Third flightmode slot (1320-1480)
@ -399,21 +396,21 @@ PARAM_DEFINE_INT32(COM_FLTMODE2, 0);
* If the main switch channel is in this range the
* selected flight mode will be applied.
*
* @value 0 Unassigned
* @value 1 MANUAL
* @value 2 ALTITUDE CONTROL
* @value 3 POSITION CONTROL
* @value 4 AUTO / MISSION
* @value 5 AUTO / PAUSE
* @value 11 AUTO / TAKEOFF
* @value 12 AUTO / LAND
* @value 6 RETURN TO LAUNCH
* @value 7 ACRO
* @value 8 OFFBOARD
* @value 9 STABILIZED
* @value 10 RATTITUDE
* @value -1 Unassigned
* @value 0 MANUAL
* @value 1 ALTITUDE CONTROL
* @value 2 POSITION CONTROL
* @value 3 AUTO / MISSION
* @value 4 AUTO / PAUSE
* @value 10 AUTO / TAKEOFF
* @value 11 AUTO / LAND
* @value 5 RETURN TO LAUNCH
* @value 6 ACRO
* @value 7 OFFBOARD
* @value 8 STABILIZED
* @value 9 RATTITUDE
*/
PARAM_DEFINE_INT32(COM_FLTMODE3, 0);
PARAM_DEFINE_INT32(COM_FLTMODE3, -1);
/**
* Fourth flightmode slot (1480-1640)
@ -421,21 +418,21 @@ PARAM_DEFINE_INT32(COM_FLTMODE3, 0);
* If the main switch channel is in this range the
* selected flight mode will be applied.
*
* @value 0 Unassigned
* @value 1 MANUAL
* @value 2 ALTITUDE CONTROL
* @value 3 POSITION CONTROL
* @value 4 AUTO / MISSION
* @value 5 AUTO / PAUSE
* @value 11 AUTO / TAKEOFF
* @value 12 AUTO / LAND
* @value 6 RETURN TO LAUNCH
* @value 7 ACRO
* @value 8 OFFBOARD
* @value 9 STABILIZED
* @value 10 RATTITUDE
* @value -1 Unassigned
* @value 0 MANUAL
* @value 1 ALTITUDE CONTROL
* @value 2 POSITION CONTROL
* @value 3 AUTO / MISSION
* @value 4 AUTO / PAUSE
* @value 10 AUTO / TAKEOFF
* @value 11 AUTO / LAND
* @value 5 RETURN TO LAUNCH
* @value 6 ACRO
* @value 7 OFFBOARD
* @value 8 STABILIZED
* @value 9 RATTITUDE
*/
PARAM_DEFINE_INT32(COM_FLTMODE4, 0);
PARAM_DEFINE_INT32(COM_FLTMODE4, -1);
/**
* Fift flightmode slot (1640-1800)
@ -443,21 +440,21 @@ PARAM_DEFINE_INT32(COM_FLTMODE4, 0);
* If the main switch channel is in this range the
* selected flight mode will be applied.
*
* @value 0 Unassigned
* @value 1 MANUAL
* @value 2 ALTITUDE CONTROL
* @value 3 POSITION CONTROL
* @value 4 AUTO / MISSION
* @value 5 AUTO / PAUSE
* @value 11 AUTO / TAKEOFF
* @value 12 AUTO / LAND
* @value 6 RETURN TO LAUNCH
* @value 7 ACRO
* @value 8 OFFBOARD
* @value 9 STABILIZED
* @value 10 RATTITUDE
* @value -1 Unassigned
* @value 0 MANUAL
* @value 1 ALTITUDE CONTROL
* @value 2 POSITION CONTROL
* @value 3 AUTO / MISSION
* @value 4 AUTO / PAUSE
* @value 10 AUTO / TAKEOFF
* @value 11 AUTO / LAND
* @value 5 RETURN TO LAUNCH
* @value 6 ACRO
* @value 7 OFFBOARD
* @value 8 STABILIZED
* @value 9 RATTITUDE
*/
PARAM_DEFINE_INT32(COM_FLTMODE5, 0);
PARAM_DEFINE_INT32(COM_FLTMODE5, -1);
/**
* Sixt flightmode slot (1800-2000)
@ -465,18 +462,18 @@ PARAM_DEFINE_INT32(COM_FLTMODE5, 0);
* If the main switch channel is in this range the
* selected flight mode will be applied.
*
* @value 0 Unassigned
* @value 1 MANUAL
* @value 2 ALTITUDE CONTROL
* @value 3 POSITION CONTROL
* @value 4 AUTO / MISSION
* @value 5 AUTO / PAUSE
* @value 11 AUTO / TAKEOFF
* @value 12 AUTO / LAND
* @value 6 RETURN TO LAUNCH
* @value 7 ACRO
* @value 8 OFFBOARD
* @value 9 STABILIZED
* @value 10 RATTITUDE
* @value -1 Unassigned
* @value 0 MANUAL
* @value 1 ALTITUDE CONTROL
* @value 2 POSITION CONTROL
* @value 3 AUTO / MISSION
* @value 4 AUTO / PAUSE
* @value 10 AUTO / TAKEOFF
* @value 11 AUTO / LAND
* @value 5 RETURN TO LAUNCH
* @value 6 ACRO
* @value 7 OFFBOARD
* @value 8 STABILIZED
* @value 9 RATTITUDE
*/
PARAM_DEFINE_INT32(COM_FLTMODE6, 0);
PARAM_DEFINE_INT32(COM_FLTMODE6, -1);