fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-27 17:33:35 +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-05 20:41:02 +01:00
parent 1c21a63101
commit ffd24e8cf0
2 changed files with 121 additions and 88 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+37 -1
View File
@@ -202,6 +202,7 @@ static struct safety_s safety;
static struct vehicle_control_mode_s control_mode; static struct vehicle_control_mode_s control_mode;
static struct offboard_control_mode_s offboard_control_mode; static struct offboard_control_mode_s offboard_control_mode;
static struct home_position_s _home; 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 unsigned _last_mission_instance = 0;
static manual_control_setpoint_s _last_sp_man; 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_disarm_land = param_find("COM_DISARM_LAND");
param_t _param_map_mode_sw = param_find("RC_MAP_MODE_SW"); 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 // These are too verbose, but we will retain them a little longer
// until we are sure we really don't need them. // 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_eph, &eph_threshold);
param_get(_param_epv, &epv_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 */ /* Set flag to autosave parameters if necessary */
if (updated && autosave_params != 0 && param_changed.saved == false) { if (updated && autosave_params != 0 && param_changed.saved == false) {
/* trigger an autosave */ /* 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.acro_switch == sp_man->acro_switch) &&
(_last_sp_man.rattitude_switch == sp_man->rattitude_switch) && (_last_sp_man.rattitude_switch == sp_man->rattitude_switch) &&
(_last_sp_man.posctl_switch == sp_man->posctl_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 // update these fields for the geofence system
_last_sp_man.timestamp = sp_man->timestamp; _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 */ /* 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 */ /* offboard and RTL switches off or denied, check main mode switch */
switch (sp_man->mode_switch) { switch (sp_man->mode_switch) {
case manual_control_setpoint_s::SWITCH_POS_NONE: case manual_control_setpoint_s::SWITCH_POS_NONE:
src/modules/commander/commander_params.c
+84 -87
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_DISARM_LAND, 0);
PARAM_DEFINE_INT32(COM_FLTMODE_CH, 0);
/** /**
* First flightmode slot (1000-1160) * First flightmode slot (1000-1160)
* *
* If the main switch channel is in this range the * If the main switch channel is in this range the
* selected flight mode will be applied. * selected flight mode will be applied.
* *
* @value 0 Unassigned * @value -1 Unassigned
* @value 1 MANUAL * @value 0 MANUAL
* @value 2 ALTITUDE CONTROL * @value 1 ALTITUDE CONTROL
* @value 3 POSITION CONTROL * @value 2 POSITION CONTROL
* @value 4 AUTO / MISSION * @value 3 AUTO / MISSION
* @value 5 AUTO / PAUSE * @value 4 AUTO / PAUSE
* @value 11 AUTO / TAKEOFF * @value 10 AUTO / TAKEOFF
* @value 12 AUTO / LAND * @value 11 AUTO / LAND
* @value 6 RETURN TO LAUNCH * @value 5 RETURN TO LAUNCH
* @value 7 ACRO * @value 6 ACRO
* @value 8 OFFBOARD * @value 7 OFFBOARD
* @value 9 STABILIZED * @value 8 STABILIZED
* @value 10 RATTITUDE * @value 9 RATTITUDE
*/ */
PARAM_DEFINE_INT32(COM_FLTMODE1, 0); PARAM_DEFINE_INT32(COM_FLTMODE1, -1);
/** /**
* Second flightmode slot (1160-1320) * 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 * If the main switch channel is in this range the
* selected flight mode will be applied. * selected flight mode will be applied.
* *
* @value 0 Unassigned * @value -1 Unassigned
* @value 1 MANUAL * @value 0 MANUAL
* @value 2 ALTITUDE CONTROL * @value 1 ALTITUDE CONTROL
* @value 3 POSITION CONTROL * @value 2 POSITION CONTROL
* @value 4 AUTO / MISSION * @value 3 AUTO / MISSION
* @value 5 AUTO / PAUSE * @value 4 AUTO / PAUSE
* @value 11 AUTO / TAKEOFF * @value 10 AUTO / TAKEOFF
* @value 12 AUTO / LAND * @value 11 AUTO / LAND
* @value 6 RETURN TO LAUNCH * @value 5 RETURN TO LAUNCH
* @value 7 ACRO * @value 6 ACRO
* @value 8 OFFBOARD * @value 7 OFFBOARD
* @value 9 STABILIZED * @value 8 STABILIZED
* @value 10 RATTITUDE * @value 9 RATTITUDE
*/ */
PARAM_DEFINE_INT32(COM_FLTMODE2, 0); PARAM_DEFINE_INT32(COM_FLTMODE2, -1);
/** /**
* Third flightmode slot (1320-1480) * 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 * If the main switch channel is in this range the
* selected flight mode will be applied. * selected flight mode will be applied.
* *
* @value 0 Unassigned * @value -1 Unassigned
* @value 1 MANUAL * @value 0 MANUAL
* @value 2 ALTITUDE CONTROL * @value 1 ALTITUDE CONTROL
* @value 3 POSITION CONTROL * @value 2 POSITION CONTROL
* @value 4 AUTO / MISSION * @value 3 AUTO / MISSION
* @value 5 AUTO / PAUSE * @value 4 AUTO / PAUSE
* @value 11 AUTO / TAKEOFF * @value 10 AUTO / TAKEOFF
* @value 12 AUTO / LAND * @value 11 AUTO / LAND
* @value 6 RETURN TO LAUNCH * @value 5 RETURN TO LAUNCH
* @value 7 ACRO * @value 6 ACRO
* @value 8 OFFBOARD * @value 7 OFFBOARD
* @value 9 STABILIZED * @value 8 STABILIZED
* @value 10 RATTITUDE * @value 9 RATTITUDE
*/ */
PARAM_DEFINE_INT32(COM_FLTMODE3, 0); PARAM_DEFINE_INT32(COM_FLTMODE3, -1);
/** /**
* Fourth flightmode slot (1480-1640) * 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 * If the main switch channel is in this range the
* selected flight mode will be applied. * selected flight mode will be applied.
* *
* @value 0 Unassigned * @value -1 Unassigned
* @value 1 MANUAL * @value 0 MANUAL
* @value 2 ALTITUDE CONTROL * @value 1 ALTITUDE CONTROL
* @value 3 POSITION CONTROL * @value 2 POSITION CONTROL
* @value 4 AUTO / MISSION * @value 3 AUTO / MISSION
* @value 5 AUTO / PAUSE * @value 4 AUTO / PAUSE
* @value 11 AUTO / TAKEOFF * @value 10 AUTO / TAKEOFF
* @value 12 AUTO / LAND * @value 11 AUTO / LAND
* @value 6 RETURN TO LAUNCH * @value 5 RETURN TO LAUNCH
* @value 7 ACRO * @value 6 ACRO
* @value 8 OFFBOARD * @value 7 OFFBOARD
* @value 9 STABILIZED * @value 8 STABILIZED
* @value 10 RATTITUDE * @value 9 RATTITUDE
*/ */
PARAM_DEFINE_INT32(COM_FLTMODE4, 0); PARAM_DEFINE_INT32(COM_FLTMODE4, -1);
/** /**
* Fift flightmode slot (1640-1800) * 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 * If the main switch channel is in this range the
* selected flight mode will be applied. * selected flight mode will be applied.
* *
* @value 0 Unassigned * @value -1 Unassigned
* @value 1 MANUAL * @value 0 MANUAL
* @value 2 ALTITUDE CONTROL * @value 1 ALTITUDE CONTROL
* @value 3 POSITION CONTROL * @value 2 POSITION CONTROL
* @value 4 AUTO / MISSION * @value 3 AUTO / MISSION
* @value 5 AUTO / PAUSE * @value 4 AUTO / PAUSE
* @value 11 AUTO / TAKEOFF * @value 10 AUTO / TAKEOFF
* @value 12 AUTO / LAND * @value 11 AUTO / LAND
* @value 6 RETURN TO LAUNCH * @value 5 RETURN TO LAUNCH
* @value 7 ACRO * @value 6 ACRO
* @value 8 OFFBOARD * @value 7 OFFBOARD
* @value 9 STABILIZED * @value 8 STABILIZED
* @value 10 RATTITUDE * @value 9 RATTITUDE
*/ */
PARAM_DEFINE_INT32(COM_FLTMODE5, 0); PARAM_DEFINE_INT32(COM_FLTMODE5, -1);
/** /**
* Sixt flightmode slot (1800-2000) * 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 * If the main switch channel is in this range the
* selected flight mode will be applied. * selected flight mode will be applied.
* *
* @value 0 Unassigned * @value -1 Unassigned
* @value 1 MANUAL * @value 0 MANUAL
* @value 2 ALTITUDE CONTROL * @value 1 ALTITUDE CONTROL
* @value 3 POSITION CONTROL * @value 2 POSITION CONTROL
* @value 4 AUTO / MISSION * @value 3 AUTO / MISSION
* @value 5 AUTO / PAUSE * @value 4 AUTO / PAUSE
* @value 11 AUTO / TAKEOFF * @value 10 AUTO / TAKEOFF
* @value 12 AUTO / LAND * @value 11 AUTO / LAND
* @value 6 RETURN TO LAUNCH * @value 5 RETURN TO LAUNCH
* @value 7 ACRO * @value 6 ACRO
* @value 8 OFFBOARD * @value 7 OFFBOARD
* @value 9 STABILIZED * @value 8 STABILIZED
* @value 10 RATTITUDE * @value 9 RATTITUDE
*/ */
PARAM_DEFINE_INT32(COM_FLTMODE6, 0); PARAM_DEFINE_INT32(COM_FLTMODE6, -1);