fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-28 06:03:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Cleaned up control mode state machine / flight mode / navigation state machine still needs a bit cleaning up

Browse Source
This commit is contained in:
Lorenz Meier
2012-12-27 18:27:08 +01:00
parent 61d7e1d285
commit f5bad08bd0
10 changed files with 625 additions and 300 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/sensors/sensor_params.c
+66 -15
View File
@@ -69,60 +69,100 @@ PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f);
PARAM_DEFINE_FLOAT(RC1_MAX, 2000.0f);
PARAM_DEFINE_FLOAT(RC1_REV, 1.0f);
PARAM_DEFINE_FLOAT(RC1_DZ, 0.0f);
PARAM_DEFINE_FLOAT(RC1_EXP, 0.0f);
// PARAM_DEFINE_FLOAT(RC1_EXP, 0.0f);
PARAM_DEFINE_FLOAT(RC2_MIN, 1000);
PARAM_DEFINE_FLOAT(RC2_TRIM, 1500);
PARAM_DEFINE_FLOAT(RC2_MAX, 2000);
PARAM_DEFINE_FLOAT(RC2_REV, 1.0f);
PARAM_DEFINE_FLOAT(RC2_DZ, 0.0f);
PARAM_DEFINE_FLOAT(RC2_EXP, 0.0f);
// PARAM_DEFINE_FLOAT(RC2_EXP, 0.0f);
PARAM_DEFINE_FLOAT(RC3_MIN, 1000);
PARAM_DEFINE_FLOAT(RC3_TRIM, 1500);
PARAM_DEFINE_FLOAT(RC3_MAX, 2000);
PARAM_DEFINE_FLOAT(RC3_REV, 1.0f);
PARAM_DEFINE_FLOAT(RC3_DZ, 0.0f);
PARAM_DEFINE_FLOAT(RC3_EXP, 0.0f);
// PARAM_DEFINE_FLOAT(RC3_EXP, 0.0f);
PARAM_DEFINE_FLOAT(RC4_MIN, 1000);
PARAM_DEFINE_FLOAT(RC4_TRIM, 1500);
PARAM_DEFINE_FLOAT(RC4_MAX, 2000);
PARAM_DEFINE_FLOAT(RC4_REV, 1.0f);
PARAM_DEFINE_FLOAT(RC4_DZ, 0.0f);
PARAM_DEFINE_FLOAT(RC4_EXP, 0.0f);
// PARAM_DEFINE_FLOAT(RC4_EXP, 0.0f);
PARAM_DEFINE_FLOAT(RC5_MIN, 1000);
PARAM_DEFINE_FLOAT(RC5_TRIM, 1500);
PARAM_DEFINE_FLOAT(RC5_MAX, 2000);
PARAM_DEFINE_FLOAT(RC5_REV, 1.0f);
PARAM_DEFINE_FLOAT(RC5_DZ, 0.0f);
PARAM_DEFINE_FLOAT(RC5_EXP, 0.0f);
// PARAM_DEFINE_FLOAT(RC5_EXP, 0.0f);
PARAM_DEFINE_FLOAT(RC6_MIN, 1000);
PARAM_DEFINE_FLOAT(RC6_TRIM, 1500);
PARAM_DEFINE_FLOAT(RC6_MAX, 2000);
PARAM_DEFINE_FLOAT(RC6_REV, 1.0f);
PARAM_DEFINE_FLOAT(RC6_DZ, 0.0f);
PARAM_DEFINE_FLOAT(RC6_EXP, 0.0f);
// PARAM_DEFINE_FLOAT(RC6_EXP, 0.0f);
PARAM_DEFINE_FLOAT(RC7_MIN, 1000);
PARAM_DEFINE_FLOAT(RC7_TRIM, 1500);
PARAM_DEFINE_FLOAT(RC7_MAX, 2000);
PARAM_DEFINE_FLOAT(RC7_REV, 1.0f);
PARAM_DEFINE_FLOAT(RC7_DZ, 0.0f);
PARAM_DEFINE_FLOAT(RC7_EXP, 0.0f);
// PARAM_DEFINE_FLOAT(RC7_EXP, 0.0f);
PARAM_DEFINE_FLOAT(RC8_MIN, 1000);
PARAM_DEFINE_FLOAT(RC8_TRIM, 1500);
PARAM_DEFINE_FLOAT(RC8_MAX, 2000);
PARAM_DEFINE_FLOAT(RC8_REV, 1.0f);
PARAM_DEFINE_FLOAT(RC8_DZ, 0.0f);
PARAM_DEFINE_FLOAT(RC8_EXP, 0.0f);
// PARAM_DEFINE_FLOAT(RC8_EXP, 0.0f);
PARAM_DEFINE_INT32(RC_TYPE, 1); // 1 = FUTABA
PARAM_DEFINE_FLOAT(RC9_MIN, 1000);
PARAM_DEFINE_FLOAT(RC9_TRIM, 1500);
PARAM_DEFINE_FLOAT(RC9_MAX, 2000);
PARAM_DEFINE_FLOAT(RC9_REV, 1.0f);
PARAM_DEFINE_FLOAT(RC9_DZ, 0.0f);
// PARAM_DEFINE_FLOAT(RC9_EXP, 0.0f);
PARAM_DEFINE_INT32(RC_DEMIX, 0); /**< 0 = off, 1 = auto, 2 = delta */
PARAM_DEFINE_FLOAT(RC10_MIN, 1000);
PARAM_DEFINE_FLOAT(RC10_TRIM, 1500);
PARAM_DEFINE_FLOAT(RC10_MAX, 2000);
PARAM_DEFINE_FLOAT(RC10_REV, 1.0f);
PARAM_DEFINE_FLOAT(RC10_DZ, 0.0f);
// PARAM_DEFINE_FLOAT(RC10_EXP, 0.0f);
PARAM_DEFINE_FLOAT(RC11_MIN, 1000);
PARAM_DEFINE_FLOAT(RC11_TRIM, 1500);
PARAM_DEFINE_FLOAT(RC11_MAX, 2000);
PARAM_DEFINE_FLOAT(RC11_REV, 1.0f);
PARAM_DEFINE_FLOAT(RC11_DZ, 0.0f);
// PARAM_DEFINE_FLOAT(RC11_EXP, 0.0f);
PARAM_DEFINE_FLOAT(RC12_MIN, 1000);
PARAM_DEFINE_FLOAT(RC12_TRIM, 1500);
PARAM_DEFINE_FLOAT(RC12_MAX, 2000);
PARAM_DEFINE_FLOAT(RC12_REV, 1.0f);
PARAM_DEFINE_FLOAT(RC12_DZ, 0.0f);
// PARAM_DEFINE_FLOAT(RC12_EXP, 0.0f);
PARAM_DEFINE_FLOAT(RC13_MIN, 1000);
PARAM_DEFINE_FLOAT(RC13_TRIM, 1500);
PARAM_DEFINE_FLOAT(RC13_MAX, 2000);
PARAM_DEFINE_FLOAT(RC13_REV, 1.0f);
PARAM_DEFINE_FLOAT(RC13_DZ, 0.0f);
// PARAM_DEFINE_FLOAT(RC13_EXP, 0.0f);
PARAM_DEFINE_FLOAT(RC14_MIN, 1000);
PARAM_DEFINE_FLOAT(RC14_TRIM, 1500);
PARAM_DEFINE_FLOAT(RC14_MAX, 2000);
PARAM_DEFINE_FLOAT(RC14_REV, 1.0f);
PARAM_DEFINE_FLOAT(RC14_DZ, 0.0f);
// PARAM_DEFINE_FLOAT(RC12_EXP, 0.0f);
PARAM_DEFINE_INT32(RC_TYPE, 1); /** 1 = FUTABA, 2 = Spektrum, 3 = Graupner HoTT, 4 = Turnigy 9x */
/* default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */
PARAM_DEFINE_FLOAT(BAT_V_SCALING, (3.3f * 52.0f / 5.0f / 4095.0f));
@@ -131,12 +171,23 @@ PARAM_DEFINE_INT32(RC_MAP_ROLL, 1);
PARAM_DEFINE_INT32(RC_MAP_PITCH, 2);
PARAM_DEFINE_INT32(RC_MAP_THROTTLE, 3);
PARAM_DEFINE_INT32(RC_MAP_YAW, 4);
PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 5);
PARAM_DEFINE_INT32(RC_MAP_AUX1, 6);
PARAM_DEFINE_INT32(RC_MAP_AUX2, 7);
PARAM_DEFINE_INT32(RC_MAP_AUX3, 8);
PARAM_DEFINE_INT32(RC_MAP_OVER_SW, 5);
PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 6);
PARAM_DEFINE_INT32(RC_MAP_MAN_SW, 0);
PARAM_DEFINE_INT32(RC_MAP_SAS_SW, 0);
PARAM_DEFINE_INT32(RC_MAP_RTL_SW, 0);
PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0);
PARAM_DEFINE_INT32(RC_MAP_FLAPS, 0);
PARAM_DEFINE_INT32(RC_MAP_AUX1, 0); /**< default function: camera yaw / azimuth */
PARAM_DEFINE_INT32(RC_MAP_AUX2, 0); /**< default function: camera pitch / tilt */
PARAM_DEFINE_INT32(RC_MAP_AUX3, 0); /**< default function: camera trigger */
PARAM_DEFINE_INT32(RC_MAP_AUX4, 0); /**< default function: camera roll */
PARAM_DEFINE_INT32(RC_MAP_AUX5, 0); /**< default function: payload drop */
PARAM_DEFINE_FLOAT(RC_SCALE_ROLL, 0.4f);
PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.4f);
PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 1.0f);
apps/sensors/sensors.cpp
+222 -114
View File
@@ -95,11 +95,7 @@
#define PPM_INPUT_TIMEOUT_INTERVAL 50000 /**< 50 ms timeout / 20 Hz */
enum RC_DEMIX {
RC_DEMIX_NONE = 0,
RC_DEMIX_AUTO = 1,
RC_DEMIX_DELTA = 2
};
#define limit_minus_one_to_one(arg) (arg < -1.0f) ? -1.0f : ((arg > 1.0f) ? 1.0f : arg)
/**
* Sensor app start / stop handling function
@@ -129,7 +125,7 @@ public:
int start();
private:
static const unsigned _rc_max_chan_count = 8; /**< maximum number of r/c channels we handle */
static const unsigned _rc_max_chan_count = RC_CHANNELS_MAX; /**< maximum number of r/c channels we handle */
#if CONFIG_HRT_PPM
hrt_abstime _ppm_last_valid; /**< last time we got a valid ppm signal */
@@ -173,7 +169,7 @@ private:
float max[_rc_max_chan_count];
float rev[_rc_max_chan_count];
float dz[_rc_max_chan_count];
float ex[_rc_max_chan_count];
// float ex[_rc_max_chan_count];
float scaling_factor[_rc_max_chan_count];
float gyro_offset[3];
@@ -184,21 +180,31 @@ private:
int rc_type;
int rc_demix; /**< enabled de-mixing of RC mixers */
int rc_map_roll;
int rc_map_pitch;
int rc_map_yaw;
int rc_map_throttle;
int rc_map_mode_sw;
int rc_map_manual_override_sw;
int rc_map_auto_mode_sw;
int rc_map_manual_mode_sw;
int rc_map_sas_mode_sw;
int rc_map_rtl_sw;
int rc_map_offboard_ctrl_mode_sw;
int rc_map_flaps;
int rc_map_aux1;
int rc_map_aux2;
int rc_map_aux3;
int rc_map_aux4;
int rc_map_aux5;
float rc_scale_roll;
float rc_scale_pitch;
float rc_scale_yaw;
float rc_scale_flaps;
float battery_voltage_scaling;
} _parameters; /**< local copies of interesting parameters */
@@ -209,7 +215,7 @@ private:
param_t max[_rc_max_chan_count];
param_t rev[_rc_max_chan_count];
param_t dz[_rc_max_chan_count];
param_t ex[_rc_max_chan_count];
// param_t ex[_rc_max_chan_count];
param_t rc_type;
param_t rc_demix;
@@ -224,15 +230,27 @@ private:
param_t rc_map_pitch;
param_t rc_map_yaw;
param_t rc_map_throttle;
param_t rc_map_mode_sw;
param_t rc_map_manual_override_sw;
param_t rc_map_auto_mode_sw;
param_t rc_map_manual_mode_sw;
param_t rc_map_sas_mode_sw;
param_t rc_map_rtl_sw;
param_t rc_map_offboard_ctrl_mode_sw;
param_t rc_map_flaps;
param_t rc_map_aux1;
param_t rc_map_aux2;
param_t rc_map_aux3;
param_t rc_map_aux4;
param_t rc_map_aux5;
param_t rc_scale_roll;
param_t rc_scale_pitch;
param_t rc_scale_yaw;
param_t rc_scale_flaps;
param_t battery_voltage_scaling;
} _parameter_handles; /**< handles for interesting parameters */
@@ -395,27 +413,43 @@ Sensors::Sensors() :
sprintf(nbuf, "RC%d_DZ", i + 1);
_parameter_handles.dz[i] = param_find(nbuf);
/* channel exponential gain */
sprintf(nbuf, "RC%d_EXP", i + 1);
_parameter_handles.ex[i] = param_find(nbuf);
// /* channel exponential gain */
// sprintf(nbuf, "RC%d_EXP", i + 1);
// _parameter_handles.ex[i] = param_find(nbuf);
}
_parameter_handles.rc_type = param_find("RC_TYPE");
_parameter_handles.rc_demix = param_find("RC_DEMIX");
// _parameter_handles.rc_demix = param_find("RC_DEMIX");
/* mandatory input switched, mapped to channels 1-4 per default */
_parameter_handles.rc_map_roll = param_find("RC_MAP_ROLL");
_parameter_handles.rc_map_pitch = param_find("RC_MAP_PITCH");
_parameter_handles.rc_map_yaw = param_find("RC_MAP_YAW");
_parameter_handles.rc_map_throttle = param_find("RC_MAP_THROTTLE");
_parameter_handles.rc_map_mode_sw = param_find("RC_MAP_MODE_SW");
/* mandatory mode switches, mapped to channel 5 and 6 per default */
_parameter_handles.rc_map_manual_override_sw = param_find("RC_MAP_OVER_SW");
_parameter_handles.rc_map_auto_mode_sw = param_find("RC_MAP_MODE_SW");
_parameter_handles.rc_map_flaps = param_find("RC_MAP_FLAPS");
/* optional mode switches, not mapped per default */
_parameter_handles.rc_map_manual_mode_sw = param_find("RC_MAP_MAN_SW");
_parameter_handles.rc_map_sas_mode_sw = param_find("RC_MAP_SAS_SW");
_parameter_handles.rc_map_rtl_sw = param_find("RC_MAP_RTL_SW");
_parameter_handles.rc_map_offboard_ctrl_mode_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");
_parameter_handles.rc_map_aux3 = param_find("RC_MAP_AUX3");
_parameter_handles.rc_map_aux4 = param_find("RC_MAP_AUX4");
_parameter_handles.rc_map_aux5 = param_find("RC_MAP_AUX5");
_parameter_handles.rc_scale_roll = param_find("RC_SCALE_ROLL");
_parameter_handles.rc_scale_pitch = param_find("RC_SCALE_PITCH");
_parameter_handles.rc_scale_yaw = param_find("RC_SCALE_YAW");
_parameter_handles.rc_scale_flaps = param_find("RC_SCALE_FLAPS");
/* gyro offsets */
_parameter_handles.gyro_offset[0] = param_find("SENS_GYRO_XOFF");
@@ -472,10 +506,10 @@ Sensors::~Sensors()
int
Sensors::parameters_update()
{
const unsigned int nchans = 8;
bool rc_valid = true;
/* rc values */
for (unsigned int i = 0; i < nchans; i++) {
for (unsigned int i = 0; i < RC_CHANNELS_MAX; i++) {
if (param_get(_parameter_handles.min[i], &(_parameters.min[i])) != OK) {
warnx("Failed getting min for chan %d", i);
@@ -492,32 +526,33 @@ Sensors::parameters_update()
if (param_get(_parameter_handles.dz[i], &(_parameters.dz[i])) != OK) {
warnx("Failed getting dead zone for chan %d", i);
}
if (param_get(_parameter_handles.ex[i], &(_parameters.ex[i])) != OK) {
warnx("Failed getting exponential gain for chan %d", i);
}
// if (param_get(_parameter_handles.ex[i], &(_parameters.ex[i])) != OK) {
// warnx("Failed getting exponential gain for chan %d", i);
// }
_parameters.scaling_factor[i] = (1.0f / ((_parameters.max[i] - _parameters.min[i]) / 2.0f) * _parameters.rev[i]);
/* handle blowup in the scaling factor calculation */
if (isnan(_parameters.scaling_factor[i]) || isinf(_parameters.scaling_factor[i])) {
if (!isfinite(_parameters.scaling_factor[i]) ||
_parameters.scaling_factor[i] * _parameters.rev[i] < 0.000001f ||
_parameters.scaling_factor[i] * _parameters.rev[i] > 0.2f) {
/* scaling factors do not make sense, lock them down */
_parameters.scaling_factor[i] = 0;
rc_valid = false;
}
/* handle wrong values */
// XXX TODO
}
/* handle wrong values */
if (!rc_valid)
warnx("WARNING WARNING WARNING\n\nRC CALIBRATION NOT SANE!\n\n");
/* remote control type */
if (param_get(_parameter_handles.rc_type, &(_parameters.rc_type)) != OK) {
warnx("Failed getting remote control type");
}
/* de-mixing */
if (param_get(_parameter_handles.rc_demix, &(_parameters.rc_demix)) != OK) {
warnx("Failed getting demix setting");
}
/* channel mapping */
if (param_get(_parameter_handles.rc_map_roll, &(_parameters.rc_map_roll)) != OK) {
warnx("Failed getting roll chan index");
@@ -531,9 +566,30 @@ Sensors::parameters_update()
if (param_get(_parameter_handles.rc_map_throttle, &(_parameters.rc_map_throttle)) != OK) {
warnx("Failed getting throttle chan index");
}
if (param_get(_parameter_handles.rc_map_mode_sw, &(_parameters.rc_map_mode_sw)) != OK) {
warnx("Failed getting mode sw chan index");
if (param_get(_parameter_handles.rc_map_manual_override_sw, &(_parameters.rc_map_manual_override_sw)) != OK) {
warnx("Failed getting override sw chan index");
}
if (param_get(_parameter_handles.rc_map_auto_mode_sw, &(_parameters.rc_map_auto_mode_sw)) != OK) {
warnx("Failed getting auto mode 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_manual_mode_sw, &(_parameters.rc_map_manual_mode_sw)) != OK) {
warnx("Failed getting manual mode sw chan index");
}
if (param_get(_parameter_handles.rc_map_rtl_sw, &(_parameters.rc_map_rtl_sw)) != OK) {
warnx("Failed getting rtl sw chan index");
}
if (param_get(_parameter_handles.rc_map_sas_mode_sw, &(_parameters.rc_map_sas_mode_sw)) != OK) {
warnx("Failed getting sas mode sw 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");
}
if (param_get(_parameter_handles.rc_map_aux1, &(_parameters.rc_map_aux1)) != OK) {
warnx("Failed getting mode aux 1 index");
}
@@ -543,6 +599,12 @@ Sensors::parameters_update()
if (param_get(_parameter_handles.rc_map_aux3, &(_parameters.rc_map_aux3)) != OK) {
warnx("Failed getting mode aux 3 index");
}
if (param_get(_parameter_handles.rc_map_aux4, &(_parameters.rc_map_aux4)) != OK) {
warnx("Failed getting mode aux 4 index");
}
if (param_get(_parameter_handles.rc_map_aux5, &(_parameters.rc_map_aux5)) != OK) {
warnx("Failed getting mode aux 5 index");
}
if (param_get(_parameter_handles.rc_scale_roll, &(_parameters.rc_scale_roll)) != OK) {
warnx("Failed getting rc scaling for roll");
@@ -553,16 +615,31 @@ Sensors::parameters_update()
if (param_get(_parameter_handles.rc_scale_yaw, &(_parameters.rc_scale_yaw)) != OK) {
warnx("Failed getting rc scaling for yaw");
}
if (param_get(_parameter_handles.rc_scale_flaps, &(_parameters.rc_scale_flaps)) != OK) {
warnx("Failed getting rc scaling for flaps");
}
/* update RC function mappings */
_rc.function[THROTTLE] = _parameters.rc_map_throttle - 1;
_rc.function[ROLL] = _parameters.rc_map_roll - 1;
_rc.function[PITCH] = _parameters.rc_map_pitch - 1;
_rc.function[YAW] = _parameters.rc_map_yaw - 1;
_rc.function[OVERRIDE] = _parameters.rc_map_mode_sw - 1;
_rc.function[FUNC_0] = _parameters.rc_map_aux1 - 1;
_rc.function[FUNC_1] = _parameters.rc_map_aux2 - 1;
_rc.function[FUNC_2] = _parameters.rc_map_aux3 - 1;
_rc.function[OVERRIDE] = _parameters.rc_map_manual_override_sw - 1;
_rc.function[AUTO_MODE] = _parameters.rc_map_auto_mode_sw - 1;
_rc.function[FLAPS] = _parameters.rc_map_flaps - 1;
_rc.function[MANUAL_MODE] = _parameters.rc_map_manual_mode_sw - 1;
_rc.function[RTL] = _parameters.rc_map_rtl_sw - 1;
_rc.function[SAS_MODE] = _parameters.rc_map_sas_mode_sw - 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;
_rc.function[AUX_4] = _parameters.rc_map_aux4 - 1;
_rc.function[AUX_5] = _parameters.rc_map_aux5 - 1;
/* gyro offsets */
param_get(_parameter_handles.gyro_offset[0], &(_parameters.gyro_offset[0]));
@@ -948,8 +1025,23 @@ Sensors::ppm_poll()
struct manual_control_setpoint_s manual_control;
/* get a copy first, to prevent altering values */
orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sub, &manual_control);
/* initialize to default values */
manual_control.roll = NAN;
manual_control.pitch = NAN;
manual_control.yaw = NAN;
manual_control.throttle = NAN;
manual_control.manual_mode_switch = NAN;
manual_control.manual_sas_switch = NAN;
manual_control.return_to_launch_switch = NAN;
manual_control.auto_offboard_input_switch = NAN;
manual_control.flaps = NAN;
manual_control.aux1 = NAN;
manual_control.aux2 = NAN;
manual_control.aux3 = NAN;
manual_control.aux4 = NAN;
manual_control.aux5 = NAN;
/* require at least four channels to consider the signal valid */
if (rc_input.channel_count < 4)
@@ -996,79 +1088,116 @@ Sensors::ppm_poll()
manual_control.timestamp = rc_input.timestamp;
/* check if input needs to be de-mixed */
if (_parameters.rc_demix == (int)RC_DEMIX_DELTA) {
// XXX hardcoded for testing purposes, replace with inverted delta mixer from ROMFS
// /* check if input needs to be de-mixed */
// if (_parameters.rc_demix == (int)RC_DEMIX_DELTA) {
// // XXX hardcoded for testing purposes, replace with inverted delta mixer from ROMFS
/* roll input - mixed roll and pitch channels */
manual_control.roll = 0.5f * (_rc.chan[_rc.function[ROLL]].scaled - _rc.chan[_rc.function[PITCH]].scaled);
/* pitch input - mixed roll and pitch channels */
manual_control.pitch = 0.5f * (_rc.chan[_rc.function[ROLL]].scaled + _rc.chan[_rc.function[PITCH]].scaled);
/* yaw input - stick right is positive and positive rotation */
manual_control.yaw = _rc.chan[_rc.function[YAW]].scaled;
/* throttle input */
manual_control.throttle = _rc.chan[_rc.function[THROTTLE]].scaled;
// /* roll input - mixed roll and pitch channels */
// manual_control.roll = 0.5f * (_rc.chan[_rc.function[ROLL]].scaled - _rc.chan[_rc.function[PITCH]].scaled);
// pitch input - mixed roll and pitch channels
// manual_control.pitch = 0.5f * (_rc.chan[_rc.function[ROLL]].scaled + _rc.chan[_rc.function[PITCH]].scaled);
// /* yaw input - stick right is positive and positive rotation */
// manual_control.yaw = _rc.chan[_rc.function[YAW]].scaled;
// /* throttle input */
// manual_control.throttle = _rc.chan[_rc.function[THROTTLE]].scaled;
/* direct pass-through of manual input */
} else {
/* roll input - rolling right is stick-wise and rotation-wise positive */
manual_control.roll = _rc.chan[_rc.function[ROLL]].scaled;
/*
* pitch input - stick down is negative, but stick down is pitching up (pos) in NED,
* so reverse sign.
*/
manual_control.pitch = -1.0f * _rc.chan[_rc.function[PITCH]].scaled;
/* yaw input - stick right is positive and positive rotation */
manual_control.yaw = _rc.chan[_rc.function[YAW]].scaled;
/* throttle input */
manual_control.throttle = _rc.chan[_rc.function[THROTTLE]].scaled;
}
// /* direct pass-through of manual input */
// } else {
/* limit outputs */
if (manual_control.roll < -1.0f) manual_control.roll = -1.0f;
if (manual_control.roll > 1.0f) manual_control.roll = 1.0f;
/* roll input - rolling right is stick-wise and rotation-wise positive */
manual_control.roll = _rc.chan[_rc.function[ROLL]].scaled;
/*
* pitch input - stick down is negative, but stick down is pitching up (pos) in NED,
* so reverse sign.
*/
manual_control.pitch = limit_minus_one_to_one(-1.0f * _rc.chan[_rc.function[PITCH]].scaled);
/* yaw input - stick right is positive and positive rotation */
manual_control.yaw = limit_minus_one_to_one(_rc.chan[_rc.function[YAW]].scaled);
/* throttle input */
manual_control.throttle = _rc.chan[_rc.function[THROTTLE]].scaled;
if (manual_control.throttle < 0.0f) manual_control.throttle = 0.0f;
if (manual_control.throttle > 1.0f) manual_control.throttle = 1.0f;
/* scale output */
if (isfinite(_parameters.rc_scale_roll) && _parameters.rc_scale_roll > 0.0f) {
manual_control.roll *= _parameters.rc_scale_roll;
}
if (manual_control.pitch < -1.0f) manual_control.pitch = -1.0f;
if (manual_control.pitch > 1.0f) manual_control.pitch = 1.0f;
if (isfinite(_parameters.rc_scale_pitch) && _parameters.rc_scale_pitch > 0.0f) {
manual_control.pitch *= _parameters.rc_scale_pitch;
}
if (manual_control.yaw < -1.0f) manual_control.yaw = -1.0f;
if (manual_control.yaw > 1.0f) manual_control.yaw = 1.0f;
if (isfinite(_parameters.rc_scale_yaw) && _parameters.rc_scale_yaw > 0.0f) {
manual_control.yaw *= _parameters.rc_scale_yaw;
}
if (manual_control.throttle < 0.0f) manual_control.throttle = 0.0f;
if (manual_control.throttle > 1.0f) manual_control.throttle = 1.0f;
/* override switch input */
manual_control.manual_override_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OVERRIDE]].scaled);
/* mode switch input */
manual_control.override_mode_switch = _rc.chan[_rc.function[OVERRIDE]].scaled;
if (manual_control.override_mode_switch < -1.0f) manual_control.override_mode_switch = -1.0f;
if (manual_control.override_mode_switch > 1.0f) manual_control.override_mode_switch = 1.0f;
manual_control.auto_mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[AUTO_MODE]].scaled);
/* aux functions */
manual_control.aux1_cam_pan_flaps = _rc.chan[_rc.function[FUNC_0]].scaled;
if (manual_control.aux1_cam_pan_flaps < -1.0f) manual_control.aux1_cam_pan_flaps = -1.0f;
if (manual_control.aux1_cam_pan_flaps > 1.0f) manual_control.aux1_cam_pan_flaps = 1.0f;
/* flaps */
if (_rc.function[FLAPS] >= 0) {
/* aux inputs */
manual_control.aux2_cam_tilt = _rc.chan[_rc.function[FUNC_1]].scaled;
if (manual_control.aux2_cam_tilt < -1.0f) manual_control.aux2_cam_tilt = -1.0f;
if (manual_control.aux2_cam_tilt > 1.0f) manual_control.aux2_cam_tilt = 1.0f;
manual_control.flaps = limit_minus_one_to_one(_rc.chan[_rc.function[FLAPS]].scaled);
/* aux inputs */
manual_control.aux3_cam_zoom = _rc.chan[_rc.function[FUNC_2]].scaled;
if (manual_control.aux3_cam_zoom < -1.0f) manual_control.aux3_cam_zoom = -1.0f;
if (manual_control.aux3_cam_zoom > 1.0f) manual_control.aux3_cam_zoom = 1.0f;
if (isfinite(_parameters.rc_scale_flaps) && _parameters.rc_scale_flaps > 0.0f) {
manual_control.flaps *= _parameters.rc_scale_flaps;
}
}
orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc);
orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual_control);
if (_rc.function[MANUAL_MODE] >= 0) {
manual_control.manual_mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MANUAL_MODE]].scaled);
}
if (_rc.function[SAS_MODE] >= 0) {
manual_control.manual_sas_switch = limit_minus_one_to_one(_rc.chan[_rc.function[SAS_MODE]].scaled);
}
if (_rc.function[RTL] >= 0) {
manual_control.return_to_launch_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RTL]].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);
}
/* aux functions, only assign if valid mapping is present */
if (_rc.function[AUX_1] >= 0) {
manual_control.aux1 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_1]].scaled);
}
if (_rc.function[AUX_2] >= 0) {
manual_control.aux2 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_2]].scaled);
}
if (_rc.function[AUX_3] >= 0) {
manual_control.aux3 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_3]].scaled);
}
if (_rc.function[AUX_4] >= 0) {
manual_control.aux4 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_4]].scaled);
}
if (_rc.function[AUX_5] >= 0) {
manual_control.aux5 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_5]].scaled);
}
/* check if ready for publishing */
if (_rc_pub > 0) {
orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc);
} else {
/* advertise the rc topic */
_rc_pub = orb_advertise(ORB_ID(rc_channels), &_rc);
}
/* check if ready for publishing */
if (_manual_control_pub > 0) {
orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual_control);
} else {
_manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual_control);
}
}
}
@@ -1117,7 +1246,7 @@ Sensors::task_main()
memset(&raw, 0, sizeof(raw));
raw.timestamp = hrt_absolute_time();
raw.battery_voltage_v = BAT_VOL_INITIAL;
raw.adc_voltage_v[0] = 0.9f;
raw.adc_voltage_v[0] = 0.0f;
raw.adc_voltage_v[1] = 0.0f;
raw.adc_voltage_v[2] = 0.0f;
raw.battery_voltage_counter = 0;
@@ -1134,27 +1263,6 @@ Sensors::task_main()
/* advertise the sensor_combined topic and make the initial publication */
_sensor_pub = orb_advertise(ORB_ID(sensor_combined), &raw);
/* advertise the manual_control topic */
struct manual_control_setpoint_s manual_control;
manual_control.mode = MANUAL_CONTROL_MODE_ATT_YAW_POS;
manual_control.roll = 0.0f;
manual_control.pitch = 0.0f;
manual_control.yaw = 0.0f;
manual_control.throttle = 0.0f;
manual_control.aux1_cam_pan_flaps = 0.0f;
manual_control.aux2_cam_tilt = 0.0f;
manual_control.aux3_cam_zoom = 0.0f;
manual_control.aux4_cam_roll = 0.0f;
_manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual_control);
/* advertise the rc topic */
{
struct rc_channels_s rc;
memset(&rc, 0, sizeof(rc));
_rc_pub = orb_advertise(ORB_ID(rc_channels), &rc);
}
/* wakeup source(s) */
struct pollfd fds[1];