aboutsummaryrefslogtreecommitdiff
path: root/src
diff options
context:
space:
mode:
Diffstat (limited to 'src')
-rw-r--r--src/drivers/airspeed/airspeed.h2
-rw-r--r--src/drivers/boards/px4fmu-v2/board_config.h17
-rw-r--r--src/drivers/drv_gpio.h4
-rw-r--r--src/drivers/ms5611/ms5611.cpp7
-rw-r--r--src/drivers/px4fmu/fmu.cpp484
-rw-r--r--src/modules/mavlink/missionlib.c10
-rw-r--r--src/systemcmds/pwm/pwm.c2
-rw-r--r--src/systemcmds/tests/module.mk5
-rw-r--r--src/systemcmds/tests/test_file.c (renamed from src/systemcmds/tests/tests_file.c)2
-rw-r--r--src/systemcmds/tests/test_param.c (renamed from src/systemcmds/tests/tests_param.c)2
-rw-r--r--src/systemcmds/tests/test_ppm_loopback.c178
-rw-r--r--src/systemcmds/tests/test_sensors.c92
-rw-r--r--src/systemcmds/tests/test_servo.c66
-rw-r--r--src/systemcmds/tests/test_uart_loopback.c70
-rw-r--r--src/systemcmds/tests/tests.h9
-rw-r--r--src/systemcmds/tests/tests_main.c14
16 files changed, 681 insertions, 283 deletions
diff --git a/src/drivers/airspeed/airspeed.h b/src/drivers/airspeed/airspeed.h
index 62c0d1f17..c341aa2c6 100644
--- a/src/drivers/airspeed/airspeed.h
+++ b/src/drivers/airspeed/airspeed.h
@@ -119,7 +119,7 @@ protected:
virtual int collect() = 0;
work_s _work;
- float _max_differential_pressure_pa;
+ float _max_differential_pressure_pa;
bool _sensor_ok;
int _measure_ticks;
bool _collect_phase;
diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h
index 586661b58..7ab63953f 100644
--- a/src/drivers/boards/px4fmu-v2/board_config.h
+++ b/src/drivers/boards/px4fmu-v2/board_config.h
@@ -81,6 +81,23 @@ __BEGIN_DECLS
#define GPIO_EXTI_ACCEL_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN4)
#define GPIO_EXTI_MPU_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTD|GPIO_PIN15)
+/* Data ready pins off */
+#define GPIO_GYRO_DRDY_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTB|GPIO_PIN0)
+#define GPIO_MAG_DRDY_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTB|GPIO_PIN1)
+#define GPIO_ACCEL_DRDY_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTB|GPIO_PIN4)
+#define GPIO_EXTI_MPU_DRDY (GPIO_INPUT|GPIO_PULLDOWN|GPIO_EXTI|GPIO_PORTD|GPIO_PIN15)
+
+/* SPI1 off */
+#define GPIO_SPI1_SCK_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTA|GPIO_PIN5)
+#define GPIO_SPI1_MISO_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTA|GPIO_PIN6)
+#define GPIO_SPI1_MOSI_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTA|GPIO_PIN7)
+
+/* SPI1 chip selects off */
+#define GPIO_SPI_CS_GYRO_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN13)
+#define GPIO_SPI_CS_ACCEL_MAG_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN15)
+#define GPIO_SPI_CS_BARO_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTD|GPIO_PIN7)
+#define GPIO_SPI_CS_MPU (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN2)
+
/* SPI chip selects */
#define GPIO_SPI_CS_GYRO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13)
#define GPIO_SPI_CS_ACCEL_MAG (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15)
diff --git a/src/drivers/drv_gpio.h b/src/drivers/drv_gpio.h
index 37af26d52..f60964c2b 100644
--- a/src/drivers/drv_gpio.h
+++ b/src/drivers/drv_gpio.h
@@ -46,7 +46,7 @@
/*
* PX4FMU GPIO numbers.
*
- * For shared pins, alternate function 1 selects the non-GPIO mode
+ * For shared pins, alternate function 1 selects the non-GPIO mode
* (USART2, CAN2, etc.)
*/
# define GPIO_EXT_1 (1<<0) /**< high-power GPIO 1 */
@@ -144,4 +144,6 @@
/** read all the GPIOs and return their values in *(uint32_t *)arg */
#define GPIO_GET GPIOC(12)
+#define GPIO_SENSOR_RAIL_RESET GPIOC(13)
+
#endif /* _DRV_GPIO_H */ \ No newline at end of file
diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp
index 938786d3f..87788824a 100644
--- a/src/drivers/ms5611/ms5611.cpp
+++ b/src/drivers/ms5611/ms5611.cpp
@@ -420,8 +420,11 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
return _reports->size();
case SENSORIOCRESET:
- /* XXX implement this */
- return -EINVAL;
+ /*
+ * Since we are initialized, we do not need to do anything, since the
+ * PROM is correctly read and the part does not need to be configured.
+ */
+ return OK;
case BAROIOCSMSLPRESSURE:
diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index 0441566e9..aab532514 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -164,6 +164,7 @@ private:
static const unsigned _ngpio;
void gpio_reset(void);
+ void sensor_reset(int ms);
void gpio_set_function(uint32_t gpios, int function);
void gpio_write(uint32_t gpios, int function);
uint32_t gpio_read(void);
@@ -226,10 +227,10 @@ PX4FMU::PX4FMU() :
_armed(false),
_pwm_on(false),
_mixers(nullptr),
- _failsafe_pwm({0}),
- _disarmed_pwm({0}),
- _num_failsafe_set(0),
- _num_disarmed_set(0)
+ _failsafe_pwm( {0}),
+ _disarmed_pwm( {0}),
+ _num_failsafe_set(0),
+ _num_disarmed_set(0)
{
for (unsigned i = 0; i < _max_actuators; i++) {
_min_pwm[i] = PWM_DEFAULT_MIN;
@@ -293,11 +294,11 @@ PX4FMU::init()
/* start the IO interface task */
_task = task_spawn_cmd("fmuservo",
- SCHED_DEFAULT,
- SCHED_PRIORITY_DEFAULT,
- 2048,
- (main_t)&PX4FMU::task_main_trampoline,
- nullptr);
+ SCHED_DEFAULT,
+ SCHED_PRIORITY_DEFAULT,
+ 2048,
+ (main_t)&PX4FMU::task_main_trampoline,
+ nullptr);
if (_task < 0) {
debug("task start failed: %d", errno);
@@ -326,7 +327,7 @@ PX4FMU::set_mode(Mode mode)
switch (mode) {
case MODE_2PWM: // v1 multi-port with flow control lines as PWM
debug("MODE_2PWM");
-
+
/* default output rates */
_pwm_default_rate = 50;
_pwm_alt_rate = 50;
@@ -340,7 +341,7 @@ PX4FMU::set_mode(Mode mode)
case MODE_4PWM: // v1 multi-port as 4 PWM outs
debug("MODE_4PWM");
-
+
/* default output rates */
_pwm_default_rate = 50;
_pwm_alt_rate = 50;
@@ -354,7 +355,7 @@ PX4FMU::set_mode(Mode mode)
case MODE_6PWM: // v2 PWMs as 6 PWM outs
debug("MODE_6PWM");
-
+
/* default output rates */
_pwm_default_rate = 50;
_pwm_alt_rate = 50;
@@ -396,6 +397,7 @@ PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate
// get the channel mask for this rate group
uint32_t mask = up_pwm_servo_get_rate_group(group);
+
if (mask == 0)
continue;
@@ -409,6 +411,7 @@ PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate
// not a legal map, bail
return -EINVAL;
}
+
} else {
// set it - errors here are unexpected
if (alt != 0) {
@@ -416,6 +419,7 @@ PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate
warn("rate group set alt failed");
return -EINVAL;
}
+
} else {
if (up_pwm_servo_set_rate_group_update(group, _pwm_default_rate) != OK) {
warn("rate group set default failed");
@@ -425,6 +429,7 @@ PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate
}
}
}
+
_pwm_alt_rate_channels = rate_map;
_pwm_default_rate = default_rate;
_pwm_alt_rate = alt_rate;
@@ -471,7 +476,7 @@ PX4FMU::task_main()
memset(&controls_effective, 0, sizeof(controls_effective));
/* advertise the effective control inputs */
_t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1),
- &controls_effective);
+ &controls_effective);
pollfd fds[2];
fds[0].fd = _t_actuators;
@@ -503,6 +508,7 @@ PX4FMU::task_main()
* We always mix at max rate; some channels may update slower.
*/
unsigned max_rate = (_pwm_default_rate > _pwm_alt_rate) ? _pwm_default_rate : _pwm_alt_rate;
+
if (_current_update_rate != max_rate) {
_current_update_rate = max_rate;
int update_rate_in_ms = int(1000 / _current_update_rate);
@@ -511,6 +517,7 @@ PX4FMU::task_main()
if (update_rate_in_ms < 2) {
update_rate_in_ms = 2;
}
+
/* reject slower than 10 Hz updates */
if (update_rate_in_ms > 100) {
update_rate_in_ms = 100;
@@ -532,6 +539,7 @@ PX4FMU::task_main()
log("poll error %d", errno);
usleep(1000000);
continue;
+
} else if (ret == 0) {
/* timeout: no control data, switch to failsafe values */
// warnx("no PWM: failsafe");
@@ -553,12 +561,15 @@ PX4FMU::task_main()
case MODE_2PWM:
num_outputs = 2;
break;
+
case MODE_4PWM:
num_outputs = 4;
break;
+
case MODE_6PWM:
num_outputs = 6;
break;
+
default:
num_outputs = 0;
break;
@@ -572,9 +583,9 @@ PX4FMU::task_main()
for (unsigned i = 0; i < num_outputs; i++) {
/* last resort: catch NaN, INF and out-of-band errors */
if (i >= outputs.noutputs ||
- !isfinite(outputs.output[i]) ||
- outputs.output[i] < -1.0f ||
- outputs.output[i] > 1.0f) {
+ !isfinite(outputs.output[i]) ||
+ outputs.output[i] < -1.0f ||
+ outputs.output[i] > 1.0f) {
/*
* Value is NaN, INF or out of band - set to the minimum value.
* This will be clearly visible on the servo status and will limit the risk of accidentally
@@ -592,6 +603,7 @@ PX4FMU::task_main()
for (unsigned i = 0; i < num_outputs; i++) {
controls_effective.control_effective[i] = (float)pwm_limited[i];
}
+
orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &controls_effective);
/* output to the servos */
@@ -613,11 +625,13 @@ PX4FMU::task_main()
/* update the armed status and check that we're not locked down */
bool set_armed = aa.armed && !aa.lockdown;
+
if (_armed != set_armed)
_armed = set_armed;
/* update PWM status if armed or if disarmed PWM values are set */
bool pwm_on = (aa.armed || _num_disarmed_set > 0);
+
if (_pwm_on != pwm_on) {
_pwm_on = pwm_on;
up_pwm_servo_arm(pwm_on);
@@ -626,25 +640,31 @@ PX4FMU::task_main()
}
#ifdef HRT_PPM_CHANNEL
+
// see if we have new PPM input data
if (ppm_last_valid_decode != rc_in.timestamp) {
// we have a new PPM frame. Publish it.
rc_in.channel_count = ppm_decoded_channels;
+
if (rc_in.channel_count > RC_INPUT_MAX_CHANNELS) {
rc_in.channel_count = RC_INPUT_MAX_CHANNELS;
}
- for (uint8_t i=0; i<rc_in.channel_count; i++) {
+
+ for (uint8_t i = 0; i < rc_in.channel_count; i++) {
rc_in.values[i] = ppm_buffer[i];
}
+
rc_in.timestamp = ppm_last_valid_decode;
/* lazily advertise on first publication */
if (to_input_rc == 0) {
to_input_rc = orb_advertise(ORB_ID(input_rc), &rc_in);
- } else {
+
+ } else {
orb_publish(ORB_ID(input_rc), to_input_rc, &rc_in);
}
}
+
#endif
}
@@ -753,142 +773,176 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
break;
case PWM_SERVO_SET_FAILSAFE_PWM: {
- struct pwm_output_values *pwm = (struct pwm_output_values*)arg;
- /* discard if too many values are sent */
- if (pwm->channel_count > _max_actuators) {
- ret = -EINVAL;
- break;
- }
- for (unsigned i = 0; i < pwm->channel_count; i++) {
- if (pwm->values[i] == 0) {
- /* ignore 0 */
- } else if (pwm->values[i] > PWM_HIGHEST_MAX) {
- _failsafe_pwm[i] = PWM_HIGHEST_MAX;
- } else if (pwm->values[i] < PWM_LOWEST_MIN) {
- _failsafe_pwm[i] = PWM_LOWEST_MIN;
- } else {
- _failsafe_pwm[i] = pwm->values[i];
+ struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
+
+ /* discard if too many values are sent */
+ if (pwm->channel_count > _max_actuators) {
+ ret = -EINVAL;
+ break;
}
- }
- /*
- * update the counter
- * this is needed to decide if disarmed PWM output should be turned on or not
- */
- _num_failsafe_set = 0;
- for (unsigned i = 0; i < _max_actuators; i++) {
- if (_failsafe_pwm[i] > 0)
- _num_failsafe_set++;
+ for (unsigned i = 0; i < pwm->channel_count; i++) {
+ if (pwm->values[i] == 0) {
+ /* ignore 0 */
+ } else if (pwm->values[i] > PWM_HIGHEST_MAX) {
+ _failsafe_pwm[i] = PWM_HIGHEST_MAX;
+
+ } else if (pwm->values[i] < PWM_LOWEST_MIN) {
+ _failsafe_pwm[i] = PWM_LOWEST_MIN;
+
+ } else {
+ _failsafe_pwm[i] = pwm->values[i];
+ }
+ }
+
+ /*
+ * update the counter
+ * this is needed to decide if disarmed PWM output should be turned on or not
+ */
+ _num_failsafe_set = 0;
+
+ for (unsigned i = 0; i < _max_actuators; i++) {
+ if (_failsafe_pwm[i] > 0)
+ _num_failsafe_set++;
+ }
+
+ break;
}
- break;
- }
+
case PWM_SERVO_GET_FAILSAFE_PWM: {
- struct pwm_output_values *pwm = (struct pwm_output_values*)arg;
- for (unsigned i = 0; i < _max_actuators; i++) {
- pwm->values[i] = _failsafe_pwm[i];
- }
- pwm->channel_count = _max_actuators;
- break;
- }
+ struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
- case PWM_SERVO_SET_DISARMED_PWM: {
- struct pwm_output_values *pwm = (struct pwm_output_values*)arg;
- /* discard if too many values are sent */
- if (pwm->channel_count > _max_actuators) {
- ret = -EINVAL;
+ for (unsigned i = 0; i < _max_actuators; i++) {
+ pwm->values[i] = _failsafe_pwm[i];
+ }
+
+ pwm->channel_count = _max_actuators;
break;
}
- for (unsigned i = 0; i < pwm->channel_count; i++) {
- if (pwm->values[i] == 0) {
- /* ignore 0 */
- } else if (pwm->values[i] > PWM_HIGHEST_MAX) {
- _disarmed_pwm[i] = PWM_HIGHEST_MAX;
- } else if (pwm->values[i] < PWM_LOWEST_MIN) {
- _disarmed_pwm[i] = PWM_LOWEST_MIN;
- } else {
- _disarmed_pwm[i] = pwm->values[i];
+
+ case PWM_SERVO_SET_DISARMED_PWM: {
+ struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
+
+ /* discard if too many values are sent */
+ if (pwm->channel_count > _max_actuators) {
+ ret = -EINVAL;
+ break;
}
- }
- /*
- * update the counter
- * this is needed to decide if disarmed PWM output should be turned on or not
- */
- _num_disarmed_set = 0;
- for (unsigned i = 0; i < _max_actuators; i++) {
- if (_disarmed_pwm[i] > 0)
- _num_disarmed_set++;
+ for (unsigned i = 0; i < pwm->channel_count; i++) {
+ if (pwm->values[i] == 0) {
+ /* ignore 0 */
+ } else if (pwm->values[i] > PWM_HIGHEST_MAX) {
+ _disarmed_pwm[i] = PWM_HIGHEST_MAX;
+
+ } else if (pwm->values[i] < PWM_LOWEST_MIN) {
+ _disarmed_pwm[i] = PWM_LOWEST_MIN;
+
+ } else {
+ _disarmed_pwm[i] = pwm->values[i];
+ }
+ }
+
+ /*
+ * update the counter
+ * this is needed to decide if disarmed PWM output should be turned on or not
+ */
+ _num_disarmed_set = 0;
+
+ for (unsigned i = 0; i < _max_actuators; i++) {
+ if (_disarmed_pwm[i] > 0)
+ _num_disarmed_set++;
+ }
+
+ break;
}
- break;
- }
+
case PWM_SERVO_GET_DISARMED_PWM: {
- struct pwm_output_values *pwm = (struct pwm_output_values*)arg;
- for (unsigned i = 0; i < _max_actuators; i++) {
- pwm->values[i] = _disarmed_pwm[i];
- }
- pwm->channel_count = _max_actuators;
- break;
- }
+ struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
- case PWM_SERVO_SET_MIN_PWM: {
- struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
- /* discard if too many values are sent */
- if (pwm->channel_count > _max_actuators) {
- ret = -EINVAL;
+ for (unsigned i = 0; i < _max_actuators; i++) {
+ pwm->values[i] = _disarmed_pwm[i];
+ }
+
+ pwm->channel_count = _max_actuators;
break;
}
- for (unsigned i = 0; i < pwm->channel_count; i++) {
- if (pwm->values[i] == 0) {
- /* ignore 0 */
- } else if (pwm->values[i] > PWM_HIGHEST_MIN) {
- _min_pwm[i] = PWM_HIGHEST_MIN;
- } else if (pwm->values[i] < PWM_LOWEST_MIN) {
- _min_pwm[i] = PWM_LOWEST_MIN;
- } else {
- _min_pwm[i] = pwm->values[i];
+
+ case PWM_SERVO_SET_MIN_PWM: {
+ struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
+
+ /* discard if too many values are sent */
+ if (pwm->channel_count > _max_actuators) {
+ ret = -EINVAL;
+ break;
}
+
+ for (unsigned i = 0; i < pwm->channel_count; i++) {
+ if (pwm->values[i] == 0) {
+ /* ignore 0 */
+ } else if (pwm->values[i] > PWM_HIGHEST_MIN) {
+ _min_pwm[i] = PWM_HIGHEST_MIN;
+
+ } else if (pwm->values[i] < PWM_LOWEST_MIN) {
+ _min_pwm[i] = PWM_LOWEST_MIN;
+
+ } else {
+ _min_pwm[i] = pwm->values[i];
+ }
+ }
+
+ break;
}
- break;
- }
+
case PWM_SERVO_GET_MIN_PWM: {
- struct pwm_output_values *pwm = (struct pwm_output_values*)arg;
- for (unsigned i = 0; i < _max_actuators; i++) {
- pwm->values[i] = _min_pwm[i];
- }
- pwm->channel_count = _max_actuators;
- arg = (unsigned long)&pwm;
- break;
- }
+ struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
- case PWM_SERVO_SET_MAX_PWM: {
- struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
- /* discard if too many values are sent */
- if (pwm->channel_count > _max_actuators) {
- ret = -EINVAL;
+ for (unsigned i = 0; i < _max_actuators; i++) {
+ pwm->values[i] = _min_pwm[i];
+ }
+
+ pwm->channel_count = _max_actuators;
+ arg = (unsigned long)&pwm;
break;
}
- for (unsigned i = 0; i < pwm->channel_count; i++) {
- if (pwm->values[i] == 0) {
- /* ignore 0 */
- } else if (pwm->values[i] < PWM_LOWEST_MAX) {
- _max_pwm[i] = PWM_LOWEST_MAX;
- } else if (pwm->values[i] > PWM_HIGHEST_MAX) {
- _max_pwm[i] = PWM_HIGHEST_MAX;
- } else {
- _max_pwm[i] = pwm->values[i];
+
+ case PWM_SERVO_SET_MAX_PWM: {
+ struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
+
+ /* discard if too many values are sent */
+ if (pwm->channel_count > _max_actuators) {
+ ret = -EINVAL;
+ break;
+ }
+
+ for (unsigned i = 0; i < pwm->channel_count; i++) {
+ if (pwm->values[i] == 0) {
+ /* ignore 0 */
+ } else if (pwm->values[i] < PWM_LOWEST_MAX) {
+ _max_pwm[i] = PWM_LOWEST_MAX;
+
+ } else if (pwm->values[i] > PWM_HIGHEST_MAX) {
+ _max_pwm[i] = PWM_HIGHEST_MAX;
+
+ } else {
+ _max_pwm[i] = pwm->values[i];
+ }
}
+
+ break;
}
- break;
- }
+
case PWM_SERVO_GET_MAX_PWM: {
- struct pwm_output_values *pwm = (struct pwm_output_values*)arg;
- for (unsigned i = 0; i < _max_actuators; i++) {
- pwm->values[i] = _max_pwm[i];
+ struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
+
+ for (unsigned i = 0; i < _max_actuators; i++) {
+ pwm->values[i] = _max_pwm[i];
+ }
+
+ pwm->channel_count = _max_actuators;
+ arg = (unsigned long)&pwm;
+ break;
}
- pwm->channel_count = _max_actuators;
- arg = (unsigned long)&pwm;
- break;
- }
case PWM_SERVO_SET(5):
case PWM_SERVO_SET(4):
@@ -910,6 +964,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
case PWM_SERVO_SET(0):
if (arg <= 2100) {
up_pwm_servo_set(cmd - PWM_SERVO_SET(0), arg);
+
} else {
ret = -EINVAL;
}
@@ -946,18 +1001,21 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
*(uint32_t *)arg = up_pwm_servo_get_rate_group(cmd - PWM_SERVO_GET_RATEGROUP(0));
break;
- case PWM_SERVO_GET_COUNT:
+ case PWM_SERVO_GET_COUNT:
case MIXERIOCGETOUTPUTCOUNT:
switch (_mode) {
case MODE_6PWM:
*(unsigned *)arg = 6;
break;
+
case MODE_4PWM:
*(unsigned *)arg = 4;
break;
+
case MODE_2PWM:
*(unsigned *)arg = 2;
break;
+
default:
ret = -EINVAL;
break;
@@ -1015,6 +1073,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
ret = -EINVAL;
}
}
+
break;
}
@@ -1049,10 +1108,81 @@ PX4FMU::write(file *filp, const char *buffer, size_t len)
for (uint8_t i = 0; i < count; i++) {
up_pwm_servo_set(i, values[i]);
}
+
return count * 2;
}
void
+PX4FMU::sensor_reset(int ms)
+{
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
+
+ if (ms < 1) {
+ ms = 1;
+ }
+
+ /* disable SPI bus */
+ stm32_configgpio(GPIO_SPI_CS_GYRO_OFF);
+ stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG_OFF);
+ stm32_configgpio(GPIO_SPI_CS_BARO_OFF);
+
+ stm32_gpiowrite(GPIO_SPI_CS_GYRO_OFF, 0);
+ stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG_OFF, 0);
+ stm32_gpiowrite(GPIO_SPI_CS_BARO_OFF, 0);
+
+ stm32_configgpio(GPIO_SPI1_SCK_OFF);
+ stm32_configgpio(GPIO_SPI1_MISO_OFF);
+ stm32_configgpio(GPIO_SPI1_MOSI_OFF);
+
+ stm32_gpiowrite(GPIO_SPI1_SCK_OFF, 0);
+ stm32_gpiowrite(GPIO_SPI1_MISO_OFF, 0);
+ stm32_gpiowrite(GPIO_SPI1_MOSI_OFF, 0);
+
+ stm32_configgpio(GPIO_GYRO_DRDY_OFF);
+ stm32_configgpio(GPIO_MAG_DRDY_OFF);
+ stm32_configgpio(GPIO_ACCEL_DRDY_OFF);
+
+ stm32_gpiowrite(GPIO_GYRO_DRDY_OFF, 0);
+ stm32_gpiowrite(GPIO_MAG_DRDY_OFF, 0);
+ stm32_gpiowrite(GPIO_ACCEL_DRDY_OFF, 0);
+
+ /* set the sensor rail off */
+ stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN);
+ stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 0);
+
+ /* wait for the sensor rail to reach GND */
+ usleep(ms * 1000);
+ warnx("reset done, %d ms", ms);
+
+ /* re-enable power */
+
+ /* switch the sensor rail back on */
+ stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 1);
+
+ /* wait a bit before starting SPI, different times didn't influence results */
+ usleep(100);
+
+ /* reconfigure the SPI pins */
+#ifdef CONFIG_STM32_SPI1
+ stm32_configgpio(GPIO_SPI_CS_GYRO);
+ stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG);
+ stm32_configgpio(GPIO_SPI_CS_BARO);
+ stm32_configgpio(GPIO_SPI_CS_MPU);
+
+ /* De-activate all peripherals,
+ * required for some peripheral
+ * state machines
+ */
+ stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
+ stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
+ stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
+ stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
+#endif
+#endif
+}
+
+
+void
PX4FMU::gpio_reset(void)
{
/*
@@ -1062,6 +1192,7 @@ PX4FMU::gpio_reset(void)
for (unsigned i = 0; i < _ngpio; i++) {
if (_gpio_tab[i].input != 0) {
stm32_configgpio(_gpio_tab[i].input);
+
} else if (_gpio_tab[i].output != 0) {
stm32_configgpio(_gpio_tab[i].output);
}
@@ -1078,6 +1209,7 @@ void
PX4FMU::gpio_set_function(uint32_t gpios, int function)
{
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
+
/*
* GPIOs 0 and 1 must have the same direction as they are buffered
* by a shared 2-port driver. Any attempt to set either sets both.
@@ -1089,6 +1221,7 @@ PX4FMU::gpio_set_function(uint32_t gpios, int function)
if (GPIO_SET_OUTPUT == function)
stm32_gpiowrite(GPIO_GPIO_DIR, 1);
}
+
#endif
/* configure selected GPIOs as required */
@@ -1113,9 +1246,11 @@ PX4FMU::gpio_set_function(uint32_t gpios, int function)
}
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
+
/* flip buffer to input mode if required */
if ((GPIO_SET_INPUT == function) && (gpios & 3))
stm32_gpiowrite(GPIO_GPIO_DIR, 0);
+
#endif
}
@@ -1154,6 +1289,10 @@ PX4FMU::gpio_ioctl(struct file *filp, int cmd, unsigned long arg)
gpio_reset();
break;
+ case GPIO_SENSOR_RAIL_RESET:
+ sensor_reset(arg);
+ break;
+
case GPIO_SET_OUTPUT:
case GPIO_SET_INPUT:
case GPIO_SET_ALT_1:
@@ -1227,8 +1366,9 @@ fmu_new_mode(PortMode new_mode)
#endif
break;
- /* mixed modes supported on v1 board only */
+ /* mixed modes supported on v1 board only */
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
+
case PORT_FULL_SERIAL:
/* set all multi-GPIOs to serial mode */
gpio_bits = GPIO_MULTI_1 | GPIO_MULTI_2 | GPIO_MULTI_3 | GPIO_MULTI_4;
@@ -1251,6 +1391,7 @@ fmu_new_mode(PortMode new_mode)
servo_mode = PX4FMU::MODE_2PWM;
break;
#endif
+
default:
return -1;
}
@@ -1305,14 +1446,30 @@ fmu_stop(void)
}
void
+sensor_reset(int ms)
+{
+ int fd;
+ int ret;
+
+ fd = open(PX4FMU_DEVICE_PATH, O_RDWR);
+
+ if (fd < 0)
+ errx(1, "open fail");
+
+ if (ioctl(fd, GPIO_SENSOR_RAIL_RESET, ms) < 0)
+ err(1, "servo arm failed");
+
+}
+
+void
test(void)
{
int fd;
- unsigned servo_count = 0;
+ unsigned servo_count = 0;
unsigned pwm_value = 1000;
int direction = 1;
int ret;
-
+
fd = open(PX4FMU_DEVICE_PATH, O_RDWR);
if (fd < 0)
@@ -1320,9 +1477,9 @@ test(void)
if (ioctl(fd, PWM_SERVO_ARM, 0) < 0) err(1, "servo arm failed");
- if (ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count) != 0) {
- err(1, "Unable to get servo count\n");
- }
+ if (ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count) != 0) {
+ err(1, "Unable to get servo count\n");
+ }
warnx("Testing %u servos", (unsigned)servo_count);
@@ -1335,32 +1492,38 @@ test(void)
for (;;) {
/* sweep all servos between 1000..2000 */
servo_position_t servos[servo_count];
+
for (unsigned i = 0; i < servo_count; i++)
servos[i] = pwm_value;
- if (direction == 1) {
- // use ioctl interface for one direction
- for (unsigned i=0; i < servo_count; i++) {
- if (ioctl(fd, PWM_SERVO_SET(i), servos[i]) < 0) {
- err(1, "servo %u set failed", i);
- }
- }
- } else {
- // and use write interface for the other direction
- ret = write(fd, servos, sizeof(servos));
- if (ret != (int)sizeof(servos))
- err(1, "error writing PWM servo data, wrote %u got %d", sizeof(servos), ret);
- }
+ if (direction == 1) {
+ // use ioctl interface for one direction
+ for (unsigned i = 0; i < servo_count; i++) {
+ if (ioctl(fd, PWM_SERVO_SET(i), servos[i]) < 0) {
+ err(1, "servo %u set failed", i);
+ }
+ }
+
+ } else {
+ // and use write interface for the other direction
+ ret = write(fd, servos, sizeof(servos));
+
+ if (ret != (int)sizeof(servos))
+ err(1, "error writing PWM servo data, wrote %u got %d", sizeof(servos), ret);
+ }
if (direction > 0) {
if (pwm_value < 2000) {
pwm_value++;
+
} else {
direction = -1;
}
+
} else {
if (pwm_value > 1000) {
pwm_value--;
+
} else {
direction = 1;
}
@@ -1372,6 +1535,7 @@ test(void)
if (ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&value))
err(1, "error reading PWM servo %d", i);
+
if (value != servos[i])
errx(1, "servo %d readback error, got %u expected %u", i, value, servos[i]);
}
@@ -1379,12 +1543,14 @@ test(void)
/* Check if user wants to quit */
char c;
ret = poll(&fds, 1, 0);
+
if (ret > 0) {
read(0, &c, 1);
+
if (c == 0x03 || c == 0x63 || c == 'q') {
warnx("User abort\n");
- break;
+ break;
}
}
}
@@ -1457,6 +1623,7 @@ fmu_main(int argc, char *argv[])
new_mode = PORT_FULL_PWM;
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
+
} else if (!strcmp(verb, "mode_serial")) {
new_mode = PORT_FULL_SERIAL;
@@ -1489,11 +1656,24 @@ fmu_main(int argc, char *argv[])
if (!strcmp(verb, "fake"))
fake(argc - 1, argv + 1);
+ if (!strcmp(verb, "sensor_reset")) {
+ if (argc > 2) {
+ int reset_time = strtol(argv[2], 0, 0);
+ sensor_reset(reset_time);
+
+ } else {
+ sensor_reset(0);
+ warnx("resettet default time");
+ }
+ exit(0);
+ }
+
+
fprintf(stderr, "FMU: unrecognised command, try:\n");
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
fprintf(stderr, " mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, test\n");
#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
- fprintf(stderr, " mode_gpio, mode_pwm, test\n");
+ fprintf(stderr, " mode_gpio, mode_pwm, test, sensor_reset [milliseconds]\n");
#endif
exit(1);
}
diff --git a/src/modules/mavlink/missionlib.c b/src/modules/mavlink/missionlib.c
index fa23f996f..124b3b2ae 100644
--- a/src/modules/mavlink/missionlib.c
+++ b/src/modules/mavlink/missionlib.c
@@ -276,7 +276,7 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
next_setpoint_index = index + 1;
}
- while (next_setpoint_index < wpm->size - 1) {
+ while (next_setpoint_index < wpm->size) {
if (wpm->waypoints[next_setpoint_index].frame == (int)MAV_FRAME_GLOBAL && (wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_WAYPOINT ||
wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_TURNS ||
@@ -301,7 +301,7 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
sp.lon = wpm->waypoints[last_setpoint_index].y * 1e7f;
sp.altitude = wpm->waypoints[last_setpoint_index].z;
sp.altitude_is_relative = false;
- sp.yaw = (wpm->waypoints[last_setpoint_index].param4 / 180.0f) * M_PI_F - M_PI_F;
+ sp.yaw = _wrap_pi(wpm->waypoints[last_setpoint_index].param4 / 180.0f * M_PI_F);
set_special_fields(wpm->waypoints[last_setpoint_index].param1,
wpm->waypoints[last_setpoint_index].param2,
wpm->waypoints[last_setpoint_index].param3,
@@ -317,7 +317,7 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
sp.lon = wpm->waypoints[next_setpoint_index].y * 1e7f;
sp.altitude = wpm->waypoints[next_setpoint_index].z;
sp.altitude_is_relative = false;
- sp.yaw = (wpm->waypoints[next_setpoint_index].param4 / 180.0f) * M_PI_F - M_PI_F;
+ sp.yaw = _wrap_pi(wpm->waypoints[next_setpoint_index].param4 / 180.0f * M_PI_F);
set_special_fields(wpm->waypoints[next_setpoint_index].param1,
wpm->waypoints[next_setpoint_index].param2,
wpm->waypoints[next_setpoint_index].param3,
@@ -343,7 +343,7 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
sp.lon = param6_lon_y * 1e7f;
sp.altitude = param7_alt_z;
sp.altitude_is_relative = true;
- sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F;
+ sp.yaw = _wrap_pi(param4 / 180.0f * M_PI_F);
set_special_fields(param1, param2, param3, param4, command, &sp);
/* Initialize publication if necessary */
@@ -364,7 +364,7 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
sp.x = param5_lat_x;
sp.y = param6_lon_y;
sp.z = param7_alt_z;
- sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F;
+ sp.yaw = _wrap_pi(param4 / 180.0f * M_PI_F);
/* Initialize publication if necessary */
if (local_position_setpoint_pub < 0) {
diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c
index 898c04cd5..7c23f38c2 100644
--- a/src/systemcmds/pwm/pwm.c
+++ b/src/systemcmds/pwm/pwm.c
@@ -210,7 +210,7 @@ pwm_main(int argc, char *argv[])
err(1, "PWM_SERVO_GET_COUNT");
if (!strcmp(argv[1], "arm")) {
- /* tell IO that its ok to disable its safety with the switch */
+ /* tell safety that its ok to disable it with the switch */
ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0);
if (ret != OK)
err(1, "PWM_SERVO_SET_ARM_OK");
diff --git a/src/systemcmds/tests/module.mk b/src/systemcmds/tests/module.mk
index 6729ce4ae..5d5fe50d3 100644
--- a/src/systemcmds/tests/module.mk
+++ b/src/systemcmds/tests/module.mk
@@ -24,6 +24,7 @@ SRCS = test_adc.c \
test_uart_loopback.c \
test_uart_send.c \
test_mixer.cpp \
- tests_file.c \
+ test_file.c \
tests_main.c \
- tests_param.c
+ test_param.c \
+ test_ppm_loopback.c
diff --git a/src/systemcmds/tests/tests_file.c b/src/systemcmds/tests/test_file.c
index 7f3a654bf..798724cf1 100644
--- a/src/systemcmds/tests/tests_file.c
+++ b/src/systemcmds/tests/test_file.c
@@ -32,7 +32,7 @@
****************************************************************************/
/**
- * @file tests_file.c
+ * @file test_file.c
*
* File write test.
*/
diff --git a/src/systemcmds/tests/tests_param.c b/src/systemcmds/tests/test_param.c
index 13f17bc43..318d2505b 100644
--- a/src/systemcmds/tests/tests_param.c
+++ b/src/systemcmds/tests/test_param.c
@@ -32,7 +32,7 @@
****************************************************************************/
/**
- * @file tests_param.c
+ * @file test_param.c
*
* Tests related to the parameter system.
*/
diff --git a/src/systemcmds/tests/test_ppm_loopback.c b/src/systemcmds/tests/test_ppm_loopback.c
new file mode 100644
index 000000000..b9041b013
--- /dev/null
+++ b/src/systemcmds/tests/test_ppm_loopback.c
@@ -0,0 +1,178 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file test_ppm_loopback.c
+ * Tests the PWM outputs and PPM input
+ *
+ */
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <debug.h>
+
+#include <arch/board/board.h>
+#include <drivers/drv_pwm_output.h>
+#include <drivers/drv_rc_input.h>
+#include <uORB/topics/rc_channels.h>
+#include <systemlib/err.h>
+
+#include "tests.h"
+
+#include <math.h>
+#include <float.h>
+
+int test_ppm_loopback(int argc, char *argv[])
+{
+
+ int _rc_sub = orb_subscribe(ORB_ID(input_rc));
+
+ int servo_fd, result;
+ servo_position_t data[PWM_OUTPUT_MAX_CHANNELS];
+ servo_position_t pos;
+
+ servo_fd = open(PWM_OUTPUT_DEVICE_PATH, O_RDWR);
+
+ if (servo_fd < 0) {
+ printf("failed opening /dev/pwm_servo\n");
+ }
+
+ printf("Servo readback, pairs of values should match defaults\n");
+
+ unsigned servo_count;
+ result = ioctl(servo_fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
+ if (result != OK) {
+ warnx("PWM_SERVO_GET_COUNT");
+ return ERROR;
+ }
+
+ for (unsigned i = 0; i < servo_count; i++) {
+ result = ioctl(servo_fd, PWM_SERVO_GET(i), (unsigned long)&pos);
+
+ if (result < 0) {
+ printf("failed reading channel %u\n", i);
+ }
+
+ //printf("%u: %u %u\n", i, pos, data[i]);
+
+ }
+
+ // /* tell safety that its ok to disable it with the switch */
+ // result = ioctl(servo_fd, PWM_SERVO_SET_ARM_OK, 0);
+ // if (result != OK)
+ // warnx("FAIL: PWM_SERVO_SET_ARM_OK");
+ // tell output device that the system is armed (it will output values if safety is off)
+ // result = ioctl(servo_fd, PWM_SERVO_ARM, 0);
+ // if (result != OK)
+ // warnx("FAIL: PWM_SERVO_ARM");
+
+ int pwm_values[] = {1200, 1300, 1900, 1700, 1500, 1250, 1800, 1400};
+
+
+ // for (unsigned i = 0; (i < servo_count) && (i < sizeof(pwm_values) / sizeof(pwm_values[0])); i++) {
+ // result = ioctl(servo_fd, PWM_SERVO_SET(i), pwm_values[i]);
+
+ // if (result) {
+ // (void)close(servo_fd);
+ // return ERROR;
+ // } else {
+ // warnx("channel %d set to %d", i, pwm_values[i]);
+ // }
+ // }
+
+ warnx("servo count: %d", servo_count);
+
+ struct pwm_output_values pwm_out = {.values = {0}, .channel_count = 0};
+
+ for (unsigned i = 0; (i < servo_count) && (i < sizeof(pwm_values) / sizeof(pwm_values[0])); i++) {
+ pwm_out.values[i] = pwm_values[i];
+ //warnx("channel %d: disarmed PWM: %d", i+1, pwm_values[i]);
+ pwm_out.channel_count++;
+ }
+
+ result = ioctl(servo_fd, PWM_SERVO_SET_DISARMED_PWM, (long unsigned int)&pwm_out);
+
+ /* give driver 10 ms to propagate */
+
+ /* 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);
+ usleep(100000);
+
+ /* open PPM input and expect values close to the output values */
+
+ bool rc_updated;
+ orb_check(_rc_sub, &rc_updated);
+
+ if (rc_updated) {
+
+ orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input);
+
+ // int ppm_fd = open(RC_INPUT_DEVICE_PATH, O_RDONLY);
+
+
+
+ // struct rc_input_values rc;
+ // result = read(ppm_fd, &rc, sizeof(rc));
+
+ // if (result != sizeof(rc)) {
+ // warnx("Error reading RC output");
+ // (void)close(servo_fd);
+ // (void)close(ppm_fd);
+ // return ERROR;
+ // }
+
+ /* go and check values */
+ for (unsigned i = 0; (i < servo_count) && (i < sizeof(pwm_values) / sizeof(pwm_values[0])); i++) {
+ if (fabsf(rc_input.values[i] - pwm_values[i]) > 10) {
+ warnx("comparison fail: RC: %d, expected: %d", rc_input.values[i], pwm_values[i]);
+ (void)close(servo_fd);
+ return ERROR;
+ }
+ }
+ } else {
+ warnx("failed reading RC input data");
+ return ERROR;
+ }
+
+ warnx("PPM LOOPBACK TEST PASSED SUCCESSFULLY!");
+
+ return 0;
+}
diff --git a/src/systemcmds/tests/test_sensors.c b/src/systemcmds/tests/test_sensors.c
index 14503276c..f6415b72f 100644
--- a/src/systemcmds/tests/test_sensors.c
+++ b/src/systemcmds/tests/test_sensors.c
@@ -1,7 +1,6 @@
/****************************************************************************
*
- * Copyright (C) 2012 PX4 Development Team. All rights reserved.
- * Author: Lorenz Meier <lm@inf.ethz.ch>
+ * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -49,6 +48,8 @@
#include <fcntl.h>
#include <errno.h>
#include <debug.h>
+#include <math.h>
+#include <systemlib/err.h>
#include <arch/board/board.h>
@@ -77,6 +78,7 @@ static int accel(int argc, char *argv[]);
static int gyro(int argc, char *argv[]);
static int mag(int argc, char *argv[]);
static int baro(int argc, char *argv[]);
+static int mpu6k(int argc, char *argv[]);
/****************************************************************************
* Private Data
@@ -91,6 +93,7 @@ struct {
{"gyro", "/dev/gyro", gyro},
{"mag", "/dev/mag", mag},
{"baro", "/dev/baro", baro},
+ {"mpu6k", "/dev/mpu6k", mpu6k},
{NULL, NULL, NULL}
};
@@ -133,23 +136,83 @@ accel(int argc, char *argv[])
printf("\tACCEL accel: x:%8.4f\ty:%8.4f\tz:%8.4f m/s^2\n", (double)buf.x, (double)buf.y, (double)buf.z);
}
- // /* wait at least 10ms, sensor should have data after no more than 2ms */
- // usleep(100000);
+ if (fabsf(buf.x) > 30.0f || fabsf(buf.y) > 30.0f || fabsf(buf.z) > 30.0f) {
+ warnx("MPU6K acceleration values out of range!");
+ return ERROR;
+ }
+
+ /* Let user know everything is ok */
+ printf("\tOK: ACCEL passed all tests successfully\n");
+ close(fd);
+
+ return OK;
+}
+
+static int
+mpu6k(int argc, char *argv[])
+{
+ printf("\tMPU6K: test start\n");
+ fflush(stdout);
+
+ int fd;
+ struct accel_report buf;
+ struct gyro_report gyro_buf;
+ int ret;
+
+ fd = open("/dev/accel_mpu6k", O_RDONLY);
+
+ if (fd < 0) {
+ printf("\tMPU6K: open fail, run <mpu6000 start> first.\n");
+ return ERROR;
+ }
+
+ /* wait at least 100ms, sensor should have data after no more than 20ms */
+ usleep(100000);
+
+ /* read data - expect samples */
+ ret = read(fd, &buf, sizeof(buf));
- // ret = read(fd, buf, sizeof(buf));
+ if (ret != sizeof(buf)) {
+ printf("\tMPU6K: read1 fail (%d)\n", ret);
+ return ERROR;
- // if (ret != sizeof(buf)) {
- // printf("\tMPU-6000: read2 fail (%d)\n", ret);
- // return ERROR;
+ } else {
+ printf("\tMPU6K accel: x:%8.4f\ty:%8.4f\tz:%8.4f m/s^2\n", (double)buf.x, (double)buf.y, (double)buf.z);
+ }
- // } else {
- // printf("\tMPU-6000 values: acc: x:%d\ty:%d\tz:%d\tgyro: r:%d\tp:%d\ty:%d\n", buf[0], buf[1], buf[2], buf[3], buf[4], buf[5]);
- // }
+ if (fabsf(buf.x) > 30.0f || fabsf(buf.y) > 30.0f || fabsf(buf.z) > 30.0f) {
+ warnx("MPU6K acceleration values out of range!");
+ return ERROR;
+ }
+
+ /* Let user know everything is ok */
+ printf("\tOK: MPU6K ACCEL passed all tests successfully\n");
- /* XXX more tests here */
+ close(fd);
+ fd = open("/dev/gyro_mpu6k", O_RDONLY);
+
+ if (fd < 0) {
+ printf("\tMPU6K GYRO: open fail, run <l3gd20 start> or <mpu6000 start> first.\n");
+ return ERROR;
+ }
+
+ /* wait at least 5 ms, sensor should have data after that */
+ usleep(5000);
+
+ /* read data - expect samples */
+ ret = read(fd, &gyro_buf, sizeof(gyro_buf));
+
+ if (ret != sizeof(gyro_buf)) {
+ printf("\tMPU6K GYRO: read fail (%d)\n", ret);
+ return ERROR;
+
+ } else {
+ printf("\tMPU6K GYRO rates: x:%8.4f\ty:%8.4f\tz:%8.4f rad/s\n", (double)gyro_buf.x, (double)gyro_buf.y, (double)gyro_buf.z);
+ }
/* Let user know everything is ok */
- printf("\tOK: ACCEL passed all tests successfully\n");
+ printf("\tOK: MPU6K GYRO passed all tests successfully\n");
+ close(fd);
return OK;
}
@@ -187,6 +250,7 @@ gyro(int argc, char *argv[])
/* Let user know everything is ok */
printf("\tOK: GYRO passed all tests successfully\n");
+ close(fd);
return OK;
}
@@ -224,6 +288,7 @@ mag(int argc, char *argv[])
/* Let user know everything is ok */
printf("\tOK: MAG passed all tests successfully\n");
+ close(fd);
return OK;
}
@@ -261,6 +326,7 @@ baro(int argc, char *argv[])
/* Let user know everything is ok */
printf("\tOK: BARO passed all tests successfully\n");
+ close(fd);
return OK;
}
diff --git a/src/systemcmds/tests/test_servo.c b/src/systemcmds/tests/test_servo.c
index f95760ca8..ef8512bf5 100644
--- a/src/systemcmds/tests/test_servo.c
+++ b/src/systemcmds/tests/test_servo.c
@@ -1,7 +1,6 @@
/****************************************************************************
- * px4/sensors/test_hrt.c
*
- * Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -13,7 +12,7 @@
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
- * 3. Neither the name NuttX nor the names of its contributors may be
+ * 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
@@ -32,9 +31,11 @@
*
****************************************************************************/
-/****************************************************************************
- * Included Files
- ****************************************************************************/
+/**
+ * @file test_servo.c
+ * Tests the servo outputs
+ *
+ */
#include <nuttx/config.h>
@@ -55,39 +56,6 @@
#include "tests.h"
-/****************************************************************************
- * Pre-processor Definitions
- ****************************************************************************/
-
-/****************************************************************************
- * Private Types
- ****************************************************************************/
-
-/****************************************************************************
- * Private Function Prototypes
- ****************************************************************************/
-
-/****************************************************************************
- * Private Data
- ****************************************************************************/
-
-/****************************************************************************
- * Public Data
- ****************************************************************************/
-
-/****************************************************************************
- * Private Functions
- ****************************************************************************/
-
-
-/****************************************************************************
- * Public Functions
- ****************************************************************************/
-
-/****************************************************************************
- * Name: test_servo
- ****************************************************************************/
-
int test_servo(int argc, char *argv[])
{
int fd, result;
@@ -110,7 +78,14 @@ int test_servo(int argc, char *argv[])
printf("Servo readback, pairs of values should match defaults\n");
- for (unsigned i = 0; i < PWM_OUTPUT_MAX_CHANNELS; i++) {
+ unsigned servo_count;
+ result = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
+ if (result != OK) {
+ warnx("PWM_SERVO_GET_COUNT");
+ return ERROR;
+ }
+
+ for (unsigned i = 0; i < servo_count; i++) {
result = ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&pos);
if (result < 0) {
@@ -122,11 +97,20 @@ int test_servo(int argc, char *argv[])
}
- printf("Servos arming at default values\n");
+ /* tell safety that its ok to disable it with the switch */
+ result = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0);
+ if (result != OK)
+ warnx("FAIL: PWM_SERVO_SET_ARM_OK");
+ /* tell output device that the system is armed (it will output values if safety is off) */
result = ioctl(fd, PWM_SERVO_ARM, 0);
+ if (result != OK)
+ warnx("FAIL: PWM_SERVO_ARM");
+
usleep(5000000);
printf("Advancing channel 0 to 1500\n");
result = ioctl(fd, PWM_SERVO_SET(0), 1500);
+ printf("Advancing channel 1 to 1800\n");
+ result = ioctl(fd, PWM_SERVO_SET(1), 1800);
out:
return 0;
}
diff --git a/src/systemcmds/tests/test_uart_loopback.c b/src/systemcmds/tests/test_uart_loopback.c
index 3be152004..f1d41739b 100644
--- a/src/systemcmds/tests/test_uart_loopback.c
+++ b/src/systemcmds/tests/test_uart_loopback.c
@@ -1,8 +1,6 @@
/****************************************************************************
- * px4/sensors/test_gpio.c
*
- * Copyright (C) 2012 PX4 Development Team. All rights reserved.
- * Lorenz Meier <lm@inf.ethz.ch>
+ * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -14,7 +12,7 @@
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
- * 3. Neither the name NuttX nor the names of its contributors may be
+ * 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
@@ -33,9 +31,11 @@
*
****************************************************************************/
-/****************************************************************************
- * Included Files
- ****************************************************************************/
+/**
+ * @file test_uart_loopback.c
+ * Tests the uart outputs
+ *
+ */
#include <nuttx/config.h>
@@ -55,40 +55,6 @@
#include <math.h>
#include <float.h>
-
-/****************************************************************************
- * Pre-processor Definitions
- ****************************************************************************/
-
-/****************************************************************************
- * Private Types
- ****************************************************************************/
-
-/****************************************************************************
- * Private Function Prototypes
- ****************************************************************************/
-
-/****************************************************************************
- * Private Data
- ****************************************************************************/
-
-/****************************************************************************
- * Public Data
- ****************************************************************************/
-
-/****************************************************************************
- * Private Functions
- ****************************************************************************/
-
-
-/****************************************************************************
- * Public Functions
- ****************************************************************************/
-
-/****************************************************************************
- * Name: test_led
- ****************************************************************************/
-
int test_uart_loopback(int argc, char *argv[])
{
@@ -97,11 +63,11 @@ int test_uart_loopback(int argc, char *argv[])
int uart5_nwrite = 0;
int uart2_nwrite = 0;
- int uart1 = open("/dev/ttyS0", O_RDWR | O_NOCTTY); //
+ /* opening stdout */
+ int stdout_fd = 1;
- /* assuming NuttShell is on UART1 (/dev/ttyS0) */
- int uart2 = open("/dev/ttyS1", O_RDWR | O_NONBLOCK | O_NOCTTY); //
- int uart5 = open("/dev/ttyS2", O_RDWR | O_NONBLOCK | O_NOCTTY); //
+ int uart2 = open("/dev/ttyS1", O_RDWR | O_NONBLOCK | O_NOCTTY);
+ int uart5 = open("/dev/ttyS2", O_RDWR | O_NONBLOCK | O_NOCTTY);
if (uart2 < 0) {
printf("ERROR opening UART2, aborting..\n");
@@ -113,7 +79,7 @@ int test_uart_loopback(int argc, char *argv[])
exit(uart5);
}
- uint8_t sample_uart1[] = {'C', 'O', 'U', 'N', 'T', ' ', '#', '\n'};
+ uint8_t sample_stdout_fd[] = {'C', 'O', 'U', 'N', 'T', ' ', '#', '\n'};
uint8_t sample_uart2[] = {'C', 'O', 'U', 'N', 'T', ' ', '#', 0};
uint8_t sample_uart5[] = {'C', 'O', 'U', 'N', 'T', ' ', '#', 0};
@@ -121,7 +87,7 @@ int test_uart_loopback(int argc, char *argv[])
for (i = 0; i < 1000; i++) {
// printf("TEST #%d\n",i);
- write(uart1, sample_uart1, sizeof(sample_uart1));
+ write(stdout_fd, sample_stdout_fd, sizeof(sample_stdout_fd));
/* uart2 -> uart5 */
r = write(uart2, sample_uart2, sizeof(sample_uart2));
@@ -130,7 +96,7 @@ int test_uart_loopback(int argc, char *argv[])
uart2_nwrite += r;
// printf("TEST #%d\n",i);
- write(uart1, sample_uart1, sizeof(sample_uart1));
+ write(stdout_fd, sample_stdout_fd, sizeof(sample_stdout_fd));
/* uart2 -> uart5 */
r = write(uart5, sample_uart5, sizeof(sample_uart5));
@@ -139,7 +105,7 @@ int test_uart_loopback(int argc, char *argv[])
uart5_nwrite += r;
// printf("TEST #%d\n",i);
- write(uart1, sample_uart1, sizeof(sample_uart1));
+ write(stdout_fd, sample_stdout_fd, sizeof(sample_stdout_fd));
/* try to read back values */
do {
@@ -150,7 +116,7 @@ int test_uart_loopback(int argc, char *argv[])
} while (r > 0);
// printf("TEST #%d\n",i);
- write(uart1, sample_uart1, sizeof(sample_uart1));
+ write(stdout_fd, sample_stdout_fd, sizeof(sample_stdout_fd));
do {
r = read(uart2, sample_uart5, sizeof(sample_uart5));
@@ -160,7 +126,7 @@ int test_uart_loopback(int argc, char *argv[])
} while (r > 0);
// printf("TEST #%d\n",i);
-// write(uart1, sample_uart1, sizeof(sample_uart5));
+// write(stdout_fd, sample_stdout_fd, sizeof(sample_uart5));
}
for (i = 0; i < 200000; i++) {
@@ -181,7 +147,7 @@ int test_uart_loopback(int argc, char *argv[])
}
- close(uart1);
+ close(stdout_fd);
close(uart2);
close(uart5);
diff --git a/src/systemcmds/tests/tests.h b/src/systemcmds/tests/tests.h
index c483108cf..5cbc5ad88 100644
--- a/src/systemcmds/tests/tests.h
+++ b/src/systemcmds/tests/tests.h
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -34,6 +34,12 @@
#ifndef __APPS_PX4_TESTS_H
#define __APPS_PX4_TESTS_H
+/**
+ * @file tests.h
+ * Tests declaration file.
+ *
+ */
+
/****************************************************************************
* Included Files
****************************************************************************/
@@ -88,6 +94,7 @@ extern int test_int(int argc, char *argv[]);
extern int test_float(int argc, char *argv[]);
extern int test_ppm(int argc, char *argv[]);
extern int test_servo(int argc, char *argv[]);
+extern int test_ppm_loopback(int argc, char *argv[]);
extern int test_uart_loopback(int argc, char *argv[]);
extern int test_uart_baudchange(int argc, char *argv[]);
extern int test_cpuload(int argc, char *argv[]);
diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c
index 9eb9c632e..cd998dd18 100644
--- a/src/systemcmds/tests/tests_main.c
+++ b/src/systemcmds/tests/tests_main.c
@@ -1,7 +1,6 @@
/****************************************************************************
*
- * Copyright (C) 2012 PX4 Development Team. All rights reserved.
- * Author: @author Lorenz Meier <lm@inf.ethz.ch>
+ * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -35,6 +34,8 @@
/**
* @file tests_main.c
* Tests main file, loads individual tests.
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
*/
#include <nuttx/config.h>
@@ -58,14 +59,6 @@
#include "tests.h"
/****************************************************************************
- * Pre-processor Definitions
- ****************************************************************************/
-
-/****************************************************************************
- * Private Types
- ****************************************************************************/
-
-/****************************************************************************
* Private Function Prototypes
****************************************************************************/
@@ -94,6 +87,7 @@ const struct {
{"hrt", test_hrt, OPT_NOJIGTEST | OPT_NOALLTEST},
{"ppm", test_ppm, OPT_NOJIGTEST | OPT_NOALLTEST},
{"servo", test_servo, OPT_NOJIGTEST | OPT_NOALLTEST},
+ {"ppm_loopback", test_ppm_loopback, OPT_NOALLTEST},
{"adc", test_adc, OPT_NOJIGTEST},
{"jig_voltages", test_jig_voltages, OPT_NOALLTEST},
{"uart_loopback", test_uart_loopback, OPT_NOJIGTEST | OPT_NOALLTEST},
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-19 21:35:07 +0000