From 1dc9785083e1ed7a7db27466fbe9f61f6bb277f7 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 22 Jan 2014 16:52:35 +0100 Subject: OFFBOARD switch added, WIP --- src/modules/commander/commander.cpp | 11 +++++++++ src/modules/sensors/sensor_params.c | 3 +-- src/modules/sensors/sensors.cpp | 29 ++++++++++------------- src/modules/uORB/topics/manual_control_setpoint.h | 4 +--- src/modules/uORB/topics/vehicle_status.h | 8 +++++++ 5 files changed, 34 insertions(+), 21 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 47053838c..e2b6ad1ca 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1410,6 +1410,17 @@ check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_sta } else { current_status->mission_switch = MISSION_SWITCH_MISSION; } + + /* offboard switch */ + if (!isfinite(sp_man->offboard_switch)) { + current_status->offboard_switch = OFFBOARD_SWITCH_NONE; + + } else if (sp_man->offboard_switch > STICK_ON_OFF_LIMIT) { + current_status->offboard_switch = OFFBOARD_SWITCH_OFFBOARD; + + } else { + current_status->offboard_switch = OFFBOARD_SWITCH_ONBOARD; + } } transition_result_t diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index bbc84ef93..eb771382d 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -390,8 +390,7 @@ PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 5); PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0); PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 6); PARAM_DEFINE_INT32(RC_MAP_MISSIO_SW, 0); - -//PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0); +PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0); PARAM_DEFINE_INT32(RC_MAP_FLAPS, 0); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 5e8c5edbc..f87f2f282 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -246,8 +246,7 @@ private: int rc_map_return_sw; int rc_map_assisted_sw; int rc_map_mission_sw; - -// int rc_map_offboard_ctrl_mode_sw; + int rc_map_offboard_sw; int rc_map_flaps; @@ -296,8 +295,7 @@ private: param_t rc_map_return_sw; param_t rc_map_assisted_sw; param_t rc_map_mission_sw; - -// param_t rc_map_offboard_ctrl_mode_sw; + param_t rc_map_offboard_sw; param_t rc_map_flaps; @@ -515,8 +513,7 @@ Sensors::Sensors() : /* optional mode switches, not mapped per default */ _parameter_handles.rc_map_assisted_sw = param_find("RC_MAP_ASSIST_SW"); _parameter_handles.rc_map_mission_sw = param_find("RC_MAP_MISSIO_SW"); - -// _parameter_handles.rc_map_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW"); + _parameter_handles.rc_map_offboard_sw = param_find("RC_MAP_OFFB_SW"); _parameter_handles.rc_map_aux1 = param_find("RC_MAP_AUX1"); _parameter_handles.rc_map_aux2 = param_find("RC_MAP_AUX2"); @@ -669,14 +666,14 @@ Sensors::parameters_update() warnx("Failed getting mission sw chan index"); } + if (param_get(_parameter_handles.rc_map_offboard_sw, &(_parameters.rc_map_offboard_sw)) != OK) { + warnx("Failed getting offboard sw chan index"); + } + if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) { warnx("Failed getting flaps chan index"); } -// if (param_get(_parameter_handles.rc_map_offboard_ctrl_mode_sw, &(_parameters.rc_map_offboard_ctrl_mode_sw)) != OK) { -// warnx("Failed getting offboard control mode sw chan index"); -// } - param_get(_parameter_handles.rc_map_aux1, &(_parameters.rc_map_aux1)); param_get(_parameter_handles.rc_map_aux2, &(_parameters.rc_map_aux2)); param_get(_parameter_handles.rc_map_aux3, &(_parameters.rc_map_aux3)); @@ -700,11 +697,10 @@ Sensors::parameters_update() _rc.function[RETURN] = _parameters.rc_map_return_sw - 1; _rc.function[ASSISTED] = _parameters.rc_map_assisted_sw - 1; _rc.function[MISSION] = _parameters.rc_map_mission_sw - 1; + _rc.function[OFFBOARD_MODE] = _parameters.rc_map_offboard_sw - 1; _rc.function[FLAPS] = _parameters.rc_map_flaps - 1; -// _rc.function[OFFBOARD_MODE] = _parameters.rc_map_offboard_ctrl_mode_sw - 1; - _rc.function[AUX_1] = _parameters.rc_map_aux1 - 1; _rc.function[AUX_2] = _parameters.rc_map_aux2 - 1; _rc.function[AUX_3] = _parameters.rc_map_aux3 - 1; @@ -1289,7 +1285,7 @@ Sensors::rc_poll() manual_control.return_switch = NAN; manual_control.assisted_switch = NAN; manual_control.mission_switch = NAN; -// manual_control.auto_offboard_input_switch = NAN; + manual_control.offboard_switch = NAN; manual_control.flaps = NAN; manual_control.aux1 = NAN; @@ -1432,9 +1428,10 @@ Sensors::rc_poll() manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled); } -// if (_rc.function[OFFBOARD_MODE] >= 0) { -// manual_control.auto_offboard_input_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled); -// } + /* offboard switch input */ + if (_rc.function[OFFBOARD_MODE] >= 0) { + manual_control.offboard_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled); + } /* aux functions, only assign if valid mapping is present */ if (_rc.function[AUX_1] >= 0) { diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h index eac6d6e98..ab0c7a411 100644 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ b/src/modules/uORB/topics/manual_control_setpoint.h @@ -60,15 +60,13 @@ struct manual_control_setpoint_s { float return_switch; /**< land 2 position switch (mandatory): land, no effect */ float assisted_switch; /**< assisted 2 position switch (optional): seatbelt, simple */ float mission_switch; /**< mission 2 position switch (optional): mission, loiter */ + float offboard_switch; /**< offboard switch (optional): offboard, onboard */ /** * Any of the channels below may not be available and be set to NaN * to indicate that it does not contain valid data. */ - // XXX needed or parameter? - //float auto_offboard_input_switch; /**< controller setpoint source (0 = onboard, 1 = offboard) */ - float flaps; /**< flap position */ float aux1; /**< default function: camera yaw / azimuth */ diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 1a9dec5f5..d74b696bb 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -64,6 +64,7 @@ typedef enum { MAIN_STATE_SEATBELT, MAIN_STATE_EASY, MAIN_STATE_AUTO, + MAIN_STATE_OFFBOARD, } main_state_t; typedef enum { @@ -109,6 +110,12 @@ typedef enum { MISSION_SWITCH_MISSION } mission_switch_pos_t; +typedef enum { + OFFBOARD_SWITCH_NONE = 0, + OFFBOARD_SWITCH_OFFBOARD, + OFFBOARD_SWITCH_ONBOARD +} offboard_switch_pos_t; + enum VEHICLE_MODE_FLAG { VEHICLE_MODE_FLAG_SAFETY_ARMED = 128, VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64, @@ -184,6 +191,7 @@ struct vehicle_status_s return_switch_pos_t return_switch; assisted_switch_pos_t assisted_switch; mission_switch_pos_t mission_switch; + offboard_switch_pos_t offboard_switch; bool condition_battery_voltage_valid; bool condition_system_in_air_restore; /**< true if we can restore in mid air */ -- cgit v1.2.3