fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-30 16:43:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added commander support for rattitude mode. Still need to incorporate attitude/rate switching in multicopter control

Browse Source
This commit is contained in:
Eddy Scott
2015-10-28 08:37:12 -04:00
committed by Eddy
parent 8ba0c003db
commit c4a82d78c8
7 changed files with 96 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sensors/sensor_params.c
+26
View File
@@ -1969,6 +1969,15 @@ PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 0);
*/
PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0);
/**
* Rattitude switch channel mapping.
*
* @min 0
* @max 18
* @group Radio Switches
*/
PARAM_DEFINE_INT32(RC_MAP_RATT_SW, 0);
/**
* Posctl switch channel mapping.
*
@@ -2132,6 +2141,23 @@ PARAM_DEFINE_FLOAT(RC_ASSIST_TH, 0.25f);
*/
PARAM_DEFINE_FLOAT(RC_AUTO_TH, 0.75f);
/**
* Threshold for selecting rattitude mode
*
* 0-1 indicate where in the full channel range the threshold sits
* 0 : min
* 1 : max
* sign indicates polarity of comparison
* positive : true when channel>th
* negative : true when channel<th
*
* @min -1
* @max 1
* @group Radio Switches
*
*/
PARAM_DEFINE_FLOAT(RC_RATT_TH, 0.5f);
/**
* Threshold for selecting posctl mode
*
src/modules/sensors/sensors.cpp
+17
View File
@@ -269,6 +269,7 @@ private:
int rc_map_mode_sw;
int rc_map_return_sw;
int rc_map_rattitude_sw;
int rc_map_posctl_sw;
int rc_map_loiter_sw;
int rc_map_acro_sw;
@@ -287,6 +288,7 @@ private:
int32_t rc_fails_thr;
float rc_assist_th;
float rc_auto_th;
float rc_rattitude_th;
float rc_posctl_th;
float rc_return_th;
float rc_loiter_th;
@@ -294,6 +296,7 @@ private:
float rc_offboard_th;
bool rc_assist_inv;
bool rc_auto_inv;
bool rc_rattitude_inv;
bool rc_posctl_inv;
bool rc_return_inv;
bool rc_loiter_inv;
@@ -325,6 +328,7 @@ private:
param_t rc_map_mode_sw;
param_t rc_map_return_sw;
param_t rc_map_rattitude_sw;
param_t rc_map_posctl_sw;
param_t rc_map_loiter_sw;
param_t rc_map_acro_sw;
@@ -347,6 +351,7 @@ private:
param_t rc_fails_thr;
param_t rc_assist_th;
param_t rc_auto_th;
param_t rc_rattitude_th;
param_t rc_posctl_th;
param_t rc_return_th;
param_t rc_loiter_th;
@@ -575,6 +580,7 @@ Sensors::Sensors() :
_parameter_handles.rc_map_flaps = param_find("RC_MAP_FLAPS");
/* optional mode switches, not mapped per default */
_parameter_handles.rc_map_rattitude_sw = param_find("RC_MAP_RATT_SW");
_parameter_handles.rc_map_posctl_sw = param_find("RC_MAP_POSCTL_SW");
_parameter_handles.rc_map_loiter_sw = param_find("RC_MAP_LOITER_SW");
_parameter_handles.rc_map_acro_sw = param_find("RC_MAP_ACRO_SW");
@@ -598,6 +604,7 @@ Sensors::Sensors() :
_parameter_handles.rc_fails_thr = param_find("RC_FAILS_THR");
_parameter_handles.rc_assist_th = param_find("RC_ASSIST_TH");
_parameter_handles.rc_auto_th = param_find("RC_AUTO_TH");
_parameter_handles.rc_rattitude_th = param_find("RC_RATT_TH");
_parameter_handles.rc_posctl_th = param_find("RC_POSCTL_TH");
_parameter_handles.rc_return_th = param_find("RC_RETURN_TH");
_parameter_handles.rc_loiter_th = param_find("RC_LOITER_TH");
@@ -742,6 +749,10 @@ Sensors::parameters_update()
warnx("%s", paramerr);
}
if (param_get(_parameter_handles.rc_map_rattitude_sw, &(_parameters.rc_map_rattitude_sw)) != OK) {
warnx("%s", paramerr);
}
if (param_get(_parameter_handles.rc_map_posctl_sw, &(_parameters.rc_map_posctl_sw)) != OK) {
warnx("%s", paramerr);
}
@@ -779,6 +790,9 @@ Sensors::parameters_update()
param_get(_parameter_handles.rc_auto_th, &(_parameters.rc_auto_th));
_parameters.rc_auto_inv = (_parameters.rc_auto_th < 0);
_parameters.rc_auto_th = fabs(_parameters.rc_auto_th);
param_get(_parameter_handles.rc_rattitude_th, &(_parameters.rc_rattitude_th));
_parameters.rc_rattitude_inv = (_parameters.rc_rattitude_th < 0);
_parameters.rc_rattitude_th = fabs(_parameters.rc_rattitude_th);
param_get(_parameter_handles.rc_posctl_th, &(_parameters.rc_posctl_th));
_parameters.rc_posctl_inv = (_parameters.rc_posctl_th < 0);
_parameters.rc_posctl_th = fabs(_parameters.rc_posctl_th);
@@ -803,6 +817,7 @@ Sensors::parameters_update()
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_MODE] = _parameters.rc_map_mode_sw - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_RETURN] = _parameters.rc_map_return_sw - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_RATTITUDE] = _parameters.rc_map_rattitude_sw -1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_POSCTL] = _parameters.rc_map_posctl_sw - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_LOITER] = _parameters.rc_map_loiter_sw - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_ACRO] = _parameters.rc_map_acro_sw - 1;
@@ -1928,6 +1943,8 @@ Sensors::rc_poll()
/* mode switches */
manual.mode_switch = get_rc_sw3pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_MODE, _parameters.rc_auto_th,
_parameters.rc_auto_inv, _parameters.rc_assist_th, _parameters.rc_assist_inv);
manual.rattitude_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_RATTITUDE, _parameters.rc_rattitude_th,
_parameters.rc_rattitude_inv);
manual.posctl_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_POSCTL, _parameters.rc_posctl_th,
_parameters.rc_posctl_inv);
manual.return_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_RETURN, _parameters.rc_return_th,