aboutsummaryrefslogtreecommitdiff
path: root/src/modules/sensors
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules/sensors')
-rw-r--r--src/modules/sensors/sensor_params.c77
-rw-r--r--src/modules/sensors/sensors.cpp431
2 files changed, 307 insertions, 201 deletions
diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index bd431c9eb..587b81588 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -44,8 +44,34 @@
#include <systemlib/param/param.h>
+/**
+ * Gyro X offset FIXME
+ *
+ * This is an X-axis offset for the gyro.
+ * Adjust it according to the calibration data.
+ *
+ * @min -10.0
+ * @max 10.0
+ * @group Gyro Config
+ */
PARAM_DEFINE_FLOAT(SENS_GYRO_XOFF, 0.0f);
+
+/**
+ * Gyro Y offset FIXME with dot.
+ *
+ * @min -10.0
+ * @max 10.0
+ * @group Gyro Config
+ */
PARAM_DEFINE_FLOAT(SENS_GYRO_YOFF, 0.0f);
+
+/**
+ * Gyro Z offset FIXME
+ *
+ * @min -5.0
+ * @max 5.0
+ * @group Gyro Config
+ */
PARAM_DEFINE_FLOAT(SENS_GYRO_ZOFF, 0.0f);
PARAM_DEFINE_FLOAT(SENS_GYRO_XSCALE, 1.0f);
@@ -68,7 +94,11 @@ PARAM_DEFINE_FLOAT(SENS_ACC_XSCALE, 1.0f);
PARAM_DEFINE_FLOAT(SENS_ACC_YSCALE, 1.0f);
PARAM_DEFINE_FLOAT(SENS_ACC_ZSCALE, 1.0f);
-PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 1667);
+PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0.0f);
+PARAM_DEFINE_INT32(SENS_DPRES_ANA, 0);
+
+PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0);
+PARAM_DEFINE_INT32(SENS_EXT_MAG_ROT, 0);
PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f);
PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f);
@@ -154,34 +184,45 @@ PARAM_DEFINE_FLOAT(RC14_MAX, 2000);
PARAM_DEFINE_FLOAT(RC14_REV, 1.0f);
PARAM_DEFINE_FLOAT(RC14_DZ, 0.0f);
-PARAM_DEFINE_INT32(RC_TYPE, 1); /** 1 = FUTABA, 2 = Spektrum, 3 = Graupner HoTT, 4 = Turnigy 9x */
+PARAM_DEFINE_FLOAT(RC15_MIN, 1000);
+PARAM_DEFINE_FLOAT(RC15_TRIM, 1500);
+PARAM_DEFINE_FLOAT(RC15_MAX, 2000);
+PARAM_DEFINE_FLOAT(RC15_REV, 1.0f);
+PARAM_DEFINE_FLOAT(RC15_DZ, 0.0f);
+
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */
-PARAM_DEFINE_INT32(RC_DSM_BIND, 0); /* 0 = Idle, 1 = Start DSM2 bind, 2 = Start DSMX bind */
+#endif
+PARAM_DEFINE_INT32(RC_DSM_BIND, -1); /* -1 = Idle, 0 = Start DSM2 bind, 1 = Start DSMX bind */
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f);
+#else
/* 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));
+/* PX4IOAR: 0.00838095238 */
+/* FMU standalone: 1/(10 / (47+10)) * (3.3 / 4095) = 0.00459340659 */
+/* FMU with PX4IOAR: (3.3f * 52.0f / 5.0f / 4095.0f) */
+PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.00459340659f);
+#endif
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_OVER_SW, 5);
-PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 6);
+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_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_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_INT32(RC_MAP_AUX1, 0); /**< default function: camera pitch */
+PARAM_DEFINE_INT32(RC_MAP_AUX2, 0); /**< default function: camera roll */
+PARAM_DEFINE_INT32(RC_MAP_AUX3, 0); /**< default function: camera azimuth / yaw */
-PARAM_DEFINE_FLOAT(RC_SCALE_ROLL, 0.4f);
-PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.4f);
-PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 1.0f);
+PARAM_DEFINE_FLOAT(RC_SCALE_ROLL, 0.6f);
+PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.6f);
+PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 2.0f);
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 42268b971..da0c11372 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -50,6 +50,7 @@
#include <stdio.h>
#include <errno.h>
#include <math.h>
+#include <mathlib/mathlib.h>
#include <nuttx/analog/adc.h>
@@ -66,15 +67,16 @@
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <systemlib/perf_counter.h>
+#include <conversion/rotation.h>
-#include <systemlib/ppm_decode.h>
#include <systemlib/airspeed.h>
#include <uORB/uORB.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/rc_channels.h>
#include <uORB/topics/manual_control_setpoint.h>
-#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/differential_pressure.h>
@@ -104,22 +106,22 @@
* IN13 - aux1
* IN14 - aux2
* IN15 - pressure sensor
- *
+ *
* IO:
* IN4 - servo supply rail
* IN5 - analog RSSI
*/
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
- #define ADC_BATTERY_VOLTAGE_CHANNEL 10
- #define ADC_AIRSPEED_VOLTAGE_CHANNEL 11
+#define ADC_BATTERY_VOLTAGE_CHANNEL 10
+#define ADC_AIRSPEED_VOLTAGE_CHANNEL 11
#endif
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
- #define ADC_BATTERY_VOLTAGE_CHANNEL 2
- #define ADC_BATTERY_CURRENT_CHANNEL 3
- #define ADC_5V_RAIL_SENSE 4
- #define ADC_AIRSPEED_VOLTAGE_CHANNEL 15
+#define ADC_BATTERY_VOLTAGE_CHANNEL 2
+#define ADC_BATTERY_CURRENT_CHANNEL 3
+#define ADC_5V_RAIL_SENSE 4
+#define ADC_AIRSPEED_VOLTAGE_CHANNEL 15
#endif
#define BAT_VOL_INITIAL 0.f
@@ -133,8 +135,6 @@
*/
#define PCB_TEMP_ESTIMATE_DEG 5.0f
-#define PPM_INPUT_TIMEOUT_INTERVAL 50000 /**< 50 ms timeout / 20 Hz */
-
#define limit_minus_one_to_one(arg) (arg < -1.0f) ? -1.0f : ((arg > 1.0f) ? 1.0f : arg)
/**
@@ -167,12 +167,12 @@ public:
private:
static const unsigned _rc_max_chan_count = RC_CHANNELS_MAX; /**< maximum number of r/c channels we handle */
- hrt_abstime _ppm_last_valid; /**< last time we got a valid ppm signal */
+ hrt_abstime _rc_last_valid; /**< last time we got a valid RC signal */
/**
- * Gather and publish PPM input data.
+ * Gather and publish RC input data.
*/
- void ppm_poll();
+ void rc_poll();
/* XXX should not be here - should be own driver */
int _fd_adc; /**< ADC driver handle */
@@ -189,14 +189,15 @@ private:
int _mag_sub; /**< raw mag data subscription */
int _rc_sub; /**< raw rc channels data subscription */
int _baro_sub; /**< raw baro data subscription */
- int _airspeed_sub; /**< airspeed subscription */
- int _diff_pres_sub; /**< raw differential pressure subscription */
- int _vstatus_sub; /**< vehicle status subscription */
+ int _airspeed_sub; /**< airspeed subscription */
+ int _diff_pres_sub; /**< raw differential pressure subscription */
+ int _vcontrol_mode_sub; /**< vehicle control mode subscription */
int _params_sub; /**< notification of parameter updates */
int _manual_control_sub; /**< notification of manual control updates */
orb_advert_t _sensor_pub; /**< combined sensor data topic */
orb_advert_t _manual_control_pub; /**< manual control signal topic */
+ orb_advert_t _actuator_group_3_pub; /**< manual control as actuator topic */
orb_advert_t _rc_pub; /**< raw r/c control topic */
orb_advert_t _battery_pub; /**< battery status */
orb_advert_t _airspeed_pub; /**< airspeed */
@@ -210,13 +211,16 @@ private:
struct differential_pressure_s _diff_pres;
struct airspeed_s _airspeed;
+ math::Matrix _board_rotation; /**< rotation matrix for the orientation that the board is mounted */
+ math::Matrix _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */
+ bool _mag_is_external; /**< true if the active mag is on an external board */
+
struct {
float min[_rc_max_chan_count];
float trim[_rc_max_chan_count];
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 scaling_factor[_rc_max_chan_count];
float gyro_offset[3];
@@ -226,21 +230,22 @@ private:
float accel_offset[3];
float accel_scale[3];
float diff_pres_offset_pa;
+ float diff_pres_analog_enabled;
- int rc_type;
+ int board_rotation;
+ int external_mag_rotation;
int rc_map_roll;
int rc_map_pitch;
int rc_map_yaw;
int rc_map_throttle;
- int rc_map_manual_override_sw;
- int rc_map_auto_mode_sw;
+ int rc_map_mode_sw;
+ int rc_map_return_sw;
+ int rc_map_assisted_sw;
+ int rc_map_mission_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_offboard_ctrl_mode_sw;
int rc_map_flaps;
@@ -265,10 +270,6 @@ 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 rc_type;
-
- param_t rc_demix;
param_t gyro_offset[3];
param_t gyro_scale[3];
@@ -277,19 +278,19 @@ private:
param_t mag_offset[3];
param_t mag_scale[3];
param_t diff_pres_offset_pa;
+ param_t diff_pres_analog_enabled;
param_t rc_map_roll;
param_t rc_map_pitch;
param_t rc_map_yaw;
param_t rc_map_throttle;
- param_t rc_map_manual_override_sw;
- param_t rc_map_auto_mode_sw;
+ param_t rc_map_mode_sw;
+ param_t rc_map_return_sw;
+ param_t rc_map_assisted_sw;
+ param_t rc_map_mission_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_offboard_ctrl_mode_sw;
param_t rc_map_flaps;
@@ -306,6 +307,9 @@ private:
param_t battery_voltage_scaling;
+ param_t board_rotation;
+ param_t external_mag_rotation;
+
} _parameter_handles; /**< handles for interesting parameters */
@@ -380,9 +384,9 @@ private:
void diff_pres_poll(struct sensor_combined_s &raw);
/**
- * Check for changes in vehicle status.
+ * Check for changes in vehicle control mode.
*/
- void vehicle_status_poll();
+ void vehicle_control_mode_poll();
/**
* Check for changes in parameters.
@@ -421,7 +425,7 @@ Sensors *g_sensors = nullptr;
}
Sensors::Sensors() :
- _ppm_last_valid(0),
+ _rc_last_valid(0),
_fd_adc(-1),
_last_adc(0),
@@ -437,20 +441,25 @@ Sensors::Sensors() :
_mag_sub(-1),
_rc_sub(-1),
_baro_sub(-1),
- _vstatus_sub(-1),
+ _vcontrol_mode_sub(-1),
_params_sub(-1),
_manual_control_sub(-1),
/* publications */
_sensor_pub(-1),
_manual_control_pub(-1),
+ _actuator_group_3_pub(-1),
_rc_pub(-1),
_battery_pub(-1),
_airspeed_pub(-1),
_diff_pres_pub(-1),
/* performance counters */
- _loop_perf(perf_alloc(PC_ELAPSED, "sensor task update"))
+ _loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")),
+
+ _board_rotation(3, 3),
+ _external_mag_rotation(3, 3),
+ _mag_is_external(false)
{
/* basic r/c parameters */
@@ -479,8 +488,6 @@ Sensors::Sensors() :
}
- _parameter_handles.rc_type = param_find("RC_TYPE");
-
/* 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");
@@ -488,16 +495,16 @@ Sensors::Sensors() :
_parameter_handles.rc_map_throttle = param_find("RC_MAP_THROTTLE");
/* 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_mode_sw = param_find("RC_MAP_MODE_SW");
+ _parameter_handles.rc_map_return_sw = param_find("RC_MAP_RETURN_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_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_aux1 = param_find("RC_MAP_AUX1");
_parameter_handles.rc_map_aux2 = param_find("RC_MAP_AUX2");
@@ -537,9 +544,14 @@ Sensors::Sensors() :
/* Differential pressure offset */
_parameter_handles.diff_pres_offset_pa = param_find("SENS_DPRES_OFF");
+ _parameter_handles.diff_pres_analog_enabled = param_find("SENS_DPRES_ANA");
_parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING");
+ /* rotations */
+ _parameter_handles.board_rotation = param_find("SENS_BOARD_ROT");
+ _parameter_handles.external_mag_rotation = param_find("SENS_EXT_MAG_ROT");
+
/* fetch initial parameter values */
parameters_update();
}
@@ -573,6 +585,8 @@ int
Sensors::parameters_update()
{
bool rc_valid = true;
+ float tmpScaleFactor = 0.0f;
+ float tmpRevFactor = 0.0f;
/* rc values */
for (unsigned int i = 0; i < RC_CHANNELS_MAX; i++) {
@@ -583,29 +597,27 @@ Sensors::parameters_update()
param_get(_parameter_handles.rev[i], &(_parameters.rev[i]));
param_get(_parameter_handles.dz[i], &(_parameters.dz[i]));
- _parameters.scaling_factor[i] = (1.0f / ((_parameters.max[i] - _parameters.min[i]) / 2.0f) * _parameters.rev[i]);
+ tmpScaleFactor = (1.0f / ((_parameters.max[i] - _parameters.min[i]) / 2.0f) * _parameters.rev[i]);
+ tmpRevFactor = tmpScaleFactor * _parameters.rev[i];
/* handle blowup in the scaling factor calculation */
- if (!isfinite(_parameters.scaling_factor[i]) ||
- _parameters.scaling_factor[i] * _parameters.rev[i] < 0.000001f ||
- _parameters.scaling_factor[i] * _parameters.rev[i] > 0.2f) {
-
+ if (!isfinite(tmpScaleFactor) ||
+ (tmpRevFactor < 0.000001f) ||
+ (tmpRevFactor > 0.2f)) {
+ warnx("RC chan %u not sane, scaling: %8.6f, rev: %d", i, tmpScaleFactor, (int)(_parameters.rev[i]));
/* scaling factors do not make sense, lock them down */
- _parameters.scaling_factor[i] = 0;
+ _parameters.scaling_factor[i] = 0.0f;
rc_valid = false;
- }
+ } else {
+ _parameters.scaling_factor[i] = tmpScaleFactor;
+ }
}
/* 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");
- }
-
/* channel mapping */
if (param_get(_parameter_handles.rc_map_roll, &(_parameters.rc_map_roll)) != OK) {
warnx("Failed getting roll chan index");
@@ -623,54 +635,35 @@ Sensors::parameters_update()
warnx("Failed getting throttle 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_mode_sw, &(_parameters.rc_map_mode_sw)) != OK) {
+ warnx("Failed getting mode 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_return_sw, &(_parameters.rc_map_return_sw)) != OK) {
+ warnx("Failed getting return 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_assisted_sw, &(_parameters.rc_map_assisted_sw)) != OK) {
+ warnx("Failed getting assisted sw chan index");
}
- if (param_get(_parameter_handles.rc_map_aux1, &(_parameters.rc_map_aux1)) != OK) {
- warnx("Failed getting mode aux 1 index");
+ if (param_get(_parameter_handles.rc_map_mission_sw, &(_parameters.rc_map_mission_sw)) != OK) {
+ warnx("Failed getting mission sw chan index");
}
- if (param_get(_parameter_handles.rc_map_aux2, &(_parameters.rc_map_aux2)) != OK) {
- warnx("Failed getting mode aux 2 index");
- }
-
- 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_flaps, &(_parameters.rc_map_flaps)) != OK) {
+ warnx("Failed getting flaps chan 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_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));
+ param_get(_parameter_handles.rc_map_aux4, &(_parameters.rc_map_aux4));
+ param_get(_parameter_handles.rc_map_aux5, &(_parameters.rc_map_aux5));
param_get(_parameter_handles.rc_scale_roll, &(_parameters.rc_scale_roll));
param_get(_parameter_handles.rc_scale_pitch, &(_parameters.rc_scale_pitch));
param_get(_parameter_handles.rc_scale_yaw, &(_parameters.rc_scale_yaw));
@@ -682,15 +675,14 @@ Sensors::parameters_update()
_rc.function[PITCH] = _parameters.rc_map_pitch - 1;
_rc.function[YAW] = _parameters.rc_map_yaw - 1;
- _rc.function[OVERRIDE] = _parameters.rc_map_manual_override_sw - 1;
- _rc.function[AUTO_MODE] = _parameters.rc_map_auto_mode_sw - 1;
+ _rc.function[MODE] = _parameters.rc_map_mode_sw - 1;
+ _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[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[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;
@@ -725,12 +717,19 @@ Sensors::parameters_update()
/* Airspeed offset */
param_get(_parameter_handles.diff_pres_offset_pa, &(_parameters.diff_pres_offset_pa));
+ param_get(_parameter_handles.diff_pres_analog_enabled, &(_parameters.diff_pres_analog_enabled));
/* scaling of ADC ticks to battery voltage */
if (param_get(_parameter_handles.battery_voltage_scaling, &(_parameters.battery_voltage_scaling)) != OK) {
warnx("Failed updating voltage scaling param");
}
+ param_get(_parameter_handles.board_rotation, &(_parameters.board_rotation));
+ param_get(_parameter_handles.external_mag_rotation, &(_parameters.external_mag_rotation));
+
+ get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation);
+ get_rot_matrix((enum Rotation)_parameters.external_mag_rotation, &_external_mag_rotation);
+
return OK;
}
@@ -749,7 +748,7 @@ Sensors::accel_init()
// XXX do the check more elegantly
- #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
/* set the accel internal sampling rate up to at leat 1000Hz */
ioctl(fd, ACCELIOCSSAMPLERATE, 1000);
@@ -757,7 +756,7 @@ Sensors::accel_init()
/* set the driver to poll at 1000Hz */
ioctl(fd, SENSORIOCSPOLLRATE, 1000);
- #else
+#elif CONFIG_ARCH_BOARD_PX4FMU_V2
/* set the accel internal sampling rate up to at leat 800Hz */
ioctl(fd, ACCELIOCSSAMPLERATE, 800);
@@ -765,7 +764,10 @@ Sensors::accel_init()
/* set the driver to poll at 800Hz */
ioctl(fd, SENSORIOCSPOLLRATE, 800);
- #endif
+#else
+#error Need a board configuration, either CONFIG_ARCH_BOARD_PX4FMU_V1 or CONFIG_ARCH_BOARD_PX4FMU_V2
+
+#endif
warnx("using system accel");
close(fd);
@@ -787,7 +789,7 @@ Sensors::gyro_init()
// XXX do the check more elegantly
- #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
/* set the gyro internal sampling rate up to at least 1000Hz */
if (ioctl(fd, GYROIOCSSAMPLERATE, 1000) != OK)
@@ -797,15 +799,15 @@ Sensors::gyro_init()
if (ioctl(fd, SENSORIOCSPOLLRATE, 1000) != OK)
ioctl(fd, SENSORIOCSPOLLRATE, 800);
- #else
+#else
- /* set the gyro internal sampling rate up to at leat 800Hz */
- ioctl(fd, GYROIOCSSAMPLERATE, 800);
+ /* set the gyro internal sampling rate up to at least 760Hz */
+ ioctl(fd, GYROIOCSSAMPLERATE, 760);
- /* set the driver to poll at 800Hz */
- ioctl(fd, SENSORIOCSPOLLRATE, 800);
+ /* set the driver to poll at 760Hz */
+ ioctl(fd, SENSORIOCSPOLLRATE, 760);
- #endif
+#endif
warnx("using system gyro");
close(fd);
@@ -816,6 +818,7 @@ void
Sensors::mag_init()
{
int fd;
+ int ret;
fd = open(MAG_DEVICE_PATH, 0);
@@ -824,11 +827,38 @@ Sensors::mag_init()
errx(1, "FATAL: no magnetometer found");
}
- /* set the mag internal poll rate to at least 150Hz */
- ioctl(fd, MAGIOCSSAMPLERATE, 150);
+ /* try different mag sampling rates */
- /* set the driver to poll at 150Hz */
- ioctl(fd, SENSORIOCSPOLLRATE, 150);
+
+ ret = ioctl(fd, MAGIOCSSAMPLERATE, 150);
+
+ if (ret == OK) {
+ /* set the pollrate accordingly */
+ ioctl(fd, SENSORIOCSPOLLRATE, 150);
+
+ } else {
+ ret = ioctl(fd, MAGIOCSSAMPLERATE, 100);
+
+ /* if the slower sampling rate still fails, something is wrong */
+ if (ret == OK) {
+ /* set the driver to poll also at the slower rate */
+ ioctl(fd, SENSORIOCSPOLLRATE, 100);
+
+ } else {
+ errx(1, "FATAL: mag sampling rate could not be set");
+ }
+ }
+
+
+
+ ret = ioctl(fd, MAGIOCGEXTERNAL, 0);
+
+ if (ret < 0)
+ errx(1, "FATAL: unknown if magnetometer is external or onboard");
+ else if (ret == 1)
+ _mag_is_external = true;
+ else
+ _mag_is_external = false;
close(fd);
}
@@ -842,7 +872,7 @@ Sensors::baro_init()
if (fd < 0) {
warn("%s", BARO_DEVICE_PATH);
- warnx("No barometer found, ignoring");
+ errx(1, "FATAL: No barometer found");
}
/* set the driver to poll at 150Hz */
@@ -874,9 +904,12 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report);
- raw.accelerometer_m_s2[0] = accel_report.x;
- raw.accelerometer_m_s2[1] = accel_report.y;
- raw.accelerometer_m_s2[2] = accel_report.z;
+ math::Vector3 vect = {accel_report.x, accel_report.y, accel_report.z};
+ vect = _board_rotation * vect;
+
+ raw.accelerometer_m_s2[0] = vect(0);
+ raw.accelerometer_m_s2[1] = vect(1);
+ raw.accelerometer_m_s2[2] = vect(2);
raw.accelerometer_raw[0] = accel_report.x_raw;
raw.accelerometer_raw[1] = accel_report.y_raw;
@@ -897,9 +930,12 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report);
- raw.gyro_rad_s[0] = gyro_report.x;
- raw.gyro_rad_s[1] = gyro_report.y;
- raw.gyro_rad_s[2] = gyro_report.z;
+ math::Vector3 vect = {gyro_report.x, gyro_report.y, gyro_report.z};
+ vect = _board_rotation * vect;
+
+ raw.gyro_rad_s[0] = vect(0);
+ raw.gyro_rad_s[1] = vect(1);
+ raw.gyro_rad_s[2] = vect(2);
raw.gyro_raw[0] = gyro_report.x_raw;
raw.gyro_raw[1] = gyro_report.y_raw;
@@ -920,9 +956,16 @@ Sensors::mag_poll(struct sensor_combined_s &raw)
orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report);
- raw.magnetometer_ga[0] = mag_report.x;
- raw.magnetometer_ga[1] = mag_report.y;
- raw.magnetometer_ga[2] = mag_report.z;
+ math::Vector3 vect = {mag_report.x, mag_report.y, mag_report.z};
+
+ if (_mag_is_external)
+ vect = _external_mag_rotation * vect;
+ else
+ vect = _board_rotation * vect;
+
+ raw.magnetometer_ga[0] = vect(0);
+ raw.magnetometer_ga[1] = vect(1);
+ raw.magnetometer_ga[2] = vect(2);
raw.magnetometer_raw[0] = mag_report.x_raw;
raw.magnetometer_raw[1] = mag_report.y_raw;
@@ -963,8 +1006,8 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw)
raw.differential_pressure_counter++;
_airspeed.indicated_airspeed_m_s = calc_indicated_airspeed(_diff_pres.differential_pressure_pa);
- _airspeed.true_airspeed_m_s = calc_true_airspeed(_diff_pres.differential_pressure_pa + raw.baro_pres_mbar*1e2f,
- raw.baro_pres_mbar*1e2f, raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG);
+ _airspeed.true_airspeed_m_s = calc_true_airspeed(_diff_pres.differential_pressure_pa + raw.baro_pres_mbar * 1e2f,
+ raw.baro_pres_mbar * 1e2f, raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG);
/* announce the airspeed if needed, just publish else */
if (_airspeed_pub > 0) {
@@ -977,21 +1020,21 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw)
}
void
-Sensors::vehicle_status_poll()
+Sensors::vehicle_control_mode_poll()
{
- struct vehicle_status_s vstatus;
- bool vstatus_updated;
+ struct vehicle_control_mode_s vcontrol_mode;
+ bool vcontrol_mode_updated;
- /* Check HIL state if vehicle status has changed */
- orb_check(_vstatus_sub, &vstatus_updated);
+ /* Check HIL state if vehicle control mode has changed */
+ orb_check(_vcontrol_mode_sub, &vcontrol_mode_updated);
- if (vstatus_updated) {
+ if (vcontrol_mode_updated) {
- orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &vstatus);
+ orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &vcontrol_mode);
/* switching from non-HIL to HIL mode */
//printf("[sensors] Vehicle mode: %i \t AND: %i, HIL: %i\n", vstatus.mode, vstatus.mode & VEHICLE_MODE_FLAG_HIL_ENABLED, hil_enabled);
- if (vstatus.flag_hil_enabled && !_hil_enabled) {
+ if (vcontrol_mode.flag_system_hil_enabled && !_hil_enabled) {
_hil_enabled = true;
_publishing = false;
@@ -1093,6 +1136,9 @@ Sensors::parameter_update_poll(bool forced)
void
Sensors::adc_poll(struct sensor_combined_s &raw)
{
+ /* only read if publishing */
+ if (!_publishing)
+ return;
/* rate limit to 100 Hz */
if (hrt_absolute_time() - _last_adc >= 10000) {
@@ -1102,12 +1148,12 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
int ret = read(_fd_adc, &buf_adc, sizeof(buf_adc));
for (unsigned i = 0; i < sizeof(buf_adc) / sizeof(buf_adc[0]); i++) {
-
+
if (ret >= (int)sizeof(buf_adc[0])) {
/* Save raw voltage values */
if (i < (sizeof(raw.adc_voltage_v)) / sizeof(raw.adc_voltage_v[0])) {
- raw.adc_voltage_v[i] = buf_adc[i].am_data / (4096.0f / 3.3f);
+ raw.adc_voltage_v[i] = buf_adc[i].am_data / (4096.0f / 3.3f);
}
/* look for specific channels and process the raw voltage to measurement data */
@@ -1135,18 +1181,19 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
} else {
_battery_pub = orb_advertise(ORB_ID(battery_status), &_battery_status);
}
- }
+ }
} else if (ADC_AIRSPEED_VOLTAGE_CHANNEL == buf_adc[i].am_channel) {
/* calculate airspeed, raw is the difference from */
- float voltage = (float)(buf_adc[i].am_data ) * 3.3f / 4096.0f * 2.0f; //V_ref/4096 * (voltage divider factor)
+ float voltage = (float)(buf_adc[i].am_data) * 3.3f / 4096.0f * 2.0f; //V_ref/4096 * (voltage divider factor)
/**
* The voltage divider pulls the signal down, only act on
- * a valid voltage from a connected sensor
+ * a valid voltage from a connected sensor. Also assume a non-
+ * zero offset from the sensor if its connected.
*/
- if (voltage > 0.4f) {
+ if (voltage > 0.4f && _parameters.diff_pres_analog_enabled) {
float diff_pres_pa = voltage * 1000.0f - _parameters.diff_pres_offset_pa; //for MPXV7002DP sensor
@@ -1171,22 +1218,19 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
}
void
-Sensors::ppm_poll()
+Sensors::rc_poll()
{
+ bool rc_updated;
+ orb_check(_rc_sub, &rc_updated);
- /* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */
- struct pollfd fds[1];
- fds[0].fd = _rc_sub;
- fds[0].events = POLLIN;
- /* check non-blocking for new data */
- int poll_ret = poll(fds, 1, 0);
-
- if (poll_ret > 0) {
+ if (rc_updated) {
+ /* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */
struct rc_input_values rc_input;
orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input);
struct manual_control_setpoint_s manual_control;
+ struct actuator_controls_s actuator_group_3;
/* initialize to default values */
manual_control.roll = NAN;
@@ -1194,10 +1238,11 @@ Sensors::ppm_poll()
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.mode_switch = NAN;
+ manual_control.return_switch = NAN;
+ manual_control.assisted_switch = NAN;
+ manual_control.mission_switch = NAN;
+// manual_control.auto_offboard_input_switch = NAN;
manual_control.flaps = NAN;
manual_control.aux1 = NAN;
@@ -1216,7 +1261,7 @@ Sensors::ppm_poll()
channel_limit = _rc_max_chan_count;
/* we are accepting this message */
- _ppm_last_valid = rc_input.timestamp;
+ _rc_last_valid = rc_input.timestamp;
/* Read out values from raw message */
for (unsigned int i = 0; i < channel_limit; i++) {
@@ -1226,6 +1271,7 @@ Sensors::ppm_poll()
*/
if (rc_input.values[i] < _parameters.min[i])
rc_input.values[i] = _parameters.min[i];
+
if (rc_input.values[i] > _parameters.max[i])
rc_input.values[i] = _parameters.max[i];
@@ -1297,11 +1343,17 @@ Sensors::ppm_poll()
manual_control.yaw *= _parameters.rc_scale_yaw;
}
- /* override switch input */
- manual_control.manual_override_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OVERRIDE]].scaled);
-
/* mode switch input */
- manual_control.auto_mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[AUTO_MODE]].scaled);
+ manual_control.mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MODE]].scaled);
+
+ /* land switch input */
+ manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled);
+
+ /* assisted switch input */
+ manual_control.assisted_switch = limit_minus_one_to_one(_rc.chan[_rc.function[ASSISTED]].scaled);
+
+ /* mission switch input */
+ manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled);
/* flaps */
if (_rc.function[FLAPS] >= 0) {
@@ -1313,21 +1365,17 @@ Sensors::ppm_poll()
}
}
- 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[MODE] >= 0) {
+ manual_control.mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[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[MISSION] >= 0) {
+ manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].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);
- }
+// 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) {
@@ -1350,6 +1398,16 @@ Sensors::ppm_poll()
manual_control.aux5 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_5]].scaled);
}
+ /* copy from mapped manual control to control group 3 */
+ actuator_group_3.control[0] = manual_control.roll;
+ actuator_group_3.control[1] = manual_control.pitch;
+ actuator_group_3.control[2] = manual_control.yaw;
+ actuator_group_3.control[3] = manual_control.throttle;
+ actuator_group_3.control[4] = manual_control.flaps;
+ actuator_group_3.control[5] = manual_control.aux1;
+ actuator_group_3.control[6] = manual_control.aux2;
+ actuator_group_3.control[7] = manual_control.aux3;
+
/* check if ready for publishing */
if (_rc_pub > 0) {
orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc);
@@ -1366,6 +1424,14 @@ Sensors::ppm_poll()
} else {
_manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual_control);
}
+
+ /* check if ready for publishing */
+ if (_actuator_group_3_pub > 0) {
+ orb_publish(ORB_ID(actuator_controls_3), _actuator_group_3_pub, &actuator_group_3);
+
+ } else {
+ _actuator_group_3_pub = orb_advertise(ORB_ID(actuator_controls_3), &actuator_group_3);
+ }
}
}
@@ -1381,8 +1447,7 @@ Sensors::task_main()
{
/* inform about start */
- printf("[sensors] Initializing..\n");
- fflush(stdout);
+ warnx("Initializing..");
/* start individual sensors */
accel_init();
@@ -1400,12 +1465,12 @@ Sensors::task_main()
_rc_sub = orb_subscribe(ORB_ID(input_rc));
_baro_sub = orb_subscribe(ORB_ID(sensor_baro));
_diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
- _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
+ _vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
/* rate limit vehicle status updates to 5Hz */
- orb_set_interval(_vstatus_sub, 200);
+ orb_set_interval(_vcontrol_mode_sub, 200);
/* rate limit gyro to 250 Hz (the gyro signal is lowpassed accordingly earlier) */
orb_set_interval(_gyro_sub, 4);
@@ -1461,7 +1526,7 @@ Sensors::task_main()
perf_begin(_loop_perf);
/* check vehicle status for changes to publication state */
- vehicle_status_poll();
+ vehicle_control_mode_poll();
/* check parameters for updates */
parameter_update_poll();
@@ -1485,7 +1550,7 @@ Sensors::task_main()
orb_publish(ORB_ID(sensor_combined), _sensor_pub, &raw);
/* Look for new r/c input data */
- ppm_poll();
+ rc_poll();
perf_end(_loop_perf);
}
@@ -1503,11 +1568,11 @@ Sensors::start()
/* start the task */
_sensors_task = task_spawn_cmd("sensors_task",
- SCHED_DEFAULT,
- SCHED_PRIORITY_MAX - 5,
- 2048,
- (main_t)&Sensors::task_main_trampoline,
- nullptr);
+ SCHED_DEFAULT,
+ SCHED_PRIORITY_MAX - 5,
+ 2048,
+ (main_t)&Sensors::task_main_trampoline,
+ nullptr);
if (_sensors_task < 0) {
warn("task start failed");
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-10 11:11:08 +0000