aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
m---------NuttX0
-rw-r--r--ROMFS/px4fmu_common/init.d/3033_wingwing2
-rw-r--r--ROMFS/px4fmu_common/mixers/wingwing.mix51
-rwxr-xr-xTools/check_submodules.sh18
-rw-r--r--src/drivers/device/spi.cpp62
-rw-r--r--src/drivers/device/spi.h2
-rw-r--r--src/drivers/drv_px4flow.h16
-rw-r--r--src/drivers/drv_range_finder.h9
-rw-r--r--src/drivers/drv_rc_input.h9
-rw-r--r--src/drivers/gps/gps.cpp4
-rw-r--r--src/drivers/px4fmu/fmu.cpp2
-rw-r--r--src/drivers/px4io/px4io_uploader.cpp22
-rw-r--r--src/modules/commander/commander.cpp24
-rw-r--r--src/modules/commander/state_machine_helper.cpp35
-rw-r--r--src/modules/commander/state_machine_helper.h2
m---------src/modules/ekf_att_pos_estimator/InertialNav0
-rw-r--r--src/modules/mavlink/mavlink_messages.cpp55
-rw-r--r--src/systemcmds/tests/test_sensors.c37
18 files changed, 244 insertions, 106 deletions
diff --git a/NuttX b/NuttX
-Subproject 2a94cc8e5babb7d5661aedc09201239d3964433
+Subproject 7e1b97bcf10d8495169eec355988ca5890bfd5d
diff --git a/ROMFS/px4fmu_common/init.d/3033_wingwing b/ROMFS/px4fmu_common/init.d/3033_wingwing
index fff4b58df..3cbbd555e 100644
--- a/ROMFS/px4fmu_common/init.d/3033_wingwing
+++ b/ROMFS/px4fmu_common/init.d/3033_wingwing
@@ -32,7 +32,7 @@ then
param set FW_THR_CRUISE 0.65
fi
-set MIXER FMU_Q
+set MIXER wingwing
# Provide ESC a constant 1000 us pulse
set PWM_OUTPUTS 4
set PWM_DISARMED 1000
diff --git a/ROMFS/px4fmu_common/mixers/wingwing.mix b/ROMFS/px4fmu_common/mixers/wingwing.mix
new file mode 100644
index 000000000..08333ba5c
--- /dev/null
+++ b/ROMFS/px4fmu_common/mixers/wingwing.mix
@@ -0,0 +1,51 @@
+Delta-wing mixer for PX4FMU
+===========================
+
+Designed for Wing Wing Z-84
+
+This file defines mixers suitable for controlling a delta wing aircraft using
+PX4FMU. The configuration assumes the elevon servos are connected to PX4FMU
+servo outputs 0 and 1 and the motor speed control to output 3. Output 2 is
+assumed to be unused.
+
+Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0
+(roll), 1 (pitch) and 3 (thrust).
+
+See the README for more information on the scaler format.
+
+Elevon mixers
+-------------
+Three scalers total (output, roll, pitch).
+
+On the assumption that the two elevon servos are physically reversed, the pitch
+input is inverted between the two servos.
+
+The scaling factor for roll inputs is adjusted to implement differential travel
+for the elevons.
+
+M: 2
+O: 10000 10000 0 -10000 10000
+S: 0 0 -6000 -6000 0 -10000 10000
+S: 0 1 6500 6500 0 -10000 10000
+
+M: 2
+O: 10000 10000 0 -10000 10000
+S: 0 0 -6000 -6000 0 -10000 10000
+S: 0 1 -6500 -6500 0 -10000 10000
+
+Output 2
+--------
+This mixer is empty.
+
+Z:
+
+Motor speed mixer
+-----------------
+Two scalers total (output, thrust).
+
+This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1)
+range. Inputs below zero are treated as zero.
+
+M: 1
+O: 10000 10000 0 -10000 10000
+S: 0 3 0 20000 -10000 -10000 10000
diff --git a/Tools/check_submodules.sh b/Tools/check_submodules.sh
index 7c3a3bfc2..cc6e7d1c0 100755
--- a/Tools/check_submodules.sh
+++ b/Tools/check_submodules.sh
@@ -8,12 +8,17 @@
if [ -d NuttX/nuttx ];
then
- STATUSRETVAL=$(git status --porcelain | grep -i "NuttX")
- if [ "$STATUSRETVAL" == "" ]; then
+ STATUSRETVAL=$(git submodule summary | grep -A20 -i "NuttX" | grep "<")
+ if [ -z "$STATUSRETVAL" ]; then
echo "Checked NuttX submodule, correct version found"
else
echo "NuttX sub repo not at correct version. Try 'git submodule update'"
echo "or follow instructions on http://pixhawk.org/dev/git/submodules"
+ echo ""
+ echo ""
+ echo "New commits required:"
+ echo "$(git submodule summary)"
+ echo ""
exit 1
fi
else
@@ -24,12 +29,17 @@ fi
if [ -d mavlink/include/mavlink/v1.0 ];
then
- STATUSRETVAL=$(git status --porcelain | grep -i "mavlink/include/mavlink/v1.0")
- if [ "$STATUSRETVAL" == "" ]; then
+ STATUSRETVAL=$(git submodule summary | grep -A20 -i "mavlink/include/mavlink/v1.0" | grep "<")
+ if [ -z "$STATUSRETVAL" ]; then
echo "Checked mavlink submodule, correct version found"
else
echo "mavlink sub repo not at correct version. Try 'git submodule update'"
echo "or follow instructions on http://pixhawk.org/dev/git/submodules"
+ echo ""
+ echo ""
+ echo "New commits required:"
+ echo "$(git submodule summary)"
+ echo ""
exit 1
fi
else
diff --git a/src/drivers/device/spi.cpp b/src/drivers/device/spi.cpp
index 0b232e158..1ab1dc699 100644
--- a/src/drivers/device/spi.cpp
+++ b/src/drivers/device/spi.cpp
@@ -133,26 +133,44 @@ SPI::probe()
int
SPI::transfer(uint8_t *send, uint8_t *recv, unsigned len)
{
- irqstate_t state;
+ int result;
if ((send == nullptr) && (recv == nullptr))
return -EINVAL;
+ LockMode mode = up_interrupt_context() ? LOCK_NONE : locking_mode;
+
/* lock the bus as required */
- if (!up_interrupt_context()) {
- switch (locking_mode) {
- default:
- case LOCK_PREEMPTION:
- state = irqsave();
- break;
- case LOCK_THREADS:
- SPI_LOCK(_dev, true);
- break;
- case LOCK_NONE:
- break;
+ switch (mode) {
+ default:
+ case LOCK_PREEMPTION:
+ {
+ irqstate_t state = irqsave();
+ result = _transfer(send, recv, len);
+ irqrestore(state);
}
+ break;
+ case LOCK_THREADS:
+ SPI_LOCK(_dev, true);
+ result = _transfer(send, recv, len);
+ SPI_LOCK(_dev, false);
+ break;
+ case LOCK_NONE:
+ result = _transfer(send, recv, len);
+ break;
}
+ return result;
+}
+void
+SPI::set_frequency(uint32_t frequency)
+{
+ _frequency = frequency;
+}
+
+int
+SPI::_transfer(uint8_t *send, uint8_t *recv, unsigned len)
+{
SPI_SETFREQUENCY(_dev, _frequency);
SPI_SETMODE(_dev, _mode);
SPI_SETBITS(_dev, 8);
@@ -164,27 +182,7 @@ SPI::transfer(uint8_t *send, uint8_t *recv, unsigned len)
/* and clean up */
SPI_SELECT(_dev, _device, false);
- if (!up_interrupt_context()) {
- switch (locking_mode) {
- default:
- case LOCK_PREEMPTION:
- irqrestore(state);
- break;
- case LOCK_THREADS:
- SPI_LOCK(_dev, false);
- break;
- case LOCK_NONE:
- break;
- }
- }
-
return OK;
}
-void
-SPI::set_frequency(uint32_t frequency)
-{
- _frequency = frequency;
-}
-
} // namespace device
diff --git a/src/drivers/device/spi.h b/src/drivers/device/spi.h
index 03986da1e..54849c8c3 100644
--- a/src/drivers/device/spi.h
+++ b/src/drivers/device/spi.h
@@ -131,6 +131,8 @@ private:
protected:
int _bus;
+
+ int _transfer(uint8_t *send, uint8_t *recv, unsigned len);
};
} // namespace device
diff --git a/src/drivers/drv_px4flow.h b/src/drivers/drv_px4flow.h
index bef02d54e..76ec55c3e 100644
--- a/src/drivers/drv_px4flow.h
+++ b/src/drivers/drv_px4flow.h
@@ -47,9 +47,17 @@
#define PX4FLOW_DEVICE_PATH "/dev/px4flow"
/**
+ * @addtogroup topics
+ * @{
+ */
+
+/**
* Optical flow in NED body frame in SI units.
*
* @see http://en.wikipedia.org/wiki/International_System_of_Units
+ *
+ * @warning If possible the usage of the raw flow and performing rotation-compensation
+ * using the autopilot angular rate estimate is recommended.
*/
struct px4flow_report {
@@ -57,14 +65,18 @@ struct px4flow_report {
int16_t flow_raw_x; /**< flow in pixels in X direction, not rotation-compensated */
int16_t flow_raw_y; /**< flow in pixels in Y direction, not rotation-compensated */
- float flow_comp_x_m; /**< speed over ground in meters, rotation-compensated */
- float flow_comp_y_m; /**< speed over ground in meters, rotation-compensated */
+ float flow_comp_x_m; /**< speed over ground in meters per second, rotation-compensated */
+ float flow_comp_y_m; /**< speed over ground in meters per second, rotation-compensated */
float ground_distance_m; /**< Altitude / distance to ground in meters */
uint8_t quality; /**< Quality of the measurement, 0: bad quality, 255: maximum quality */
uint8_t sensor_id; /**< id of the sensor emitting the flow value */
};
+/**
+ * @}
+ */
+
/*
* ObjDev tag for px4flow data.
*/
diff --git a/src/drivers/drv_range_finder.h b/src/drivers/drv_range_finder.h
index e45939b37..0f8362f58 100644
--- a/src/drivers/drv_range_finder.h
+++ b/src/drivers/drv_range_finder.h
@@ -51,6 +51,11 @@ enum RANGE_FINDER_TYPE {
};
/**
+ * @addtogroup topics
+ * @{
+ */
+
+/**
* range finder report structure. Reads from the device must be in multiples of this
* structure.
*/
@@ -64,6 +69,10 @@ struct range_finder_report {
uint8_t valid; /**< 1 == within sensor range, 0 = outside sensor range */
};
+/**
+ * @}
+ */
+
/*
* ObjDev tag for raw range finder data.
*/
diff --git a/src/drivers/drv_rc_input.h b/src/drivers/drv_rc_input.h
index 20763e265..47fa8fa59 100644
--- a/src/drivers/drv_rc_input.h
+++ b/src/drivers/drv_rc_input.h
@@ -68,6 +68,11 @@
#define RC_INPUT_RSSI_MAX 255
/**
+ * @addtogroup topics
+ * @{
+ */
+
+/**
* Input signal type, value is a control position from zero to 100
* percent.
*/
@@ -141,6 +146,10 @@ struct rc_input_values {
rc_input_t values[RC_INPUT_MAX_CHANNELS];
};
+/**
+ * @}
+ */
+
/*
* ObjDev tag for R/C inputs.
*/
diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index 401b65dd4..34dd63086 100644
--- a/src/drivers/gps/gps.cpp
+++ b/src/drivers/gps/gps.cpp
@@ -127,7 +127,7 @@ private:
/**
* Try to configure the GPS, handle outgoing communication to the GPS
*/
- void config();
+ void config();
/**
* Trampoline to the worker task
@@ -486,6 +486,8 @@ GPS::print_info()
warnx("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report_gps_pos.fix_type,
_report_gps_pos.satellites_used, (double)(hrt_absolute_time() - _report_gps_pos.timestamp_position) / 1000.0);
warnx("lat: %d, lon: %d, alt: %d", _report_gps_pos.lat, _report_gps_pos.lon, _report_gps_pos.alt);
+ warnx("vel: %.2fm/s, %.2fm/s, %.2fm/s", (double)_report_gps_pos.vel_n_m_s,
+ (double)_report_gps_pos.vel_e_m_s, (double)_report_gps_pos.vel_d_m_s);
warnx("eph: %.2fm, epv: %.2fm", (double)_report_gps_pos.eph, (double)_report_gps_pos.epv);
warnx("rate position: \t%6.2f Hz", (double)_Helper->get_position_update_rate());
warnx("rate velocity: \t%6.2f Hz", (double)_Helper->get_velocity_update_rate());
diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index 39cc32967..8cc1141aa 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -329,7 +329,7 @@ PX4FMU::init()
_task = task_spawn_cmd("fmuservo",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
- 2048,
+ 1600,
(main_t)&PX4FMU::task_main_trampoline,
nullptr);
diff --git a/src/drivers/px4io/px4io_uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp
index 7b6361a7c..d134c0246 100644
--- a/src/drivers/px4io/px4io_uploader.cpp
+++ b/src/drivers/px4io/px4io_uploader.cpp
@@ -39,6 +39,7 @@
#include <nuttx/config.h>
#include <sys/types.h>
+#include <stdlib.h>
#include <stdint.h>
#include <stdbool.h>
#include <assert.h>
@@ -413,11 +414,17 @@ static int read_with_retry(int fd, void *buf, size_t n)
int
PX4IO_Uploader::program(size_t fw_size)
{
- uint8_t file_buf[PROG_MULTI_MAX];
+ uint8_t *file_buf;
ssize_t count;
int ret;
size_t sent = 0;
+ file_buf = (uint8_t *)malloc(PROG_MULTI_MAX);
+ if (!file_buf) {
+ log("Can't allocate program buffer");
+ return -ENOMEM;
+ }
+
log("programming %u bytes...", (unsigned)fw_size);
ret = lseek(_fw_fd, 0, SEEK_SET);
@@ -425,8 +432,8 @@ PX4IO_Uploader::program(size_t fw_size)
while (sent < fw_size) {
/* get more bytes to program */
size_t n = fw_size - sent;
- if (n > sizeof(file_buf)) {
- n = sizeof(file_buf);
+ if (n > PROG_MULTI_MAX) {
+ n = PROG_MULTI_MAX;
}
count = read_with_retry(_fw_fd, file_buf, n);
@@ -438,8 +445,10 @@ PX4IO_Uploader::program(size_t fw_size)
(int)errno);
}
- if (count == 0)
+ if (count == 0) {
+ free(file_buf);
return OK;
+ }
sent += count;
@@ -455,9 +464,12 @@ PX4IO_Uploader::program(size_t fw_size)
ret = get_sync(1000);
- if (ret != OK)
+ if (ret != OK) {
+ free(file_buf);
return ret;
+ }
}
+ free(file_buf);
return OK;
}
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 2058cd92e..18761665a 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -286,15 +286,22 @@ int commander_main(int argc, char *argv[])
exit(0);
}
- if (!strcmp(argv[1], "status")) {
- if (thread_running) {
- warnx("\tcommander is running");
- print_status();
+ /* commands needing the app to run below */
+ if (!thread_running) {
+ warnx("\tcommander not started");
+ exit(1);
+ }
- } else {
- warnx("\tcommander not started");
- }
+ if (!strcmp(argv[1], "status")) {
+ print_status();
+ exit(0);
+ }
+ if (!strcmp(argv[1], "check")) {
+ int mavlink_fd_local = open(MAVLINK_LOG_DEVICE, 0);
+ int checkres = prearm_check(&status, mavlink_fd_local);
+ close(mavlink_fd_local);
+ warnx("FINAL RESULT: %s", (checkres == 0) ? "OK" : "FAILED");
exit(0);
}
@@ -303,7 +310,7 @@ int commander_main(int argc, char *argv[])
exit(0);
}
- if (!strcmp(argv[1], "2")) {
+ if (!strcmp(argv[1], "disarm")) {
arm_disarm(false, mavlink_fd, "command line");
exit(0);
}
@@ -324,6 +331,7 @@ void usage(const char *reason)
void print_status()
{
+ warnx("type: %s", (status.is_rotary_wing) ? "ROTARY" : "PLANE");
warnx("usb powered: %s", (on_usb_power) ? "yes" : "no");
/* read all relevant states */
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 6b96e3a3f..363f5e8eb 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -70,8 +70,6 @@
#endif
static const int ERROR = -1;
-static int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd);
-
// This array defines the arming state transitions. The rows are the new state, and the columns
// are the current state. Using new state and current state you can index into the array which
// will be true for a valid transition or false for a invalid transition. In some cases even
@@ -622,12 +620,13 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
{
int ret;
+ bool failed = false;
int fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
if (fd < 0) {
mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL SENSOR MISSING");
- ret = fd;
+ failed = true;
goto system_eval;
}
@@ -635,6 +634,7 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
if (ret != OK) {
mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL CALIBRATION");
+ failed = true;
goto system_eval;
}
@@ -644,33 +644,31 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
if (ret == sizeof(acc)) {
/* evaluate values */
- float accel_scale = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);
-
- if (accel_scale < 9.78f || accel_scale > 9.83f) {
- mavlink_log_info(mavlink_fd, "#audio: Accelerometer calibration recommended.");
- }
+ float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);
- if (accel_scale > 30.0f /* m/s^2 */) {
+ if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) {
mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL RANGE");
+ mavlink_log_info(mavlink_fd, "#audio: hold still while arming");
/* this is frickin' fatal */
- ret = ERROR;
+ failed = true;
goto system_eval;
- } else {
- ret = OK;
}
} else {
mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL READ");
/* this is frickin' fatal */
- ret = ERROR;
+ failed = true;
goto system_eval;
}
if (!status->is_rotary_wing) {
+
+ /* accel done, close it */
+ close(fd);
fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY);
- if (fd < 0) {
+ if (fd <= 0) {
mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED SENSOR MISSING");
- ret = fd;
+ failed = true;
goto system_eval;
}
@@ -679,20 +677,19 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
ret = read(fd, &diff_pres, sizeof(diff_pres));
if (ret == sizeof(diff_pres)) {
- if (fabsf(diff_pres.differential_pressure_filtered_pa > 5.0f)) {
+ if (fabsf(diff_pres.differential_pressure_filtered_pa > 6.0f)) {
mavlink_log_critical(mavlink_fd, "#audio: WARNING AIRSPEED CALIBRATION MISSING");
// XXX do not make this fatal yet
- ret = OK;
}
} else {
mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED READ");
/* this is frickin' fatal */
- ret = ERROR;
+ failed = true;
goto system_eval;
}
}
system_eval:
close(fd);
- return ret;
+ return (failed);
}
diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h
index 11072403e..ddc5bf154 100644
--- a/src/modules/commander/state_machine_helper.h
+++ b/src/modules/commander/state_machine_helper.h
@@ -67,4 +67,6 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub,
bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished);
+int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd);
+
#endif /* STATE_MACHINE_HELPER_H_ */
diff --git a/src/modules/ekf_att_pos_estimator/InertialNav b/src/modules/ekf_att_pos_estimator/InertialNav
new file mode 160000
+Subproject 8b65d755b8c4834f90a323172c25d91c45068e2
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 667be87b7..c7ad605c5 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -1051,10 +1051,16 @@ protected:
uint32_t mavlink_custom_mode;
get_mavlink_mode_state(&status, &pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
+ float out[8];
+
+ const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2;
+
+ /* scale outputs depending on system type */
if (mavlink_system.type == MAV_TYPE_QUADROTOR ||
mavlink_system.type == MAV_TYPE_HEXAROTOR ||
mavlink_system.type == MAV_TYPE_OCTOROTOR) {
- /* set number of valid outputs depending on vehicle type */
+ /* multirotors: set number of rotor outputs depending on type */
+
unsigned n;
switch (mavlink_system.type) {
@@ -1071,59 +1077,44 @@ protected:
break;
}
- /* scale / assign outputs depending on system type */
- float out[8];
-
for (unsigned i = 0; i < 8; i++) {
- if (i < n) {
- if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) {
- /* scale fake PWM out 900..2100 us to 0..1 for normal multirotors */
+ if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) {
+ if (i < n) {
+ /* scale PWM out 900..2100 us to 0..1 for rotors */
out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN);
} else {
- /* send 0 when disarmed */
- out[i] = 0.0f;
+ /* scale PWM out 900..2100 us to -1..1 for other channels */
+ out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2);
}
} else {
- out[i] = -1.0f;
+ /* send 0 when disarmed */
+ out[i] = 0.0f;
}
}
- mavlink_msg_hil_controls_send(_channel,
- hrt_absolute_time(),
- out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7],
- mavlink_base_mode,
- 0);
} else {
-
- /* fixed wing: scale all channels except throttle -1 .. 1
- * because we know that we set the mixers up this way
- */
-
- float out[8];
-
- const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2;
+ /* fixed wing: scale throttle to 0..1 and other channels to -1..1 */
for (unsigned i = 0; i < 8; i++) {
if (i != 3) {
- /* scale fake PWM out 900..2100 us to -1..+1 for normal channels */
+ /* scale PWM out 900..2100 us to -1..1 for normal channels */
out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2);
} else {
-
- /* scale fake PWM out 900..2100 us to 0..1 for throttle */
+ /* scale PWM out 900..2100 us to 0..1 for throttle */
out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN);
}
}
-
- mavlink_msg_hil_controls_send(_channel,
- hrt_absolute_time(),
- out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7],
- mavlink_base_mode,
- 0);
}
+
+ mavlink_msg_hil_controls_send(_channel,
+ hrt_absolute_time(),
+ out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7],
+ mavlink_base_mode,
+ 0);
}
}
};
diff --git a/src/systemcmds/tests/test_sensors.c b/src/systemcmds/tests/test_sensors.c
index 096106ff3..a4f17eebd 100644
--- a/src/systemcmds/tests/test_sensors.c
+++ b/src/systemcmds/tests/test_sensors.c
@@ -139,7 +139,14 @@ accel(int argc, char *argv[])
}
if (fabsf(buf.x) > 30.0f || fabsf(buf.y) > 30.0f || fabsf(buf.z) > 30.0f) {
- warnx("ACCEL1 acceleration values out of range!");
+ warnx("ACCEL acceleration values out of range!");
+ return ERROR;
+ }
+
+ float len = sqrtf(buf.x * buf.x + buf.y * buf.y + buf.z * buf.z);
+
+ if (len < 8.0f || len > 12.0f) {
+ warnx("ACCEL scale error!");
return ERROR;
}
@@ -186,6 +193,13 @@ accel1(int argc, char *argv[])
return ERROR;
}
+ float len = sqrtf(buf.x * buf.x + buf.y * buf.y + buf.z * buf.z);
+
+ if (len < 8.0f || len > 12.0f) {
+ warnx("ACCEL1 scale error!");
+ return ERROR;
+ }
+
/* Let user know everything is ok */
printf("\tOK: ACCEL1 passed all tests successfully\n");
@@ -225,6 +239,13 @@ gyro(int argc, char *argv[])
printf("\tGYRO rates: x:%8.4f\ty:%8.4f\tz:%8.4f rad/s\n", (double)buf.x, (double)buf.y, (double)buf.z);
}
+ float len = sqrtf(buf.x * buf.x + buf.y * buf.y + buf.z * buf.z);
+
+ if (len > 0.3f) {
+ warnx("GYRO scale error!");
+ return ERROR;
+ }
+
/* Let user know everything is ok */
printf("\tOK: GYRO passed all tests successfully\n");
close(fd);
@@ -263,6 +284,13 @@ gyro1(int argc, char *argv[])
printf("\tGYRO1 rates: x:%8.4f\ty:%8.4f\tz:%8.4f rad/s\n", (double)buf.x, (double)buf.y, (double)buf.z);
}
+ float len = sqrtf(buf.x * buf.x + buf.y * buf.y + buf.z * buf.z);
+
+ if (len > 0.3f) {
+ warnx("GYRO1 scale error!");
+ return ERROR;
+ }
+
/* Let user know everything is ok */
printf("\tOK: GYRO1 passed all tests successfully\n");
close(fd);
@@ -301,6 +329,13 @@ mag(int argc, char *argv[])
printf("\tMAG values: x:%8.4f\ty:%8.4f\tz:%8.4f\n", (double)buf.x, (double)buf.y, (double)buf.z);
}
+ float len = sqrtf(buf.x * buf.x + buf.y * buf.y + buf.z * buf.z);
+
+ if (len < 1.0f || len > 3.0f) {
+ warnx("MAG scale error!");
+ return ERROR;
+ }
+
/* Let user know everything is ok */
printf("\tOK: MAG passed all tests successfully\n");
close(fd);
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-03 23:17:41 +0000