zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Commander: Support single channeel flight mode selection

sbg
Lorenz Meier 9 years ago
parent
1c21a63101
commit
ffd24e8cf0
2 changed files with 121 additions and 88 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 38
      src/modules/commander/commander.cpp
  2. 171
      src/modules/commander/commander_params.c

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

@ -202,6 +202,7 @@ static struct safety_s safety; @@ -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[]) @@ -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[]) @@ -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 @@ -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 @@ -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
Unescape View File

@ -346,30 +346,27 @@ PARAM_DEFINE_INT32(COM_RC_IN_MODE, 0); @@ -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); @@ -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); @@ -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); @@ -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); @@ -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); @@ -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);

Write Preview
Loading…
Cancel
Save