From ffd24e8cf03d4b77ea809b91de741c5cbba21888 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 4 Mar 2016 11:47:33 +0100 Subject: [PATCH] Commander: Support single channeel flight mode selection --- src/modules/commander/commander.cpp | 38 ++++- src/modules/commander/commander_params.c | 171 +++++++++++------------ 2 files changed, 121 insertions(+), 88 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 39b8dfdf2f..6a11587db8 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -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: diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 1f29e97763..358924ac66 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -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);