diff options
author | px4dev <px4@purgatory.org> | 2012-08-04 15:12:36 -0700 |
---|---|---|
committer | px4dev <px4@purgatory.org> | 2012-08-04 15:12:36 -0700 |
commit | 8a365179eafdf3aea98e60ab9f5882b200d4c759 (patch) | |
tree | 4f38d6d4cd80bd0b6e22e2bb534c3f117ce44e56 /apps/px4 | |
download | px4-firmware-8a365179eafdf3aea98e60ab9f5882b200d4c759.tar.gz px4-firmware-8a365179eafdf3aea98e60ab9f5882b200d4c759.tar.bz2 px4-firmware-8a365179eafdf3aea98e60ab9f5882b200d4c759.zip |
Fresh import of the PX4 firmware sources.
Diffstat (limited to 'apps/px4')
42 files changed, 7200 insertions, 0 deletions
diff --git a/apps/px4/attitude_estimator_bm/.context b/apps/px4/attitude_estimator_bm/.context new file mode 100644 index 000000000..e69de29bb --- /dev/null +++ b/apps/px4/attitude_estimator_bm/.context diff --git a/apps/px4/attitude_estimator_bm/Makefile b/apps/px4/attitude_estimator_bm/Makefile new file mode 100644 index 000000000..358b062c0 --- /dev/null +++ b/apps/px4/attitude_estimator_bm/Makefile @@ -0,0 +1,45 @@ +############################################################################ +# +# Copyright (C) 2012 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. +# +############################################################################ + +# +# Makefile to build the black magic attitude estimator +# + +APPNAME = attitude_estimator_bm +PRIORITY = SCHED_PRIORITY_MAX - 10 +STACKSIZE = 3000 + +# XXX this is *horribly* broken +INCLUDES = $(TOPDIR)/../mavlink/include/mavlink + +include $(APPDIR)/mk/app.mk diff --git a/apps/px4/attitude_estimator_bm/attitude_bm.c b/apps/px4/attitude_estimator_bm/attitude_bm.c new file mode 100644 index 000000000..41ef47918 --- /dev/null +++ b/apps/px4/attitude_estimator_bm/attitude_bm.c @@ -0,0 +1,314 @@ + +/* + * attitude_bm.c + * + * Created on: 21.12.2010 + * Author: Laurens Mackay, Tobias Naegeli + */ + +#include <math.h> +#include "attitude_bm.h" +#include "kalman.h" + + +#define TIME_STEP (1.0f / 500.0f) + +static kalman_t attitude_blackmagic_kal; + +void vect_norm(float_vect3 *vect) +{ + float length = sqrtf( + vect->x * vect->x + vect->y * vect->y + vect->z * vect->z); + + if (length != 0) { + vect->x /= length; + vect->y /= length; + vect->z /= length; + } +} + + +void vect_cross_product(const float_vect3 *a, const float_vect3 *b, + float_vect3 *c) +{ + c->x = a->y * b->z - a->z * b->y; + c->y = a->z * b->x - a->x * b->z; + c->z = a->x * b->y - a->y * b->x; +} + +void attitude_blackmagic_update_a(void) +{ + // for acc + // Idendity matrix already in A. + M(attitude_blackmagic_kal.a, 0, 1) = TIME_STEP * kalman_get_state( + &attitude_blackmagic_kal, 11); + M(attitude_blackmagic_kal.a, 0, 2) = -TIME_STEP * kalman_get_state( + &attitude_blackmagic_kal, 10); + + M(attitude_blackmagic_kal.a, 1, 0) = -TIME_STEP * kalman_get_state( + &attitude_blackmagic_kal, 11); + M(attitude_blackmagic_kal.a, 1, 2) = TIME_STEP * kalman_get_state( + &attitude_blackmagic_kal, 9); + + M(attitude_blackmagic_kal.a, 2, 0) = TIME_STEP * kalman_get_state( + &attitude_blackmagic_kal, 10); + M(attitude_blackmagic_kal.a, 2, 1) = -TIME_STEP * kalman_get_state( + &attitude_blackmagic_kal, 9); + + // for mag + // Idendity matrix already in A. + M(attitude_blackmagic_kal.a, 3, 4) = TIME_STEP * kalman_get_state( + &attitude_blackmagic_kal, 11); + M(attitude_blackmagic_kal.a, 3, 5) = -TIME_STEP * kalman_get_state( + &attitude_blackmagic_kal, 10); + + M(attitude_blackmagic_kal.a, 4, 3) = -TIME_STEP * kalman_get_state( + &attitude_blackmagic_kal, 11); + M(attitude_blackmagic_kal.a, 4, 5) = TIME_STEP * kalman_get_state( + &attitude_blackmagic_kal, 9); + + M(attitude_blackmagic_kal.a, 5, 3) = TIME_STEP * kalman_get_state( + &attitude_blackmagic_kal, 10); + M(attitude_blackmagic_kal.a, 5, 4) = -TIME_STEP * kalman_get_state( + &attitude_blackmagic_kal, 9); + +} + +void attitude_blackmagic_init(void) +{ + //X Kalmanfilter + //initalize matrices + + static m_elem kal_a[12 * 12] = { + 1.0f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + + 0, 1.0f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + + 0, 0, 1.0f, 0, 0, 0, 0, 0, 0, 0, 0, 0, + + 0, 0, 0, 1.0f, 0, 0, 0, 0, 0, 0, 0, 0, + + 0, 0, 0, 0, 1.0f, 0, 0, 0, 0, 0, 0, 0, + + 0, 0, 0, 0, 0, 1.0f, 0, 0, 0, 0, 0, 0, + + 0, 0, 0, 0, 0, 0, 1.0f, 0, 0, 0, 0, 0, + + 0, 0, 0, 0, 0, 0, 0, 1.0f, 0, 0, 0, 0, + + 0, 0, 0, 0, 0, 0, 0, 0, 1.0f, 0, 0, 0, + + 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0f, 0, 0, + + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0f, 0, + + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0f + }; + + static m_elem kal_c[9 * 12] = { + 1.0f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + + 0, 1.0f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + + 0, 0, 1.0f, 0, 0, 0, 0, 0, 0, 0, 0, 0, + + 0, 0, 0, 1.0f, 0, 0, 0, 0, 0, 0, 0, 0, + + 0, 0, 0, 0, 1.0f, 0, 0, 0, 0, 0, 0, 0, + + 0, 0, 0, 0, 0, 1.0f, 0, 0, 0, 0, 0, 0, + + 0, 0, 0, 0, 0, 0, 1.0f, 0, 0, 1.0f, 0, 0, + + 0, 0, 0, 0, 0, 0, 0, 1.0f, 0, 0, 1.0f, 0, + + 0, 0, 0, 0, 0, 0, 0, 0, 1.0f, 0, 0, 1.0f + }; + + + +#define FACTOR 0.5 +#define FACTORstart 1 + + +// static m_elem kal_gain[12 * 9] = +// { 0.004 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0, +// 0 , 0.004 , 0 , 0 , 0 , 0 , 0 , 0 , 0, +// 0 , 0 , 0.004 , 0 , 0 , 0 , 0 , 0 , 0, +// 0 , 0 , 0 , 0.015, 0 , 0 , 0 , 0 , 0, +// 0 , 0 , 0 , 0 , 0.015, 0 , 0 , 0 , 0, +// 0 , 0 , 0 , 0 , 0 , 0.015, 0 , 0 , 0, +// 0.0000 , +0.00002,0 , 0 , 0, 0, 0, 0 , 0, +// -0.00002,0 , 0 , 0 , 0, 0, 0, 0, 0, +// 0, 0 , 0 , 0, 0, 0, 0, 0, 0, +// 0 , 0 , 0 , 0 , 0 , 0 , 0.4 , 0 , 0, +// 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0.4 , 0, +// 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0.4 +// }; + + static m_elem kal_gain[12 * 9] = { + 0.0006f , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0, + 0 , 0.0006f , 0 , 0 , 0 , 0 , 0 , 0 , 0, + 0 , 0 , 0.0006f , 0 , 0 , 0 , 0 , 0 , 0, + 0 , 0 , 0 , 0.015f, 0 , 0 , 0 , 0 , 0, + 0 , 0 , 0 , 0 , 0.015f, 0 , 0 , 0 , 0, + 0 , 0 , 0 , 0 , 0 , 0.015f, 0 , 0 , 0, + 0.0000f , +0.00002f, 0 , 0 , 0, 0, 0, 0 , 0, + -0.00002f, 0 , 0 , 0 , 0, 0, 0, 0, 0, + 0, 0 , 0 , 0, 0, 0, 0, 0, 0, + 0 , 0 , 0 , 0 , 0 , 0 , 0.6f , 0 , 0, + 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0.6f , 0, + 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0.6f + }; + //offset update only correct if not upside down. + +#define K (10.0f*TIME_STEP) + + static m_elem kal_gain_start[12 * 9] = { + K, 0, 0, 0, 0, 0, 0, 0, 0, + + 0, K, 0, 0, 0, 0, 0, 0, 0, + + 0, 0, K, 0, 0, 0, 0, 0, 0, + + 0, 0, 0, K, 0, 0, 0, 0, 0, + + 0, 0, 0, 0, K, 0, 0, 0, 0, + + 0, 0, 0, 0, 0, K, 0, 0, 0, + + 0, 0, 0, 0, 0, 0, K, 0, 0, + + 0, 0, 0, 0, 0, 0, 0, K, 0, + + 0, 0, 0, 0, 0, 0, 0, 0, K, + + 0, 0, 0, 0, 0, 0, 0, 0, 0, + + 0, 0, 0, 0, 0, 0, 0, 0, 0, + + 0, 0, 0, 0, 0, 0, 0, 0, 0 + }; + + + + static m_elem kal_x_apriori[12 * 1] = + { }; + + + //---> initial states sind aposteriori!? ---> fehler + static m_elem kal_x_aposteriori[12 * 1] = + { 0.0f, 0.0f, -1.0f, 0.6f, 0.0f, 0.8f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f }; + + kalman_init(&attitude_blackmagic_kal, 12, 9, kal_a, kal_c, + kal_gain_start, kal_gain, kal_x_apriori, kal_x_aposteriori, 1000); + +} + +void attitude_blackmagic(const float_vect3 *accel, const float_vect3 *mag, const float_vect3 *gyro) +{ + //Transform accelerometer used in all directions + // float_vect3 acc_nav; + //body2navi(&global_data.accel_si, &global_data.attitude, &acc_nav); + + // Kalman Filter + + //Calculate new linearized A matrix + attitude_blackmagic_update_a(); + + kalman_predict(&attitude_blackmagic_kal); + + //correction update + + m_elem measurement[9] = + { }; + m_elem mask[9] = + { 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f }; + + measurement[0] = accel->x; + measurement[1] = accel->y; + measurement[2] = accel->z; + + measurement[3] = mag->x; + measurement[4] = mag->y; + measurement[5] = mag->z; + + measurement[6] = gyro->x; + measurement[7] = gyro->y; + measurement[8] = gyro->z; + + //Put measurements into filter + + +// static int j = 0; +// if (j >= 3) +// { +// j = 0; +// +// mask[3]=1; +// mask[4]=1; +// mask[5]=1; +// j=0; +// +// }else{ +// j++;} + + kalman_correct(&attitude_blackmagic_kal, measurement, mask); + +} +void attitude_blackmagic_get_all(float_vect3 *euler, float_vect3 *rates, float_vect3 *x_n_b, float_vect3 *y_n_b, float_vect3 *z_n_b) +{ + //debug + + // save outputs + float_vect3 kal_acc; + float_vect3 kal_mag; +// float_vect3 kal_w0, kal_w; + + kal_acc.x = kalman_get_state(&attitude_blackmagic_kal, 0); + kal_acc.y = kalman_get_state(&attitude_blackmagic_kal, 1); + kal_acc.z = kalman_get_state(&attitude_blackmagic_kal, 2); + + kal_mag.x = kalman_get_state(&attitude_blackmagic_kal, 3); + kal_mag.y = kalman_get_state(&attitude_blackmagic_kal, 4); + kal_mag.z = kalman_get_state(&attitude_blackmagic_kal, 5); + +// kal_w0.x = kalman_get_state(&attitude_blackmagic_kal, 6); +// kal_w0.y = kalman_get_state(&attitude_blackmagic_kal, 7); +// kal_w0.z = kalman_get_state(&attitude_blackmagic_kal, 8); +// +// kal_w.x = kalman_get_state(&attitude_blackmagic_kal, 9); +// kal_w.y = kalman_get_state(&attitude_blackmagic_kal, 10); +// kal_w.z = kalman_get_state(&attitude_blackmagic_kal, 11); + + rates->x = kalman_get_state(&attitude_blackmagic_kal, 9); + rates->y = kalman_get_state(&attitude_blackmagic_kal, 10); + rates->z = kalman_get_state(&attitude_blackmagic_kal, 11); + + + +// kal_w = kal_w; // XXX hack to silence compiler warning +// kal_w0 = kal_w0; // XXX hack to silence compiler warning + + + + //debug_vect("magn", mag); + + //float_vect3 x_n_b, y_n_b, z_n_b; + z_n_b->x = -kal_acc.x; + z_n_b->y = -kal_acc.y; + z_n_b->z = -kal_acc.z; + vect_norm(z_n_b); + vect_cross_product(z_n_b, &kal_mag, y_n_b); + vect_norm(y_n_b); + + vect_cross_product(y_n_b, z_n_b, x_n_b); + + + + //save euler angles + euler->x = atan2f(z_n_b->y, z_n_b->z); + euler->y = -asinf(z_n_b->x); + euler->z = atan2f(y_n_b->x, x_n_b->x); + +} diff --git a/apps/px4/attitude_estimator_bm/attitude_bm.h b/apps/px4/attitude_estimator_bm/attitude_bm.h new file mode 100644 index 000000000..c21b3d6f1 --- /dev/null +++ b/apps/px4/attitude_estimator_bm/attitude_bm.h @@ -0,0 +1,24 @@ +/* + * attitude_blackmagic.h + * + * Created on: May 31, 2011 + * Author: pixhawk + */ + +#ifndef attitude_blackmagic_H_ +#define attitude_blackmagic_H_ + +#include "matrix.h" + +void vect_norm(float_vect3 *vect); + +void vect_cross_product(const float_vect3 *a, const float_vect3 *b, float_vect3 *c); + +void attitude_blackmagic_update_a(void); + +void attitude_blackmagic_init(void); + +void attitude_blackmagic(const float_vect3 *accel, const float_vect3 *mag, const float_vect3 *gyro); + +void attitude_blackmagic_get_all(float_vect3 *euler, float_vect3 *rates, float_vect3 *x_n_b, float_vect3 *y_n_b, float_vect3 *z_n_b); +#endif /* attitude_blackmagic_H_ */ diff --git a/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c b/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c new file mode 100644 index 000000000..3c6c8eec3 --- /dev/null +++ b/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c @@ -0,0 +1,228 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Tobias Naegeli <naegelit@student.ethz.ch> + * Laurens Mackay <mackayl@student.ethz.ch> + * + * 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 Black Magic Attitude Estimator */ + + + +#include <nuttx/config.h> +#include <stdio.h> +#include <unistd.h> +#include <sys/time.h> +#include <stdbool.h> +#include <fcntl.h> +#include <arch/board/up_hrt.h> +#include <string.h> +#include <poll.h> +#include <uORB/uORB.h> +#include <uORB/topics/sensor_combined.h> +#include <uORB/topics/vehicle_attitude.h> +#include <math.h> +#include <errno.h> + +#include "attitude_bm.h" + +static unsigned int loop_interval_alarm = 4500; // loop interval in microseconds + +__EXPORT int attitude_estimator_bm_main(int argc, char *argv[]); + +/**************************************************************************** + * Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * user_start + ****************************************************************************/ +int attitude_estimator_bm_update(struct sensor_combined_s *raw, float_vect3 *euler, float_vect3 *rates, float_vect3 *x_n_b, float_vect3 *y_n_b, float_vect3 *z_n_b); + +int attitude_estimator_bm_update(struct sensor_combined_s *raw, float_vect3 *euler, float_vect3 *rates, float_vect3 *x_n_b, float_vect3 *y_n_b, float_vect3 *z_n_b) +{ + float_vect3 gyro_values; + gyro_values.x = raw->gyro_rad_s[0]; + gyro_values.y = raw->gyro_rad_s[1]; + gyro_values.z = raw->gyro_rad_s[2]; + + float_vect3 accel_values; + accel_values.x = raw->accelerometer_m_s2[0]; + accel_values.y = raw->accelerometer_m_s2[1]; + accel_values.z = raw->accelerometer_m_s2[2]; + + float_vect3 mag_values; + mag_values.x = raw->magnetometer_ga[0]; + mag_values.y = raw->magnetometer_ga[1]; + mag_values.z = raw->magnetometer_ga[2]; + + attitude_blackmagic(&accel_values, &mag_values, &gyro_values); + + /* read out values */ + attitude_blackmagic_get_all(euler, rates, x_n_b, y_n_b, z_n_b); + + return OK; +} + + +int attitude_estimator_bm_main(int argc, char *argv[]) +{ + // print text + printf("Black Magic Attitude Estimator initialized..\n\n"); + fflush(stdout); + + /* data structures to read euler angles and rotation matrix back */ + float_vect3 euler = {.x = 0, .y = 0, .z = 0}; + float_vect3 rates; + float_vect3 x_n_b; + float_vect3 y_n_b; + float_vect3 z_n_b; + + int overloadcounter = 19; + + /* initialize */ + attitude_blackmagic_init(); + + /* store start time to guard against too slow update rates */ + uint64_t last_run = hrt_absolute_time(); + + struct sensor_combined_s sensor_combined_s_local = { .gyro_raw = {0}}; + struct vehicle_attitude_s att = {.roll = 0.0f, .pitch = 0.0f, .yaw = 0.0f, + .rollspeed = 0.0f, .pitchspeed = 0.0f, .yawspeed = 0.0f, + .R = {0}, .timestamp = 0}; + + uint64_t last_data = 0; + + /* subscribe to raw data */ + int sub_raw = orb_subscribe(ORB_ID(sensor_combined)); + + /* rate-limit raw data updates to 200Hz */ + //orb_set_interval(sub_raw, 5); + + /* advertise attitude */ + int pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); + + struct pollfd fds[] = { + { .fd = sub_raw, .events = POLLIN }, + }; + + // int paramcounter = 100; + + /* Main loop*/ + while (true) { + + /* wait for sensor update */ + int ret = poll(fds, 1, 5000); + + if (ret < 0) { + /* XXX this is seriously bad - should be an emergency */ + } else if (ret == 0) { + /* XXX this means no sensor data - should be critical or emergency */ + printf("[attitude estimator bm] WARNING: Not getting sensor data - sensor app running?\n"); + continue; + } + + orb_copy(ORB_ID(sensor_combined), sub_raw, &sensor_combined_s_local); + + uint64_t now = hrt_absolute_time(); + unsigned int time_elapsed = now - last_run; + last_run = now; + +//#if 0 + + if (time_elapsed > loop_interval_alarm) { + //TODO: add warning, cpu overload here + if (overloadcounter == 20) { + printf("CPU OVERLOAD DETECTED IN ATTITUDE ESTIMATOR BLACK MAGIC (%lu > %lu)\n", time_elapsed, loop_interval_alarm); + overloadcounter = 0; + } + + overloadcounter++; + } + +//#endif +// now = hrt_absolute_time(); + /* filter values */ + attitude_estimator_bm_update(&sensor_combined_s_local, &euler, &rates, &x_n_b, &y_n_b, &z_n_b); + +// time_elapsed = hrt_absolute_time() - now; +// if (blubb == 20) +// { +// printf("Estimator: %lu\n", time_elapsed); +// blubb = 0; +// } +// blubb++; + +// if (last_data > 0 && sensor_combined_s_local.timestamp - last_data > 8000) printf("sensor data missed! (%llu)\n", sensor_combined_s_local.timestamp - last_data); + +// printf("%llu -> %llu = %llu\n", last_data, sensor_combined_s_local.timestamp, sensor_combined_s_local.timestamp - last_data); + // last_data = sensor_combined_s_local.timestamp; + + /*correct yaw */ +// euler.z = euler.z + M_PI; + + /* send out */ + + att.timestamp = sensor_combined_s_local.timestamp; + att.roll = euler.x; + att.pitch = euler.y; + att.yaw = euler.z + M_PI; + + if (att.yaw > 2.0f * ((float)M_PI)) { + att.yaw -= 2.0f * ((float)M_PI); + } + + att.rollspeed = rates.x; + att.pitchspeed = rates.y; + att.yawspeed = rates.z; + + att.R[0][0] = x_n_b.x; + att.R[0][1] = x_n_b.y; + att.R[0][2] = x_n_b.z; + // XXX add remaining entries + + // Broadcast + orb_publish(ORB_ID(vehicle_attitude), pub_att, &att); + } + + /* Should never reach here */ + return 0; +} + + diff --git a/apps/px4/attitude_estimator_bm/kalman.c b/apps/px4/attitude_estimator_bm/kalman.c new file mode 100644 index 000000000..e4ea7a417 --- /dev/null +++ b/apps/px4/attitude_estimator_bm/kalman.c @@ -0,0 +1,115 @@ +/* + * kalman.c + * + * Created on: 01.12.2010 + * Author: Laurens Mackay + */ +#include "kalman.h" +//#include "mavlink_debug.h" + +void kalman_init(kalman_t *kalman, int states, int measurements, m_elem a[], + m_elem c[], m_elem gain_start[], m_elem gain[], m_elem x_apriori[], + m_elem x_aposteriori[], int gainfactorsteps) +{ + kalman->states = states; + kalman->measurements = measurements; + kalman->gainfactorsteps = gainfactorsteps; + kalman->gainfactor = 0; + + //Create all matrices that are persistent + kalman->a = matrix_create(states, states, a); + kalman->c = matrix_create(measurements, states, c); + kalman->gain_start = matrix_create(states, measurements, gain_start); + kalman->gain = matrix_create(states, measurements, gain); + kalman->x_apriori = matrix_create(states, 1, x_apriori); + kalman->x_aposteriori = matrix_create(states, 1, x_aposteriori); +} + +void kalman_predict(kalman_t *kalman) +{ + matrix_mult(kalman->a, kalman->x_aposteriori, kalman->x_apriori); +} + +void kalman_correct(kalman_t *kalman, m_elem measurement_a[], m_elem mask_a[]) +{ + //create matrices from inputs + matrix_t measurement = + matrix_create(kalman->measurements, 1, measurement_a); + matrix_t mask = matrix_create(kalman->measurements, 1, mask_a); + + //create temporary matrices + m_elem gain_start_part_a[KALMAN_MAX_STATES * KALMAN_MAX_MEASUREMENTS] = + { }; + matrix_t gain_start_part = matrix_create(kalman->states, + kalman->measurements, gain_start_part_a); + + m_elem gain_part_a[KALMAN_MAX_STATES * KALMAN_MAX_MEASUREMENTS] = + { }; + matrix_t gain_part = matrix_create(kalman->states, kalman->measurements, + gain_part_a); + + m_elem gain_sum_a[KALMAN_MAX_STATES * KALMAN_MAX_MEASUREMENTS] = + { }; + matrix_t gain_sum = matrix_create(kalman->states, kalman->measurements, + gain_sum_a); + + m_elem error_a[KALMAN_MAX_MEASUREMENTS * 1] = + { }; + matrix_t error = matrix_create(kalman->measurements, 1, error_a); + + m_elem measurement_estimate_a[KALMAN_MAX_MEASUREMENTS * 1] = + { }; + matrix_t measurement_estimate = matrix_create(kalman->measurements, 1, + measurement_estimate_a); + + m_elem x_update_a[KALMAN_MAX_STATES * 1] = + { }; + matrix_t x_update = matrix_create(kalman->states, 1, x_update_a); + + + + //x(:,i+1)=xapriori+(gainfactor*[M_50(:,1) M(:,2)]+(1-gainfactor)*M_start)*(z-C*xapriori); + + + //est=C*xapriori; + matrix_mult(kalman->c, kalman->x_apriori, measurement_estimate); + //error=(z-C*xapriori) = measurement-estimate + matrix_sub(measurement, measurement_estimate, error); + matrix_mult_element(error, mask, error); + + kalman->gainfactor = kalman->gainfactor * (1.0f - 1.0f + / kalman->gainfactorsteps) + 1.0f * 1.0f / kalman->gainfactorsteps; + + + + matrix_mult_scalar(kalman->gainfactor, kalman->gain, gain_part); + + matrix_mult_scalar(1.0f - kalman->gainfactor, kalman->gain_start, + gain_start_part); + + matrix_add(gain_start_part, gain_part, gain_sum); + + //gain*(z-C*xapriori) + matrix_mult(gain_sum, error, x_update); + + //xaposteriori = xapriori + update + + matrix_add(kalman->x_apriori, x_update, kalman->x_aposteriori); + + +// static int i=0; +// if(i++==4){ +// i=0; +// float_vect3 out_kal; +// out_kal.x = M(gain_sum,0,1); +//// out_kal_z.x = z_measurement[1]; +// out_kal.y = M(gain_sum,1,1); +// out_kal.z = M(gain_sum,2,1); +// debug_vect("out_kal", out_kal); +// } +} + +m_elem kalman_get_state(kalman_t *kalman, int state) +{ + return M(kalman->x_aposteriori, state, 0); +} diff --git a/apps/px4/attitude_estimator_bm/kalman.h b/apps/px4/attitude_estimator_bm/kalman.h new file mode 100644 index 000000000..0a6a18505 --- /dev/null +++ b/apps/px4/attitude_estimator_bm/kalman.h @@ -0,0 +1,35 @@ +/* + * kalman.h + * + * Created on: 01.12.2010 + * Author: Laurens Mackay + */ + +#ifndef KALMAN_H_ +#define KALMAN_H_ + +#include "matrix.h" + +#define KALMAN_MAX_STATES 12 +#define KALMAN_MAX_MEASUREMENTS 9 +typedef struct { + int states; + int measurements; + matrix_t a; + matrix_t c; + matrix_t gain_start; + matrix_t gain; + matrix_t x_apriori; + matrix_t x_aposteriori; + float gainfactor; + int gainfactorsteps; +} kalman_t; + +void kalman_init(kalman_t *kalman, int states, int measurements, m_elem a[], + m_elem c[], m_elem gain_start[], m_elem gain[], m_elem x_apriori[], + m_elem x_aposteriori[], int gainfactorsteps); +void kalman_predict(kalman_t *kalman); +void kalman_correct(kalman_t *kalman, m_elem measurement_a[], m_elem mask_a[]); +m_elem kalman_get_state(kalman_t *kalman, int state); + +#endif /* KALMAN_H_ */ diff --git a/apps/px4/attitude_estimator_bm/matrix.h b/apps/px4/attitude_estimator_bm/matrix.h new file mode 100644 index 000000000..613a2d081 --- /dev/null +++ b/apps/px4/attitude_estimator_bm/matrix.h @@ -0,0 +1,156 @@ +/* + * matrix.h + * + * Created on: 18.11.2010 + * Author: Laurens Mackay + */ + +#ifndef MATRIX_H_ +#define MATRIX_H_ + +typedef float m_elem; + +typedef struct { + int rows; + int cols; + m_elem *a; +} matrix_t; + +typedef struct { + float x; + float y; + float z; +} float_vect3; + +#define M(m,i,j) m.a[m.cols*i+j] + +///* This is the datatype used for the math and non-type specific ops. */ +// +//matrix_t matrix_create(const int rows, const int cols, m_elem * a); +///* matrix C = matrix A + matrix B , both of size m x n */ +//void matrix_add(const matrix_t a, const matrix_t b, matrix_t c); +// +///* matrix C = matrix A - matrix B , all of size m x n */ +//void matrix_sub(const matrix_t a, const matrix_t b, matrix_t c); +// +///* matrix C = matrix A x matrix B , A(a_rows x a_cols), B(a_cols x b_cols) */ +//void matrix_mult(const matrix_t a, const matrix_t b, matrix_t c); +// +//void matrix_mult_scalar(const float f, const matrix_t a, matrix_t c); +// +//void matrix_mult_element(const matrix_t a, const matrix_t b, matrix_t c); +// +///* matrix C = A*B'*/ +//void matrix_mult_trans(const matrix_t a, const matrix_t b, matrix_t c); + + +static inline matrix_t matrix_create(const int rows, const int cols, m_elem *a) +{ + matrix_t ret; + ret.rows = rows; + ret.cols = cols; + ret.a = a; + return ret; +} + +static inline void matrix_add(const matrix_t a, const matrix_t b, matrix_t c) +{ + if (a.rows != c.rows || a.cols != c.cols || b.rows != c.rows || b.cols + != c.cols) { + //debug_message_buffer("matrix_add: Dimension mismatch"); + } + + for (int i = 0; i < c.rows; i++) { + for (int j = 0; j < c.cols; j++) { + M(c, i, j) = M(a, i, j) + M(b, i, j); + } + + } +} + +static inline void matrix_sub(const matrix_t a, const matrix_t b, matrix_t c) +{ + if (a.rows != c.rows || a.cols != c.cols || b.rows != c.rows || b.cols + != c.cols) { + //debug_message_buffer("matrix_sub: Dimension mismatch"); + } + + for (int i = 0; i < c.rows; i++) { + for (int j = 0; j < c.cols; j++) { + M(c, i, j) = M(a, i, j) - M(b, i, j); + } + + } +} + +static inline void matrix_mult(const matrix_t a, const matrix_t b, matrix_t c) +{ + if (a.rows != c.rows || b.cols != c.cols || a.cols != b.rows) { + //debug_message_buffer("matrix_mult: Dimension mismatch"); + } + + for (int i = 0; i < a.rows; i++) { + for (int j = 0; j < b.cols; j++) { + M(c, i, j) = 0; + + for (int k = 0; k < a.cols; k++) { + M(c, i, j) += M(a, i, k) * M(b, k, j); + } + } + + } +} + +static inline void matrix_mult_trans(const matrix_t a, const matrix_t b, matrix_t c) +{ + + if (a.rows != c.rows || b.rows != c.cols || a.cols != b.cols) { + //debug_message_buffer("matrix_mult: Dimension mismatch"); + } + + for (int i = 0; i < a.rows; i++) { + for (int j = 0; j < b.cols; j++) { + M(c, i, j) = 0; + + for (int k = 0; k < a.cols; k++) { + M(c, i, j) += M(a, i, k) * M(b, j, k); + } + } + + } + +} + +static inline void matrix_mult_scalar(const float f, const matrix_t a, matrix_t c) +{ + if (a.rows != c.rows || a.cols != c.cols) { + //debug_message_buffer("matrix_mult_scalar: Dimension mismatch"); + } + + for (int i = 0; i < c.rows; i++) { + for (int j = 0; j < c.cols; j++) { + M(c, i, j) = f * M(a, i, j); + } + + } +} + + +static inline void matrix_mult_element(const matrix_t a, const matrix_t b, matrix_t c) +{ + if (a.rows != c.rows || a.cols != c.cols || b.rows != c.rows || b.cols + != c.cols) { + //debug_message_buffer("matrix_mult_element: Dimension mismatch"); + } + + for (int i = 0; i < c.rows; i++) { + for (int j = 0; j < c.cols; j++) { + M(c, i, j) = M(a, i, j) * M(b, i, j); + } + + } +} + + + +#endif /* MATRIX_H_ */ diff --git a/apps/px4/fmu/Makefile b/apps/px4/fmu/Makefile new file mode 100644 index 000000000..7f1f836e3 --- /dev/null +++ b/apps/px4/fmu/Makefile @@ -0,0 +1,38 @@ +############################################################################ +# +# Copyright (C) 2012 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. +# +############################################################################ + +# +# Interface driver for the PX4FMU board +# + +include $(APPDIR)/mk/app.mk diff --git a/apps/px4/fmu/fmu.cpp b/apps/px4/fmu/fmu.cpp new file mode 100644 index 000000000..6c7d9f742 --- /dev/null +++ b/apps/px4/fmu/fmu.cpp @@ -0,0 +1,452 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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 Driver/configurator for the PX4 FMU multi-purpose port. + */ + +#include <nuttx/config.h> + +#include <sys/types.h> +#include <stdint.h> +#include <stdbool.h> +#include <semaphore.h> +#include <string.h> +#include <fcntl.h> +#include <poll.h> +#include <errno.h> +#include <stdio.h> +#include <math.h> +#include <unistd.h> + +#include <nuttx/arch.h> + +#include <drivers/device/device.h> +#include <drivers/drv_pwm_output.h> +#include <drivers/drv_gpio.h> + +#include <uORB/topics/actuator_controls.h> +#include <systemlib/mixer.h> + +#include <arch/board/up_pwm_servo.h> + +class FMUServo : public device::CDev +{ +public: + enum Mode { + MODE_2PWM, + MODE_4PWM, + MODE_NONE + }; + FMUServo(Mode mode); + ~FMUServo(); + + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + + virtual int init(); + +private: + Mode _mode; + int _task; + int _t_actuators; + int _t_armed; + unsigned _num_outputs; + + volatile bool _task_should_exit; + bool _armed; + + MixMixer *_mixer[4]; + + static void task_main_trampoline(int argc, char *argv[]); + void task_main(); +}; + +namespace { + +FMUServo *g_servo; + +} // namespace + +FMUServo::FMUServo(Mode mode) : + CDev("fmuservo", PWM_OUTPUT_DEVICE_PATH), + _mode(mode), + _task(-1), + _t_actuators(-1), + _t_armed(-1), + _task_should_exit(false), + _armed(false) +{ + for (unsigned i = 0; i < 4; i++) + _mixer[i] = nullptr; +} + +FMUServo::~FMUServo() +{ + if (_task != -1) { + /* task should wake up every 100ms or so at least */ + _task_should_exit = true; + + unsigned i = 0; + do { + /* wait 20ms */ + usleep(20000); + + /* if we have given up, kill it */ + if (++i > 10) { + task_delete(_task); + break; + } + + } while (_task != -1); + } + + g_servo = nullptr; +} + +int +FMUServo::init() +{ + int ret; + + ASSERT(_task == -1); + + /* do regular cdev init */ + ret = CDev::init(); + if (ret != OK) + return ret; + + /* start the IO interface task */ + _task = task_create("fmuservo", SCHED_PRIORITY_DEFAULT, 1024, (main_t)&FMUServo::task_main_trampoline, nullptr); + if (_task < 0) { + debug("task start failed: %d", errno); + return -errno; + } + + return OK; +} + +void +FMUServo::task_main_trampoline(int argc, char *argv[]) +{ + g_servo->task_main(); +} + +void +FMUServo::task_main() +{ + log("ready"); + + /* configure for PWM output */ + switch (_mode) { + case MODE_2PWM: + /* multi-port with flow control lines as PWM */ + /* XXX magic numbers */ + up_pwm_servo_init(0x3); + break; + case MODE_4PWM: + /* multi-port as 4 PWM outs */ + /* XXX magic numbers */ + up_pwm_servo_init(0xf); + break; + case MODE_NONE: + /* we should never get here... */ + break; + } + + /* subscribe to objects that we are interested in watching */ + _t_actuators = orb_subscribe(ORB_ID(actuator_controls)); + orb_set_interval(_t_actuators, 20); /* 50Hz update rate */ + + _t_armed = orb_subscribe(ORB_ID(actuator_armed)); + orb_set_interval(_t_armed, 100); /* 10Hz update rate */ + + struct pollfd fds[2]; + fds[0].fd = _t_actuators; + fds[0].events = POLLIN; + fds[1].fd = _t_armed; + fds[1].events = POLLIN; + + unsigned num_outputs = (_mode == MODE_2PWM) ? 2 : 4; + + /* loop until killed */ + while (!_task_should_exit) { + + /* sleep waiting for data, but no more than 100ms */ + int ret = ::poll(&fds[0], 2, 100); + + /* this would be bad... */ + if (ret < 0) { + log("poll error %d", errno); + usleep(1000000); + continue; + } + + /* do we have a control update? */ + if (fds[0].revents & POLLIN) { + struct actuator_controls ac; + + /* get controls */ + orb_copy(ORB_ID(actuator_controls), _t_actuators, &ac); + + /* iterate actuators */ + for (unsigned i = 0; i < num_outputs; i++) { + + /* if the actuator is configured */ + if (_mixer[i] != nullptr) { + + /* mix controls to the actuator */ + float output = mixer_mix(_mixer[i], &ac.control[0]); + + /* scale for PWM output 900 - 2100us */ + up_pwm_servo_set(i, 1500 + (600 * output)); + } + } + } + + /* how about an arming update? */ + if (fds[1].revents & POLLIN) { + struct actuator_armed aa; + + /* get new value */ + orb_copy(ORB_ID(actuator_armed), _t_armed, &aa); + + /* update PMW servo armed status */ + up_pwm_servo_arm(aa.armed); + } + } + + ::close(_t_actuators); + ::close(_t_armed); + + /* make sure servos are off */ + up_pwm_servo_deinit(); + + /* note - someone else is responsible for restoring the GPIO config */ + + /* tell the dtor that we are exiting */ + _task = -1; + _exit(0); +} + +int +FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + int ret = OK; + + switch (cmd) { + case PWM_SERVO_ARM: + up_pwm_servo_arm(true); + break; + + case PWM_SERVO_DISARM: + up_pwm_servo_arm(false); + break; + + case PWM_SERVO_SET(2): + case PWM_SERVO_SET(3): + if (_mode != MODE_4PWM) { + ret = -EINVAL; + break; + } + /* FALLTHROUGH */ + case PWM_SERVO_SET(0): + case PWM_SERVO_SET(1): + if (arg < 2100) { + int channel = cmd - PWM_SERVO_SET(0); + up_pwm_servo_set(channel, arg); + } else { + ret = -EINVAL; + } + break; + + case PWM_SERVO_GET(2): + case PWM_SERVO_GET(3): + if (_mode != MODE_4PWM) { + ret = -EINVAL; + break; + } + /* FALLTHROUGH */ + case PWM_SERVO_GET(0): + case PWM_SERVO_GET(1): { + int channel = cmd - PWM_SERVO_SET(0); + *(servo_position_t *)arg = up_pwm_servo_get(channel); + break; + } + + default: + ret = -ENOTTY; + break; + } + return ret; +} + +namespace { + +enum PortMode { + PORT_FULL_GPIO, + PORT_FULL_SERIAL, + PORT_FULL_PWM, + PORT_GPIO_AND_SERIAL, + PORT_PWM_AND_SERIAL, + PORT_PWM_AND_GPIO, + PORT_MODE_UNSET +}; + +PortMode g_port_mode; + +int +fmu_new_mode(PortMode new_mode) +{ + int fd; + int ret = OK; + uint32_t gpio_bits; + FMUServo::Mode servo_mode; + + /* get hold of the GPIO configuration descriptor */ + fd = open(GPIO_DEVICE_PATH, 0); + if (fd < 0) + return -errno; + + /* start by tearing down any existing state and revert to all-GPIO-inputs */ + if (g_servo != nullptr) { + delete g_servo; + g_servo = nullptr; + } + + /* reset to all-inputs */ + ioctl(fd, GPIO_RESET, 0); + + gpio_bits = 0; + servo_mode = FMUServo::MODE_NONE; + + switch (new_mode) { + case PORT_FULL_GPIO: + case PORT_MODE_UNSET: + /* nothing more to do here */ + break; + + 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; + break; + + case PORT_FULL_PWM: + /* select 4-pin PWM mode */ + servo_mode = FMUServo::MODE_4PWM; + break; + + case PORT_GPIO_AND_SERIAL: + /* set RX/TX multi-GPIOs to serial mode */ + gpio_bits = GPIO_MULTI_3 | GPIO_MULTI_4; + break; + + case PORT_PWM_AND_SERIAL: + /* select 2-pin PWM mode */ + servo_mode = FMUServo::MODE_2PWM; + /* set RX/TX multi-GPIOs to serial mode */ + gpio_bits = GPIO_MULTI_3 | GPIO_MULTI_4; + break; + + case PORT_PWM_AND_GPIO: + /* select 2-pin PWM mode */ + servo_mode = FMUServo::MODE_2PWM; + break; + } + + /* adjust GPIO config for serial mode(s) */ + if (gpio_bits != 0) + ioctl(fd, GPIO_SET_ALT_1, gpio_bits); + close(fd); + + /* create new PWM driver if required */ + if (servo_mode != FMUServo::MODE_NONE) { + g_servo = new FMUServo(servo_mode); + if (g_servo == nullptr) { + ret = -ENOMEM; + } else { + ret = g_servo->init(); + if (ret != OK) { + delete g_servo; + g_servo = nullptr; + } + } + } + + return ret; +} + +} // namespace + +extern "C" int fmu_main(int argc, char *argv[]); + +int +fmu_main(int argc, char *argv[]) +{ + PortMode new_mode = PORT_MODE_UNSET; + + /* + * Mode switches. + * + * XXX use getopt + */ + if (!strcmp(argv[1], "mode_gpio")) { + new_mode = PORT_FULL_GPIO; + } else if (!strcmp(argv[1], "mode_serial")) { + new_mode = PORT_FULL_SERIAL; + } else if (!strcmp(argv[1], "mode_pwm")) { + new_mode = PORT_FULL_PWM; + } else if (!strcmp(argv[1], "mode_gpio_serial")) { + new_mode = PORT_GPIO_AND_SERIAL; + } else if (!strcmp(argv[1], "mode_pwm_serial")) { + new_mode = PORT_PWM_AND_SERIAL; + } else if (!strcmp(argv[1], "mode_pwm_gpio")) { + new_mode = PORT_PWM_AND_GPIO; + } + + /* was a new mode set? */ + if (new_mode != PORT_MODE_UNSET) { + + /* yes but it's the same mode */ + if (new_mode == g_port_mode) + return OK; + + /* switch modes */ + return fmu_new_mode(new_mode); + } + + /* test, etc. here */ + + fprintf(stderr, "FMU: unrecognised command, try:\n"); + fprintf(stderr, " mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio\n"); + return -EINVAL; +} diff --git a/apps/px4/px4io/driver/.context b/apps/px4/px4io/driver/.context new file mode 100644 index 000000000..e69de29bb --- /dev/null +++ b/apps/px4/px4io/driver/.context diff --git a/apps/px4/px4io/driver/Makefile b/apps/px4/px4io/driver/Makefile new file mode 100644 index 000000000..cbd942546 --- /dev/null +++ b/apps/px4/px4io/driver/Makefile @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (C) 2012 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. +# +############################################################################ + +# +# Interface driver for the PX4IO board. +# + +APPNAME = px4io +PRIORITY = SCHED_PRIORITY_DEFAULT +STACKSIZE = 2048 + +include $(APPDIR)/mk/app.mk diff --git a/apps/px4/px4io/driver/px4io.cpp b/apps/px4/px4io/driver/px4io.cpp new file mode 100644 index 000000000..c3f14e3a4 --- /dev/null +++ b/apps/px4/px4io/driver/px4io.cpp @@ -0,0 +1,560 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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 Driver for the PX4IO board. + * + * PX4IO is connected via serial (or possibly some other interface at a later + * point). + * + * XXX current design is racy as all hell; need a locking strategy. + */ + +#include <nuttx/config.h> + +#include <sys/types.h> +#include <stdint.h> +#include <stdbool.h> +#include <assert.h> +#include <debug.h> +#include <time.h> +#include <queue.h> +#include <errno.h> +#include <string.h> +#include <stdio.h> +#include <unistd.h> +#include <fcntl.h> + +#include <arch/board/board.h> + +#include <drivers/device/device.h> +#include <drivers/drv_rc_input.h> +#include <drivers/drv_pwm_output.h> + +#include <systemlib/perf_counter.h> +#include <systemlib/hx_stream.h> + +#include "../protocol.h" +#include "uploader.h" + +class PX4IO; + + +namespace +{ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +const int ERROR = -1; + +PX4IO *g_dev; + +} + + +class PX4IO_RC : public device::CDev +{ +public: + PX4IO_RC(); + ~PX4IO_RC(); + + virtual int init(); + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + + friend class PX4IO; +protected: + void set_channels(unsigned count, const servo_position_t *data); + +private: + int _publication; + struct rc_input_values _input; +}; + +/* XXX this may conflict with the onboard PPM input */ +PX4IO_RC::PX4IO_RC() : + CDev("px4io_rc", RC_INPUT_DEVICE_PATH), + _publication(-1) +{ + for (unsigned i = 0; i < RC_INPUT_MAX_CHANNELS; i++) { + _input.values[i] = 0; + } + _input.channel_count = 0; +} + +PX4IO_RC::~PX4IO_RC() +{ + if (_publication != -1) + ::close(_publication); +} + +int +PX4IO_RC::init() +{ + int ret; + + ret = CDev::init(); + + /* advertise ourselves as the RC input controller */ + if (ret == OK) { + _publication = orb_advertise(ORB_ID(input_rc), &_input); + if (_publication < 0) + ret = -errno; + } + + return ret; +} + +ssize_t +PX4IO_RC::read(struct file *filp, char *buffer, size_t buflen) +{ + unsigned channels = buflen / sizeof(rc_input_t); + rc_input_t *pdata = (rc_input_t *)buffer; + unsigned i; + + if (channels > PX4IO_INPUT_CHANNELS) + return -EIO; + + lock(); + for (i = 0; i < channels; i++) + pdata[i] = _input.values[i]; + unlock(); + + return i * sizeof(servo_position_t); +} + +void +PX4IO_RC::set_channels(unsigned count, const servo_position_t *data) +{ + + ASSERT(count <= PX4IO_INPUT_CHANNELS); + + /* convert incoming servo position values into 0-100 range */ + lock(); + for (unsigned i = 0; i < count; i++) { + rc_input_t chn; + + if (data[i] < 1000) { + chn = 0; + } else if (data[i] > 2000) { + chn = 100; + } else { + chn = (data[i] - 1000) / 10; + } + + _input.values[i] = chn; + } + _input.channel_count = count; + unlock(); + + /* publish to anyone that might be listening */ + if (_publication != -1) + orb_publish(ORB_ID(input_rc), _publication, &_input); + +} + +class PX4IO : public device::CDev +{ +public: + PX4IO(); + ~PX4IO(); + + virtual int init(); + + virtual ssize_t write(struct file *filp, const char *buffer, size_t buflen); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + +private: + int _fd; + int _task; + PX4IO_RC *_rc; + + /** command to be sent to IO */ + struct px4io_command _next_command; + + /** RC channel input from IO */ + servo_position_t _rc_channel[PX4IO_INPUT_CHANNELS]; + int _rc_channel_count; + + volatile bool _armed; + volatile bool _task_should_exit; + + bool _send_needed; + + hx_stream_t _io_stream; + + static void task_main_trampoline(int argc, char *argv[]); + void task_main(); + void io_recv(); + + static void rx_callback_trampoline(void *arg, const void *buffer, size_t bytes_received); + void rx_callback(const uint8_t *buffer, size_t bytes_received); + + void io_send(); +}; + +PX4IO::PX4IO() : + CDev("px4io", "/dev/px4io"), + _fd(-1), + _task(-1), + _rc(new PX4IO_RC), + _rc_channel_count(0), + _armed(false), + _task_should_exit(false), + _send_needed(false), + _io_stream(nullptr) +{ + /* set up the command we will use */ + _next_command.f2i_magic = F2I_MAGIC; + + /* we need this potentially before it could be set in px4io_main */ + g_dev = this; + + _debug_enabled = true; +} + +PX4IO::~PX4IO() +{ + if (_rc != nullptr) + delete _rc; + if (_task != -1) { + /* task should wake up every 100ms or so at least */ + _task_should_exit = true; + + unsigned i = 0; + do { + /* wait 20ms */ + usleep(20000); + + /* if we have given up, kill it */ + if (++i > 10) { + task_delete(_task); + break; + } + + } while (_task != -1); + } + + g_dev = nullptr; +} + +int +PX4IO::init() +{ + int ret; + + ASSERT(_task == -1); + + /* XXX send a who-are-you request */ + + /* XXX verify firmware/protocol version */ + + /* do regular cdev init */ + ret = CDev::init(); + if (ret != OK) + return ret; + + /* start the IO interface task */ + _task = task_create("px4io", SCHED_PRIORITY_DEFAULT, 1024, (main_t)&PX4IO::task_main_trampoline, nullptr); + if (_task < 0) { + debug("task start failed: %d", errno); + return -errno; + } + + return OK; +} + +void +PX4IO::task_main_trampoline(int argc, char *argv[]) +{ + g_dev->task_main(); +} + +void +PX4IO::task_main() +{ + ASSERT(_fd == -1); + + log("ready"); + + /* open the serial port */ + _fd = ::open("/dev/ttyS2", O_RDWR | O_NONBLOCK); + if (_fd < 0) { + debug("failed to open serial port for IO: %d", errno); + _task = -1; + _exit(errno); + } + + /* protocol stream */ + _io_stream = hx_stream_init(_fd, &PX4IO::rx_callback_trampoline, this); + + perf_counter_t pc_tx_bytes = perf_alloc(PC_COUNT, "PX4IO frames transmitted"); + perf_counter_t pc_rx_bytes = perf_alloc(PC_COUNT, "PX4IO frames received"); + perf_counter_t pc_rx_errors = perf_alloc(PC_COUNT, "PX4IO receive errors"); + hx_stream_set_counters(_io_stream, pc_tx_bytes, pc_rx_bytes, pc_rx_errors); + + /* poll descriptor(s) */ + struct pollfd fds[1]; + fds[0].fd = _fd; + fds[0].events = POLLIN; + + /* loop handling received serial bytes */ + while (!_task_should_exit) { + + /* sleep waiting for data, but no more than 100ms */ + int ret = ::poll(&fds[0], 1, 100); + + /* this would be bad... */ + if (ret < 0) { + log("poll error %d", errno); + usleep(1000000); + continue; + } + + /* if we timed out waiting, we should send an update */ + if (ret == 0) + _send_needed = true; + + /* if we have new data from IO, go handle it */ + if ((ret > 0) && (fds[0].revents & POLLIN)) + io_recv(); + + /* send an update to IO if required */ + if (_send_needed) { + _send_needed = false; + io_send(); + } + } + if (_io_stream != nullptr) + hx_stream_free(_io_stream); + ::close(_fd); + + /* tell the dtor that we are exiting */ + _task = -1; + _exit(0); +} + +void +PX4IO::io_recv() +{ + uint8_t c; + + /* handle bytes from IO */ + while (::read(_fd, &c, 1) == 1) + hx_stream_rx(_io_stream, c); +} + +void +PX4IO::rx_callback_trampoline(void *arg, const void *buffer, size_t bytes_received) +{ + g_dev->rx_callback((const uint8_t *)buffer, bytes_received); +} + +void +PX4IO::rx_callback(const uint8_t *buffer, size_t bytes_received) +{ + const struct px4io_report *rep = (const struct px4io_report *)buffer; + + /* sanity-check the received frame size */ + if (bytes_received != sizeof(struct px4io_report)) + return; + + lock(); + + /* pass RC input data to the driver */ + if (_rc != nullptr) + _rc->set_channels(rep->channel_count, &rep->rc_channel[0]); + _armed = rep->armed; + + /* send an update frame */ + _send_needed = true; + + unlock(); + +} + +void +PX4IO::io_send() +{ + lock(); + + /* send packet to IO while we're guaranteed it won't change */ + hx_stream_send(_io_stream, &_next_command, sizeof(_next_command)); + + unlock(); +} + +ssize_t +PX4IO::write(struct file *filp, const char *buffer, size_t len) +{ + unsigned channels = len / sizeof(servo_position_t); + servo_position_t *pdata = (servo_position_t *)buffer; + unsigned i; + + if (channels > PX4IO_OUTPUT_CHANNELS) + return -EIO; + + lock(); + for (i = 0; i < channels; i++) + _next_command.servo_command[i] = pdata[i]; + unlock(); + + return i * sizeof(servo_position_t); +} + +int +PX4IO::ioctl(struct file *filep, int cmd, unsigned long arg) +{ + int ret = -ENOTTY; + + lock(); + + /* regular ioctl? */ + switch (cmd) { + case PWM_SERVO_ARM: + _next_command.arm_ok = true; + ret = 0; + break; + + case PWM_SERVO_DISARM: + _next_command.arm_ok = false; + ret = 0; + break; + + default: + /* channel set? */ + if ((cmd >= PWM_SERVO_SET(0)) && (cmd < PWM_SERVO_SET(PX4IO_OUTPUT_CHANNELS))) { + /* XXX sanity-check value? */ + _next_command.servo_command[cmd - PWM_SERVO_SET(0)] = arg; + ret = 0; + break; + } + + /* channel get? */ + if ((cmd >= PWM_SERVO_GET(0)) && (cmd < PWM_SERVO_GET(PX4IO_INPUT_CHANNELS))) { + int channel = cmd - PWM_SERVO_GET(0); + + /* currently no data for this channel */ + if (channel >= _rc_channel_count) { + ret = -ERANGE; + break; + } + + *(servo_position_t *)arg = _rc_channel[channel]; + ret = 0; + break; + } + + /* not a recognised value */ + ret = -ENOTTY; + } + unlock(); + + return ret; +} + +extern "C" __EXPORT int px4io_main(int argc, char *argv[]); + +int +px4io_main(int argc, char *argv[]) +{ + if (!strcmp(argv[1], "start")) { + + if (g_dev != nullptr) { + fprintf(stderr, "PX4IO: already loaded\n"); + return -EBUSY; + } + + /* create the driver - it will set g_dev */ + (void)new PX4IO; + + if (g_dev == nullptr) { + fprintf(stderr, "PX4IO: driver alloc failed\n"); + return -ENOMEM; + } + + if (OK != g_dev->init()) { + fprintf(stderr, "PX4IO: driver init failed\n"); + delete g_dev; + return -EIO; + } + + return OK; + } + + if (!strcmp(argv[1], "update")) { + PX4IO_Uploader *up; + const char *fn[3]; + + /* work out what we're uploading... */ + if (argc > 2) { + fn[0] = argv[2]; + fn[1] = nullptr; + } else { + fn[0] = "/fs/microsd/px4io.bin"; + fn[1] = "/etc/px4io.bin"; + fn[2] = nullptr; + } + + up = new PX4IO_Uploader; + int ret = up->upload(&fn[0]); + delete up; + + switch (ret) { + case OK: + break; + case -ENOENT: + fprintf(stderr, "PX4IO firmware file '%s' not found\n", fn); + break; + case -EEXIST: + case -EIO: + fprintf(stderr, "error updating PX4IO - check that bootloader mode is enabled\n"); + break; + case -EINVAL: + fprintf(stderr, "verify failed - retry the update\n"); + break; + case -ETIMEDOUT: + fprintf(stderr, "timed out waiting for bootloader - power-cycle and try again\n"); + default: + fprintf(stderr, "unexpected error %d\n", ret); + break; + } + return ret; + } + + + + printf("need a verb, only support 'start'\n"); + return ERROR; +} diff --git a/apps/px4/px4io/driver/uploader.cpp b/apps/px4/px4io/driver/uploader.cpp new file mode 100644 index 000000000..7d6a9e171 --- /dev/null +++ b/apps/px4/px4io/driver/uploader.cpp @@ -0,0 +1,387 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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 Firmware uploader for PX4IO + */ + +#include <nuttx/config.h> + +#include <sys/types.h> +#include <stdint.h> +#include <stdbool.h> +#include <assert.h> +#include <errno.h> +#include <string.h> +#include <stdio.h> +#include <stdarg.h> +#include <unistd.h> +#include <fcntl.h> +#include <poll.h> + +#include "uploader.h" + +PX4IO_Uploader::PX4IO_Uploader() : + _io_fd(-1), + _fw_fd(-1) +{ +} + +PX4IO_Uploader::~PX4IO_Uploader() +{ +} + +int +PX4IO_Uploader::upload(const char *filenames[]) +{ + int ret; + + _io_fd = open("/dev/ttyS2", O_RDWR); + if (_io_fd < 0) { + log("could not open interface"); + return -errno; + } + + /* look for the bootloader */ + ret = sync(); + if (ret != OK) { + /* this is immediately fatal */ + log("bootloader not responding"); + return -EIO; + } + + for (unsigned i = 0; filenames[i] != nullptr; i++) { + _fw_fd = open(filenames[i], O_RDONLY); + + if (_fw_fd < 0) { + log("failed to open %s", filenames[i]); + continue; + } + log("using firmware from %s", filenames[i]); + break; + } + if (_fw_fd == -1) + return -ENOENT; + + /* do the usual program thing - allow for failure */ + for (unsigned retries = 0; retries < 1; retries++) { + if (retries > 0) { + log("retrying update..."); + ret = sync(); + if (ret != OK) { + /* this is immediately fatal */ + log("bootloader not responding"); + return -EIO; + } + } + + ret = erase(); + if (ret != OK) { + log("erase failed"); + continue; + } + ret = program(); + if (ret != OK) { + log("program failed"); + continue; + } + ret = verify(); + if (ret != OK) { + log("verify failed"); + continue; + } + ret = reboot(); + if (ret != OK) { + log("reboot failed"); + return ret; + } + log("update complete"); + + ret = OK; + break; + } + + close(_fw_fd); + return ret; +} + +int +PX4IO_Uploader::recv(uint8_t &c, unsigned timeout) +{ + struct pollfd fds[1]; + + fds[0].fd = _io_fd; + fds[0].events = POLLIN; + + /* wait 100 ms for a character */ + int ret = ::poll(&fds[0], 1, timeout); + if (ret < 1) { + //log("poll timeout %d", ret); + return -ETIMEDOUT; + } + + read(_io_fd, &c, 1); + //log("recv 0x%02x", c); + return OK; +} + +int +PX4IO_Uploader::recv(uint8_t *p, unsigned count) +{ + while (count--) { + int ret = recv(*p++); + if (ret != OK) + return ret; + } + return OK; +} + +void +PX4IO_Uploader::drain() +{ + uint8_t c; + int ret; + + do { + ret = recv(c, 10); + //log("discard 0x%02x", c); + } while(ret == OK); +} + +int +PX4IO_Uploader::send(uint8_t c) +{ + //log("send 0x%02x", c); + if (write(_io_fd, &c, 1) != 1) + return -errno; + return OK; +} + +int +PX4IO_Uploader::send(uint8_t *p, unsigned count) +{ + while (count--) { + int ret = send(*p++); + if (ret != OK) + return ret; + } + return OK; +} + +int +PX4IO_Uploader::get_sync(unsigned timeout) +{ + uint8_t c[2]; + int ret; + + ret = recv(c[0], timeout); + if (ret != OK) + return ret; + ret = recv(c[1], timeout); + if (ret != OK) + return ret; + if ((c[0] != PROTO_INSYNC) || (c[1] != PROTO_OK)) { + log("bad sync 0x%02x,0x%02x", c[0], c[1]); + return -EIO; + } + return OK; +} + +int +PX4IO_Uploader::sync() +{ + drain(); + /* complete any pending program operation */ + for (unsigned i = 0; i < (PROG_MULTI_MAX + 6); i++) + send(0); + send(PROTO_GET_SYNC); + send(PROTO_EOC); + return get_sync(); +} + +int +PX4IO_Uploader::get_info(int param, uint32_t &val) +{ + int ret; + + send(PROTO_GET_DEVICE); + send(param); + send(PROTO_EOC); + + ret = recv((uint8_t *)&val, sizeof(val)); + if (ret != OK) + return ret; + return get_sync(); +} + +int +PX4IO_Uploader::erase() +{ + log("erase..."); + send(PROTO_CHIP_ERASE); + send(PROTO_EOC); + return get_sync(10000); /* allow 10s timeout */ +} + +int +PX4IO_Uploader::program() +{ + uint8_t file_buf[PROG_MULTI_MAX]; + ssize_t count; + int ret; + + log("program..."); + lseek(_fw_fd, 0, SEEK_SET); + + while (true) { + /* get more bytes to program */ + //log(" %d", (int)lseek(_fw_fd, 0, SEEK_CUR)); + count = read(_fw_fd, file_buf, sizeof(file_buf)); + if (count == 0) + return OK; + if (count < 0) + return -errno; + ASSERT((count % 4) == 0); + + send(PROTO_PROG_MULTI); + send(count); + send(&file_buf[0], count); + send(PROTO_EOC); + + ret = get_sync(1000); + if (ret != OK) + return ret; + } +} + +int +PX4IO_Uploader::verify() +{ + uint8_t file_buf[PROG_MULTI_MAX]; + ssize_t count; + int ret; + + log("verify..."); + lseek(_fw_fd, 0, SEEK_SET); + + send(PROTO_CHIP_VERIFY); + send(PROTO_EOC); + ret = get_sync(); + if (ret != OK) + return ret; + + while (true) { + /* get more bytes to verify */ + int base = (int)lseek(_fw_fd, 0, SEEK_CUR); + count = read(_fw_fd, file_buf, sizeof(file_buf)); + if (count == 0) + break; + if (count < 0) + return -errno; + ASSERT((count % 4) == 0); + + send(PROTO_READ_MULTI); + send(count); + send(PROTO_EOC); + for (ssize_t i = 0; i < count; i++) { + uint8_t c; + + ret = recv(c); + if (ret != OK) { + log("%d: got %d waiting for bytes", ret); + return ret; + } + + if (c != file_buf[i]) { + log("%d: got 0x%02x expected 0x%02x", base + i, c, file_buf[i]); + return -EINVAL; + } + } + ret = get_sync(); + if (ret != OK) { + log("timeout waiting for post-verify sync"); + return ret; + } + } + return OK; +} + +int +PX4IO_Uploader::reboot() +{ + send(PROTO_REBOOT); + send(PROTO_EOC); + + return OK; +} + +int +PX4IO_Uploader::compare(bool &identical) +{ + uint32_t file_vectors[15]; + uint32_t fw_vectors[15]; + int ret; + + lseek(_fw_fd, 0, SEEK_SET); + ret = read(_fw_fd, &file_vectors[0], sizeof(file_vectors)); + + send(PROTO_CHIP_VERIFY); + send(PROTO_EOC); + ret = get_sync(); + if (ret != OK) + return ret; + + send(PROTO_READ_MULTI); + send(sizeof(fw_vectors)); + send(PROTO_EOC); + ret = recv((uint8_t *)&fw_vectors[0], sizeof(fw_vectors)); + if (ret != OK) + return ret; + + identical = (memcmp(&file_vectors[0], &fw_vectors[0], sizeof(file_vectors))) ? true : false; + + return OK; +} + +void +PX4IO_Uploader::log(const char *fmt, ...) +{ + va_list ap; + + printf("[PX4IO] "); + va_start(ap, fmt); + vprintf(fmt, ap); + va_end(ap); + printf("\n"); + fflush(stdout); +}
\ No newline at end of file diff --git a/apps/px4/px4io/driver/uploader.h b/apps/px4/px4io/driver/uploader.h new file mode 100644 index 000000000..8d41992f8 --- /dev/null +++ b/apps/px4/px4io/driver/uploader.h @@ -0,0 +1,99 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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 Firmware uploader for PX4IO + */ + +#ifndef _PX4IO_UPLOADER_H +#define _PX4IO_UPLOADER_H value + +#include <stdint.h> +#include <stdbool.h> + + +class PX4IO_Uploader +{ +public: + PX4IO_Uploader(); + ~PX4IO_Uploader(); + + int upload(const char *filenames[]); + +private: + enum { + + PROTO_NOP = 0x00, + PROTO_OK = 0x10, + PROTO_FAILED = 0x11, + PROTO_INSYNC = 0x12, + PROTO_EOC = 0x20, + PROTO_GET_SYNC = 0x21, + PROTO_GET_DEVICE = 0x22, + PROTO_CHIP_ERASE = 0x23, + PROTO_CHIP_VERIFY = 0x24, + PROTO_PROG_MULTI = 0x27, + PROTO_READ_MULTI = 0x28, + PROTO_REBOOT = 0x30, + + INFO_BL_REV = 1, /**< bootloader protocol revision */ + BL_REV = 2, /**< supported bootloader protocol */ + INFO_BOARD_ID = 2, /**< board type */ + INFO_BOARD_REV = 3, /**< board revision */ + INFO_FLASH_SIZE = 4, /**< max firmware size in bytes */ + + PROG_MULTI_MAX = 60, /**< protocol max is 255, must be multiple of 4 */ + READ_MULTI_MAX = 60, /**< protocol max is 255, something overflows with >= 64 */ + }; + + int _io_fd; + int _fw_fd; + + void log(const char *fmt, ...); + + int recv(uint8_t &c, unsigned timeout = 1000); + int recv(uint8_t *p, unsigned count); + void drain(); + int send(uint8_t c); + int send(uint8_t *p, unsigned count); + int get_sync(unsigned timeout = 1000); + int sync(); + int get_info(int param, uint32_t &val); + int erase(); + int program(); + int verify(); + int reboot(); + int compare(bool &identical); +}; + +#endif
\ No newline at end of file diff --git a/apps/px4/px4io/protocol.h b/apps/px4/px4io/protocol.h new file mode 100644 index 000000000..c186c5b86 --- /dev/null +++ b/apps/px4/px4io/protocol.h @@ -0,0 +1,73 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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 PX4FMU <-> PX4IO messaging protocol. + * + * This initial version of the protocol is very simple; each side transmits a + * complete update with each frame. This avoids the sending of many small + * messages and the corresponding complexity involved. + */ + +/* + * XXX MUST BE KEPT IN SYNC WITH THE VERSION IN PX4FMU UNTIL + * TREES ARE MERGED. + */ + +#define PX4IO_OUTPUT_CHANNELS 8 +#define PX4IO_INPUT_CHANNELS 12 +#define PX4IO_RELAY_CHANNELS 2 + +#pragma pack(push,1) + +/* command from FMU to IO */ +struct px4io_command { + uint16_t f2i_magic; +#define F2I_MAGIC 0x636d + + uint16_t servo_command[PX4IO_OUTPUT_CHANNELS]; + bool relay_state[PX4IO_RELAY_CHANNELS]; + bool arm_ok; +}; + +/* report from IO to FMU */ +struct px4io_report { + uint16_t i2f_magic; +#define I2F_MAGIC 0x7570 + + uint16_t rc_channel[PX4IO_INPUT_CHANNELS]; + bool armed; + uint8_t channel_count; +} __attribute__((packed)); + +#pragma pack(pop) diff --git a/apps/px4/sensors_bringup/.context b/apps/px4/sensors_bringup/.context new file mode 100644 index 000000000..e69de29bb --- /dev/null +++ b/apps/px4/sensors_bringup/.context diff --git a/apps/px4/sensors_bringup/Makefile b/apps/px4/sensors_bringup/Makefile new file mode 100644 index 000000000..8867653a0 --- /dev/null +++ b/apps/px4/sensors_bringup/Makefile @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (C) 2012 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. +# +############################################################################ + +# +# Makefile to build the sensor bringup tests +# + +APPNAME = sensors_bringup +PRIORITY = SCHED_PRIORITY_DEFAULT +STACKSIZE = 2048 + +include $(APPDIR)/mk/app.mk diff --git a/apps/px4/sensors_bringup/bma180.c b/apps/px4/sensors_bringup/bma180.c new file mode 100644 index 000000000..6c4b9d483 --- /dev/null +++ b/apps/px4/sensors_bringup/bma180.c @@ -0,0 +1,209 @@ +/* + * Operations for the Bosch BMA180 3D Accelerometer + */ + +#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 <nuttx/spi.h> + +#include "sensors.h" + +#define DIR_READ (1<<7) +#define DIR_WRITE (0<<7) +#define ADDR_INCREMENT (1<<6) + +#define ADDR_CHIP_ID 0x00 +#define CHIP_ID 0x03 +#define ADDR_VERSION 0x01 + +#define ADDR_CTRL_REG0 0x0D +#define ADDR_CTRL_REG1 0x0E +#define ADDR_CTRL_REG2 0x0F +#define ADDR_BWTCS 0x20 +#define ADDR_CTRL_REG3 0x21 +#define ADDR_CTRL_REG4 0x22 +#define ADDR_OLSB1 0x35 + +#define ADDR_ACC_X_LSB 0x02 +#define ADDR_ACC_Z_MSB 0x07 +#define ADDR_TEMPERATURE 0x08 + +#define ADDR_STATUS_REG1 0x09 +#define ADDR_STATUS_REG2 0x0A +#define ADDR_STATUS_REG3 0x0B +#define ADDR_STATUS_REG4 0x0C + +#define ADDR_RESET 0x10 +#define SOFT_RESET 0xB6 + +#define ADDR_DIS_I2C 0x27 + +#define REG0_WRITE_ENABLE 0x10 + +#define BWTCS_LP_10HZ (0<<4) +#define BWTCS_LP_20HZ (1<<4) +#define BWTCS_LP_40HZ (2<<4) +#define BWTCS_LP_75HZ (3<<4) +#define BWTCS_LP_150HZ (4<<4) +#define BWTCS_LP_300HZ (5<<4) +#define BWTCS_LP_600HZ (6<<4) +#define BWTCS_LP_1200HZ (7<<4) + +#define RANGE_1G (0<<1) +#define RANGE_1_5G (1<<1) +#define RANGE_2G (2<<1) +#define RANGE_3G (3<<1) +#define RANGE_4G (4<<1) +#define RANGE_8G (5<<1) +#define RANGE_16G (6<<1) + +#define RANGEMASK 0x0E +#define BWMASK 0xF0 + + +static void +write_reg(struct spi_dev_s *spi, uint8_t address, uint8_t data) +{ + uint8_t cmd[2] = { address | DIR_WRITE, data }; + + SPI_SELECT(spi, PX4_SPIDEV_ACCEL, true); + SPI_SNDBLOCK(spi, &cmd, sizeof(cmd)); + SPI_SELECT(spi, PX4_SPIDEV_ACCEL, false); +} + +static uint8_t +read_reg(struct spi_dev_s *spi, uint8_t address) +{ + uint8_t cmd[2] = {address | DIR_READ, 0}; + uint8_t data[2]; + + SPI_SELECT(spi, PX4_SPIDEV_ACCEL, true); + SPI_EXCHANGE(spi, cmd, data, sizeof(cmd)); + SPI_SELECT(spi, PX4_SPIDEV_ACCEL, false); + + return data[1]; +} + +int +bma180_test_configure(struct spi_dev_s *spi) +{ + uint8_t id; + + id = read_reg(spi, ADDR_CHIP_ID); + uint8_t version = read_reg(spi, 0x01); + + if (id == CHIP_ID) + { + message("BMA180 SUCCESS: 0x%02x, version: %d\n", id, version); + } + else + { + message("BMA180 FAIL: 0x%02x\n", id); + } + //message("got id 0x%02x, expected ID 0x03\n", id); + + write_reg(spi, ADDR_RESET, SOFT_RESET); // page 48 + usleep(12000); // wait 10 ms, see page 49 + + // Configuring the BMA180 + + /* enable writing to chip config */ + uint8_t ctrl0 = read_reg(spi, ADDR_CTRL_REG0); + ctrl0 |= REG0_WRITE_ENABLE; + write_reg(spi, ADDR_CTRL_REG0, ctrl0); + + uint8_t disi2c = read_reg(spi, ADDR_DIS_I2C); // read + disi2c |= 0x01; // set bit0 to 1, SPI only + write_reg(spi, ADDR_DIS_I2C, disi2c); // Set spi, disable i2c, page 31 + + /* set bandwidth */ + uint8_t bwtcs = read_reg(spi, ADDR_BWTCS); + printf("bwtcs: %d\n", bwtcs); + bwtcs &= (~BWMASK); + bwtcs |= (BWTCS_LP_600HZ);// & BWMASK); + write_reg(spi, ADDR_BWTCS, bwtcs); + + /* set range */ + uint8_t olsb1 = read_reg(spi, ADDR_OLSB1); + printf("olsb1: %d\n", olsb1); + olsb1 &= (~RANGEMASK); + olsb1 |= (RANGE_4G);// & RANGEMASK); + write_reg(spi, ADDR_OLSB1, olsb1); + +// uint8_t reg3 = read_reg(spi, ADDR_CTRL_REG3); +// //reg3 &= 0xFD; // REset bit 1 enable interrupt +// //reg3 |= 0x02; // enable +// write_reg(spi, ADDR_CTRL_REG3, reg3); // + + /* block writing to chip config */ + ctrl0 = read_reg(spi, ADDR_CTRL_REG0); + ctrl0 &= (~REG0_WRITE_ENABLE); + printf("ctrl0: %d\n", ctrl0); + write_reg(spi, ADDR_CTRL_REG0, ctrl0); + + return 0; +} + +int +bma180_test_read(struct spi_dev_s *spi) +{ + + + + struct { /* status register and data as read back from the device */ + uint8_t cmd; + int16_t x; + int16_t y; + int16_t z; + uint8_t temp; + } __attribute__((packed)) report; + + report.x = 0; + report.y = 0; + report.z = 0; + +// uint8_t temp; +// uint8_t status1; +// uint8_t status2; +// uint8_t status3; +// uint8_t status4; + + report.cmd = ADDR_ACC_X_LSB | DIR_READ | ADDR_INCREMENT; + + //SPI_LOCK(spi, true); + //SPI_SELECT(spi, PX4_SPIDEV_ACCEL, true); + //SPI_EXCHANGE(spi, &report, &report, sizeof(report)); + //SPI_SELECT(spi, PX4_SPIDEV_ACCEL, false); + //SPI_LOCK(spi, false); + + report.x = read_reg(spi, ADDR_ACC_X_LSB); + report.x |= (read_reg(spi, ADDR_ACC_X_LSB+1) << 8); + report.y = read_reg(spi, ADDR_ACC_X_LSB+2); + report.y |= (read_reg(spi, ADDR_ACC_X_LSB+3) << 8); + report.z = read_reg(spi, ADDR_ACC_X_LSB+4); + report.z |= (read_reg(spi, ADDR_ACC_X_LSB+5) << 8); + report.temp = read_reg(spi, ADDR_ACC_X_LSB+6); + + // Collect status and remove two top bits + + uint8_t new_data = (report.x & 0x01) + (report.x & 0x01) + (report.x & 0x01); + report.x = (report.x >> 2); + report.y = (report.y >> 2); + report.z = (report.z >> 2); + + message("ACC: x: %d\ty: %d\tz: %d\ttemp: %d new: %d\n", report.x, report.y, report.z, report.temp, new_data); + usleep(2000); + + return 0; +} diff --git a/apps/px4/sensors_bringup/l3gd20.c b/apps/px4/sensors_bringup/l3gd20.c new file mode 100644 index 000000000..5bdc10e06 --- /dev/null +++ b/apps/px4/sensors_bringup/l3gd20.c @@ -0,0 +1,184 @@ +/* + * Operations for the l3g4200 + */ + +#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 <nuttx/spi.h> + +#include "sensors.h" + +#define DIR_READ (1<<7) +#define DIR_WRITE (0<<7) +#define ADDR_INCREMENT (1<<6) + +#define ADDR_WHO_AM_I 0x0f +#define WHO_I_AM 0xd4 + +#define ADDR_CTRL_REG1 0x20 /* sample rate constants are in the public header */ +#define REG1_POWER_NORMAL (1<<3) +#define REG1_Z_ENABLE (1<<2) +#define REG1_Y_ENABLE (1<<1) +#define REG1_X_ENABLE (1<<0) + +#define ADDR_CTRL_REG2 0x21 +/* high-pass filter - usefulness TBD */ + +#define ADDR_CTRL_REG3 0x22 + +#define ADDR_CTRL_REG4 0x23 +#define REG4_BDU (1<<7) +#define REG4_BIG_ENDIAN (1<<6) +#define REG4_SPI_3WIRE (1<<0) + +#define ADDR_CTRL_REG5 0x24 +#define REG5_BOOT (1<<7) +#define REG5_FIFO_EN (1<<6) +#define REG5_HIGHPASS_ENABLE (1<<4) + +#define ADDR_REFERENCE 0x25 +#define ADDR_TEMPERATURE 0x26 + +#define ADDR_STATUS_REG 0x27 +#define STATUS_ZYXOR (1<<7) +#define SATAUS_ZOR (1<<6) +#define STATUS_YOR (1<<5) +#define STATUS_XOR (1<<4) +#define STATUS_ZYXDA (1<<3) +#define STATUS_ZDA (1<<2) +#define STATUS_YDA (1<<1) +#define STATUS_XDA (1<<0) + +#define ADDR_OUT_X 0x28 /* 16 bits */ +#define ADDR_OUT_Y 0x2A /* 16 bits */ +#define ADDR_OUT_Z 0x2C /* 16 bits */ + +#define ADDR_FIFO_CTRL 0x2e +#define FIFO_MODE_BYPASS (0<<5) +#define FIFO_MODE_FIFO (1<<5) +#define FIFO_MODE_STREAM (2<<5) +#define FIFO_MODE_STREAM_TO_FIFO (3<<5) +#define FIFO_MODE_BYPASS_TO_STREAM (4<<5) +#define FIFO_THRESHOLD_MASK 0x1f + +#define ADDR_FIFO_SRC 0x2f +#define FIFO_THREHSHOLD_OVER (1<<7) +#define FIFO_OVERRUN (1<<6) +#define FIFO_EMPTY (1<<5) + +#define L3G4200_RATE_100Hz ((0<<6) | (0<<4)) +#define L3G4200_RATE_200Hz ((1<<6) | (0<<4)) +#define L3G4200_RATE_400Hz ((2<<6) | (1<<4)) +#define L3G4200_RATE_800Hz ((3<<6) | (2<<4)) + +#define L3G4200_RANGE_250dps (0<<4) +#define L3G4200_RANGE_500dps (1<<4) +#define L3G4200_RANGE_2000dps (3<<4) + +static void +write_reg(struct spi_dev_s *spi, uint8_t address, uint8_t data) +{ + uint8_t cmd[2] = { address | DIR_WRITE, data }; + + SPI_SELECT(spi, PX4_SPIDEV_GYRO, true); + SPI_SNDBLOCK(spi, &cmd, sizeof(cmd)); + SPI_SELECT(spi, PX4_SPIDEV_GYRO, false); +} + +static uint8_t +read_reg(struct spi_dev_s *spi, uint8_t address) +{ + uint8_t cmd[2] = {address | DIR_READ, 0}; + uint8_t data[2]; + + SPI_SELECT(spi, PX4_SPIDEV_GYRO, true); + SPI_EXCHANGE(spi, cmd, data, sizeof(cmd)); + SPI_SELECT(spi, PX4_SPIDEV_GYRO, false); + + return data[1]; +} + +int +l3gd20_test_configure(struct spi_dev_s *spi) +{ + uint8_t id; + + id = read_reg(spi, ADDR_WHO_AM_I); + + if (id == WHO_I_AM) + { + message("L3GD20 SUCCESS: 0x%02x\n", id); + } + else + { + message("L3GD20 FAIL: 0x%02x\n", id); + } + + struct { /* status register and data as read back from the device */ + uint8_t cmd; + uint8_t temp; + uint8_t status; + int16_t x; + int16_t y; + int16_t z; + } __attribute__((packed)) report; + + report.cmd = 0x26 | DIR_READ | ADDR_INCREMENT; + + write_reg(spi, ADDR_CTRL_REG2, 0); /* disable high-pass filters */ + write_reg(spi, ADDR_CTRL_REG3, 0); /* no interrupts - we don't use them */ + write_reg(spi, ADDR_CTRL_REG5, 0); /* turn off FIFO mode */ + + write_reg(spi, ADDR_CTRL_REG4, ((3<<4) & 0x30) | REG4_BDU); + + + write_reg(spi, ADDR_CTRL_REG1, + (((2<<6) | (1<<4)) & 0xf0) | REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE); + + SPI_SELECT(spi, PX4_SPIDEV_GYRO, true); + SPI_EXCHANGE(spi, &report, &report, sizeof(report)); + SPI_SELECT(spi, PX4_SPIDEV_GYRO, false); + + message("Init-read: gyro: x: %d\ty: %d\tz: %d\n", report.x, report.y, report.z); + usleep(1000); + + //message("got id 0x%02x, expected ID 0xd4\n", id); + + return 0; +} + +int +l3gd20_test_read(struct spi_dev_s *spi) +{ + struct { /* status register and data as read back from the device */ + uint8_t cmd; + uint8_t temp; + uint8_t status; + int16_t x; + int16_t y; + int16_t z; + } __attribute__((packed)) report; + + report.cmd = 0x26 | DIR_READ | ADDR_INCREMENT; + + SPI_LOCK(spi, true); + SPI_SELECT(spi, PX4_SPIDEV_GYRO, true); + SPI_EXCHANGE(spi, &report, &report, sizeof(report)); + SPI_SELECT(spi, PX4_SPIDEV_GYRO, false); + SPI_LOCK(spi, false); + + message("gyro: x: %d\ty: %d\tz: %d\n", report.x, report.y, report.z); + usleep(1000); + return 0; +} diff --git a/apps/px4/sensors_bringup/sensors.h b/apps/px4/sensors_bringup/sensors.h new file mode 100644 index 000000000..5fc2fbb08 --- /dev/null +++ b/apps/px4/sensors_bringup/sensors.h @@ -0,0 +1,88 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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. + * + ****************************************************************************/ + +#ifndef __APPS_PX4_SENSORS_H +#define __APPS_PX4_SENSORS_H + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> +#include <nuttx/spi.h> +#include <nuttx/i2c.h> + +/**************************************************************************** + * Definitions + ****************************************************************************/ + +/* Debug ********************************************************************/ + +#ifdef CONFIG_CPP_HAVE_VARARGS +# ifdef CONFIG_DEBUG +# define message(...) lib_rawprintf(__VA_ARGS__) +# define msgflush() +# else +# define message(...) printf(__VA_ARGS__) +# define msgflush() fflush(stdout) +# endif +#else +# ifdef CONFIG_DEBUG +# define message lib_rawprintf +# define msgflush() +# else +# define message printf +# define msgflush() fflush(stdout) +# endif +#endif + +/**************************************************************************** + * Public Types + ****************************************************************************/ + +/**************************************************************************** + * Public Variables + ****************************************************************************/ + +/**************************************************************************** + * Public Function Prototypes + ****************************************************************************/ + +int l3gd20_test_configure(struct spi_dev_s *spi); +int l3gd20_test_read(struct spi_dev_s *spi); +int bma180_test_configure(struct spi_dev_s *spi); +int bma180_test_read(struct spi_dev_s *spi); +int bma180_test(struct spi_dev_s *spi); +int hmc5883l_test(struct i2c_dev_s *i2c); + +#endif /* __APPS_PX4_SENSORS_H */ diff --git a/apps/px4/sensors_bringup/sensors_main.c b/apps/px4/sensors_bringup/sensors_main.c new file mode 100644 index 000000000..3aa21ae14 --- /dev/null +++ b/apps/px4/sensors_bringup/sensors_main.c @@ -0,0 +1,409 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#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 <nuttx/spi.h> +#include <nuttx/i2c.h> + +#include "sensors.h" + +__EXPORT int sensors_bringup_main(int argc, char *argv[]); + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: user_start/adc_main + ****************************************************************************/ + +int sensors_bringup_main(int argc, char *argv[]) +{ + struct spi_dev_s *spi; + int result = -1; + + spi = up_spiinitialize(1); + if (!spi) { + message("Failed to initialize SPI port 1\n"); + goto out; + } + + struct i2c_dev_s *i2c; + i2c = up_i2cinitialize(2); + if (!i2c) { + message("Failed to initialize I2C bus 2\n"); + goto out; + } + + int ret; + +#define EEPROM_ADDRESS 0x50 +#define HMC5883L_ADDRESS 0x1E + + //uint8_t devaddr = EEPROM_ADDRESS; + + I2C_SETFREQUENCY(i2c, 100000); +// +// uint8_t subaddr = 0x00; +// int ret = 0; +// +// // ATTEMPT HMC5883L CONFIG +// I2C_SETADDRESS(i2c, HMC5883L_ADDRESS, 7); +// subaddr = 0x02; // mode register +// ret = I2C_WRITE(i2c, &subaddr, 0); +// if (ret < 0) +// { +// message("I2C_WRITE failed: %d\n", ret); +// } +// else +// { +// message("I2C_WRITE SUCCEEDED: %d\n", ret); +// } + + //fflush(stdout); +// +// +// + + +#define STATUS_REGISTER 0x09 // Of HMC5883L + + // ATTEMPT HMC5883L WRITE + I2C_SETADDRESS(i2c, HMC5883L_ADDRESS, 7); + uint8_t cmd = 0x09; + uint8_t status_id[4] = {0, 0, 0, 0}; + + + ret = I2C_WRITEREAD(i2c, &cmd, 1, status_id, 4); + + if (ret >= 0 && status_id[1] == 'H' && status_id[2] == '4' && status_id[3] == '3') + { + message("HMC5883L identified, device status: %d\n", status_id[0]); + } else { + message("HMC5883L identification failed: %d\n", ret); + } + +#define HMC5883L_ADDR_CONF_A 0x00 +#define HMC5883L_ADDR_CONF_B 0x01 +#define HMC5883L_ADDR_MODE 0x02 + +#define HMC5883L_AVERAGING_1 (0 << 5) /* conf a register */ +#define HMC5883L_AVERAGING_2 (1 << 5) +#define HMC5883L_AVERAGING_4 (2 << 5) +#define HMC5883L_AVERAGING_8 (3 << 5) + +#define HMC5883L_RATE_75HZ (6 << 2) /* 75 Hz */ + +#define HMC5883L_RANGE_0_88GA (0 << 5) + + uint8_t rate_cmd[] = {HMC5883L_ADDR_CONF_A, HMC5883L_RATE_75HZ | HMC5883L_AVERAGING_8}; + ret = I2C_WRITE(i2c, rate_cmd, sizeof(rate_cmd)); + message("Wrote %d into register 0x00 of HMC, result: %d (0 = success)\n", HMC5883L_RATE_75HZ | HMC5883L_AVERAGING_8, ret); + + uint8_t range_cmd[] = {HMC5883L_ADDR_CONF_B, HMC5883L_RANGE_0_88GA}; + ret = I2C_WRITE(i2c, range_cmd, sizeof(range_cmd)); + message("Wrote %d into register 0x01 of HMC, result: %d (0 = success)\n", HMC5883L_RANGE_0_88GA, ret); + + // Set HMC into continous mode + // First write address, then value + uint8_t cont_address[] = {HMC5883L_ADDR_MODE, 0x00}; + ret = I2C_WRITE(i2c, cont_address, sizeof(cont_address)); + + message("Wrote 0x00 into register 0x02 of HMC, result: %d (0 = success)\n", ret); + + + // ATTEMPT HMC5883L READ + int h = 0; + + I2C_SETADDRESS(i2c, HMC5883L_ADDRESS, 7); + for (h = 0; h < 5; h++) + { + + cont_address[0] = HMC5883L_ADDR_MODE; + cont_address[1] = 0x01; + ret = I2C_WRITE(i2c, cont_address, sizeof(cont_address)); + + message("Wrote 0x01 into register 0x02 of HMC, result: %d (0 = success)\n", ret); + + usleep(100000); + + cont_address[1] = 0x00; + uint8_t dummy; + ret = I2C_WRITEREAD(i2c, cont_address, sizeof(cont_address), &dummy, 1); + + message("Wrote 0x00 into register 0x02 of HMC, result: %d (0 = success)\n", ret); + + usleep(100000); + + + int16_t hmc5883l_data[3] = {0, 0, 0}; + uint8_t data_address = 0x03; + uint8_t* data_ptr = (uint8_t*)hmc5883l_data; + ret = I2C_WRITEREAD(i2c, &data_address, 1, data_ptr, 6); + if (ret < 0) + { + message("HMC5883L READ failed: %d\n", ret); + } + else + { + // mask out top four bits as only 12 bits are valid + hmc5883l_data[0] &= 0xFFF; + hmc5883l_data[1] &= 0xFFF; + hmc5883l_data[2] &= 0xFFF; + + message("HMC5883L READ SUCCEEDED: %d, val: %d %d %d\n", ret, hmc5883l_data[0], hmc5883l_data[1], hmc5883l_data[2]); + uint8_t hmc_status; + ret = I2C_WRITEREAD(i2c, &cmd, 1, &hmc_status, 1); + + message("\t status: %d\n", hmc_status); + } + } + + + // Possible addresses: 0x77 or 0x76 +#define MS5611_ADDRESS_1 0x76 +#define MS5611_ADDRESS_2 0x77 + I2C_SETADDRESS(i2c, MS5611_ADDRESS_1, 7); + // Reset cmd + uint8_t ms5611_cmd[2] = {0x00, 0x1E}; + ret = I2C_WRITE(i2c, ms5611_cmd, 2); + if (ret < 0) + { + message("MS5611 #1 WRITE failed: %d\n", ret); + } + else + { + message("MS5611 #1 WRITE SUCCEEDED: %d\n", ret); + } + + fflush(stdout); + + I2C_SETADDRESS(i2c, MS5611_ADDRESS_2, 7); + ret = I2C_WRITE(i2c, ms5611_cmd, 2); + if (ret < 0) + { + message("MS5611 #2 WRITE failed: %d\n", ret); + } + else + { + message("MS5611 #2 WRITE SUCCEEDED: %d\n", ret); + } + + fflush(stdout); + + + // Wait for reset to complete (10 ms nominal, wait: 100 ms) + usleep(100000); + + // Read PROM data + uint8_t prom_buf[2] = {0,1}; + + uint16_t calibration[6]; + + int i = 0; + + prom_buf[0] = 0xA2 + (i*2); + + struct i2c_msg_s msgv[2] = { + { + .addr = MS5611_ADDRESS_2, + .flags = 0, + .buffer = prom_buf, + .length = 1 + }, + { + .addr = MS5611_ADDRESS_2, + .flags = I2C_M_READ, + .buffer = prom_buf, + .length = 1 + } + }; + + calibration[i] = prom_buf[0]*256; + calibration[i]+= prom_buf[1]; + + int retval; + + if ( (retval = I2C_TRANSFER(i2c, msgv, 2)) == OK ) + { + printf("SUCCESS ACCESSING PROM OF MS5611: %d, value C1: %d\n", retval, (int)calibration[0]); + } + else + { + printf("FAIL ACCESSING PROM OF MS5611\n"); + } + + + + + // TESTING CODE, EEPROM READ/WRITE + uint8_t val[1] = {10}; + int retval_eeprom; + uint8_t eeprom_subaddr[2] = {0, 0}; + + struct i2c_msg_s msgv_eeprom[2] = { + { + .addr = EEPROM_ADDRESS, + .flags = 0, + .buffer = eeprom_subaddr, + .length = 2 + }, + { + .addr = EEPROM_ADDRESS, + .flags = I2C_M_READ, + .buffer = val, + .length = 1 + } + }; + + val[0] = 5; + + if ( (retval_eeprom = I2C_TRANSFER(i2c, msgv_eeprom, 2)) == OK ) + { + printf("SUCCESS READING EEPROM: %d, value: %d\n", retval_eeprom, (int)val[0]); + } + else + { + printf("FAIL READING EEPROM: %d, value: %d\n", retval_eeprom, (int)val[0]); + } + + // Increment val + val[0] = val[0] + 1; + + struct i2c_msg_s msgv_eeprom_write[2] = { + { + .addr = EEPROM_ADDRESS, + .flags = I2C_M_NORESTART, + .buffer = eeprom_subaddr, + .length = 2 + }, + { + .addr = EEPROM_ADDRESS, + .flags = I2C_M_NORESTART, + .buffer = val, + .length = 1 + } + }; + + + if ( (retval_eeprom = I2C_TRANSFER(i2c, msgv_eeprom_write, 2)) == OK ) + { + printf("SUCCESS WRITING EEPROM: %d\n", retval_eeprom); + } + + usleep(10000); + + struct i2c_msg_s msgv_eeprom2[2] = { + { + .addr = EEPROM_ADDRESS, + .flags = 0, + .buffer = eeprom_subaddr, + .length = 2 + }, + { + .addr = EEPROM_ADDRESS, + .flags = I2C_M_READ, + .buffer = val, + .length = 1 + } + }; + + val[0] = 5; + + + if ( (retval_eeprom = I2C_TRANSFER(i2c, msgv_eeprom2, 2)) == OK ) + { + printf("SUCCESS READING WRITE RESULT EEPROM: %d, value: %d\n", retval_eeprom, (int)val[0]); + } + else + { + printf("FAIL READING WRITE RESULT EEPROM: %d, value: %d\n", retval_eeprom, (int)val[0]); + } + + // Configure sensors + l3gd20_test_configure(spi); + bma180_test_configure(spi); + + for (int i = 0; i < 3; i++) + { + l3gd20_test_read(spi); + bma180_test_read(spi); + printf("# %d of 10\n", i+1); + usleep(50000); + } + + + out: + msgflush(); + return result; +} diff --git a/apps/px4/tests/.context b/apps/px4/tests/.context new file mode 100644 index 000000000..e69de29bb --- /dev/null +++ b/apps/px4/tests/.context diff --git a/apps/px4/tests/Makefile b/apps/px4/tests/Makefile new file mode 100644 index 000000000..41979f85a --- /dev/null +++ b/apps/px4/tests/Makefile @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (C) 2012 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. +# +############################################################################ + +# +# Makefile to build assorted test cases +# + +APPNAME = tests +PRIORITY = SCHED_PRIORITY_DEFAULT +STACKSIZE = 4096 + +include $(APPDIR)/mk/app.mk diff --git a/apps/px4/tests/test_adc.c b/apps/px4/tests/test_adc.c new file mode 100644 index 000000000..7795f07ac --- /dev/null +++ b/apps/px4/tests/test_adc.c @@ -0,0 +1,211 @@ +/**************************************************************************** + * px4/sensors/test_gpio.c + * + * Copyright (C) 2012 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> +#include <nuttx/arch.h> + +#include <sys/types.h> + +#include <stdio.h> +#include <stdlib.h> +#include <unistd.h> +#include <fcntl.h> +#include <errno.h> +#include <debug.h> + +#include <nuttx/spi.h> + +#include "tests.h" + +#include <nuttx/analog/adc.h> + + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: test_gpio + ****************************************************************************/ + +int test_adc(int argc, char *argv[]) +{ + int fd0; + int ret = 0; + //struct adc_msg_s sample[4],sample2[4],sample3[4],sample4[4],sample5[4],sample6[4],sample7[4],sample8[4],sample9[4]; + + struct adc_msg4_s { + uint8_t am_channel1; /* The 8-bit ADC Channel */ + int32_t am_data1; /* ADC convert result (4 bytes) */ + uint8_t am_channel2; /* The 8-bit ADC Channel */ + int32_t am_data2; /* ADC convert result (4 bytes) */ + uint8_t am_channel3; /* The 8-bit ADC Channel */ + int32_t am_data3; /* ADC convert result (4 bytes) */ + uint8_t am_channel4; /* The 8-bit ADC Channel */ + int32_t am_data4; /* ADC convert result (4 bytes) */ + } __attribute__((__packed__));; + + struct adc_msg4_s sample1[4], sample2[4]; + + size_t readsize; + ssize_t nbytes, nbytes2; + int i; + int j; + int errval; + + for (j = 0; j < 1; j++) { + char name[11]; + sprintf(name, "/dev/adc%d", j); + fd0 = open(name, O_RDONLY | O_NONBLOCK); + + if (fd0 < 0) { + printf("ADC: %s open fail\n", name); + return ERROR; + + } else { + printf("Opened %s successfully\n", name); + } + + /* first adc read round */ + readsize = 4 * sizeof(struct adc_msg_s); + up_udelay(10000);//microseconds + nbytes = read(fd0, sample1, readsize); + up_udelay(10000);//microseconds + nbytes2 = read(fd0, sample2, readsize); +// nbytes2 = read(fd0, sample3, readsize); +// nbytes2 = read(fd0, sample4, readsize); +// nbytes2 = read(fd0, sample5, readsize); +// nbytes2 = read(fd0, sample6, readsize); +// nbytes2 = read(fd0, sample7, readsize); +// nbytes2 = read(fd0, sample8, readsize); + //nbytes2 = read(fd0, sample9, readsize); + + /* Handle unexpected return values */ + + if (nbytes < 0) { + errval = errno; + + if (errval != EINTR) { + message("read %s failed: %d\n", + name, errval); + errval = 3; + goto errout_with_dev; + } + + message("\tInterrupted read...\n"); + + } else if (nbytes == 0) { + message("\tNo data read, Ignoring\n"); + } + + /* Print the sample data on successful return */ + + else { + int nsamples = nbytes / sizeof(struct adc_msg_s); + + if (nsamples * sizeof(struct adc_msg_s) != nbytes) { + message("\tread size=%d is not a multiple of sample size=%d, Ignoring\n", + nbytes, sizeof(struct adc_msg_s)); + + } else { + message("Sample:"); + + for (i = 0; i < 1 ; i++) { + message("%d: channel: %d value: %d\n", + i, sample1[i].am_channel1, sample1[i].am_data1); + message("Sample:"); + message("%d: channel: %d value: %d\n", + i, sample1[i].am_channel2, sample1[i].am_data2); + message("Sample:"); + message("%d: channel: %d value: %d\n", + i, sample1[i].am_channel3, sample1[i].am_data3); + message("Sample:"); + message("%d: channel: %d value: %d\n", + i, sample1[i].am_channel4, sample1[i].am_data4); + message("Sample:"); + message("%d: channel: %d value: %d\n", + i + 1, sample2[i].am_channel1, sample2[i].am_data1); + message("Sample:"); + message("%d: channel: %d value: %d\n", + i + 1, sample2[i].am_channel2, sample2[i].am_data2); + message("Sample:"); + message("%d: channel: %d value: %d\n", + i + 1, sample2[i].am_channel3, sample2[i].am_data3); + message("Sample:"); + message("%d: channel: %d value: %d\n", + i + 1, sample2[i].am_channel4, sample2[i].am_data4); +// message("%d: channel: %d value: %d\n", +// i, sample9[i].am_channel, sample9[i].am_data); + } + } + } + } + + printf("\t ADC test successful.\n"); + +errout_with_dev: + + if (fd0 != 0) close(fd0); + + return ret; +} diff --git a/apps/px4/tests/test_eeproms.c b/apps/px4/tests/test_eeproms.c new file mode 100644 index 000000000..29ca8267f --- /dev/null +++ b/apps/px4/tests/test_eeproms.c @@ -0,0 +1,328 @@ +/**************************************************************************** + * px4/eeproms/test_eeproms.c + * + * Copyright (C) 2012 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <sys/types.h> + +#include <stdio.h> +#include <stdlib.h> +#include <unistd.h> +#include <string.h> +#include <fcntl.h> +#include <errno.h> +#include <debug.h> + +#include <arch/board/board.h> + +#include "tests.h" + +#include <arch/board/drv_eeprom.h> + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +static int onboard_eeprom(int argc, char *argv[]); +static int baseboard_eeprom(int argc, char *argv[]); + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +struct { + const char *name; + const char *path; + int (* test)(int argc, char *argv[]); +} eeproms[] = { + {"onboard_eeprom", "/dev/eeprom", onboard_eeprom}, + {"baseboard_eeprom", "/dev/baseboard_eeprom", baseboard_eeprom}, + {NULL, NULL, NULL} +}; + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +static int +onboard_eeprom(int argc, char *argv[]) +{ + printf("\tonboard_eeprom: test start\n"); + fflush(stdout); + + int fd; + uint8_t buf1[210] = {' ', 'P', 'X', '4', ' ', 'E', 'E', 'P', 'R', 'O', 'M', ' ', 'T', 'E', 'S', 'T', ' '}; + int ret; + bool force_write = false; + if (strcmp(argv[0], "jig") == 0) force_write = true; + + /* fill with spaces */ + //memset(buf1+16, 'x', sizeof(buf1-16)); + + /* fill in some magic values at magic positions */ + buf1[63] = 'E'; + buf1[64] = 'S'; + buf1[127] = 'F'; + buf1[128] = 'T'; + + /* terminate string */ + buf1[sizeof(buf1) - 1] = '\0'; + + fd = open("/dev/eeprom", O_RDWR | O_NONBLOCK); + + if (fd < 0) { + printf("onboard eeprom: open fail\n"); + return ERROR; + } + + /* read data */ + ret = read(fd, buf1, 1); + + if (ret != 1) { + printf("\tonboard eeprom: ERROR: reading first byte fail: %d\n", ret); + + switch (-ret) { + case EPERM: + printf("\treason: %s\n", EPERM_STR); + break; + + case ENOENT: + printf("\treason: %s\n", ENOENT_STR); + break; + + case ESRCH: + printf("\treason: %s\n", ESRCH_STR); + break; + + case EINTR: + printf("\treason: %s\n", EINTR_STR); + break; + + } + } + + printf("\tonboard eeprom: first byte: %d\n", buf1[0]); + if (!force_write) { + printf("\tonboard eeprom: WARNING: FURTHER TEST STEPS WILL DESTROY YOUR FLIGHT PARAMETER CONFIGURATION. PROCEED? (y/N)\n"); + + printf("Input: "); + char c = getchar(); + printf("%c\n", c); + if (c != 'y' && c != 'Y') { + /* not yes, abort */ + close(fd); + + /* Let user know everything is ok */ + printf("\tOK: onboard eeprom test aborted by user, read test successful\r\n"); + return OK; + } + } + + printf("\tonboard eeprom: proceeding with write test\r\n"); + + /* increment counter */ + buf1[0] = buf1[0] + 1; + + /* rewind to the start of the file */ + lseek(fd, 0, SEEK_SET); + + /* write data */ + ret = write(fd, buf1, sizeof(buf1)); + + if (ret != sizeof(buf1)) { + printf("\tonboard eeprom: ERROR: write fail: %d\n", (char)ret); + + switch (-ret) { + case EPERM: + printf("\treason: %s\n", EPERM_STR); + break; + + case ENOENT: + printf("\treason: %s\n", ENOENT_STR); + break; + + case ESRCH: + printf("\treason: %s\n", ESRCH_STR); + break; + + case EINTR: + printf("\treason: %s\n", EINTR_STR); + break; + + } + + //return ERROR; + } + + /* rewind to the start of the file */ + lseek(fd, 0, SEEK_SET); + + /* read data */ + ret = read(fd, buf1, sizeof(buf1)); + + if (ret != sizeof(buf1)) { + printf("\tonboard eeprom: ERROR: read fail: %d\n", ret); + + switch (-ret) { + case EPERM: + printf("\treason: %s\n", EPERM_STR); + break; + + case ENOENT: + printf("\treason: %s\n", ENOENT_STR); + break; + + case ESRCH: + printf("\treason: %s\n", ESRCH_STR); + break; + + case EINTR: + printf("\treason: %s\n", EINTR_STR); + break; + + } + + return ERROR; + + } else { + /* enforce null termination and print as string */ + if (buf1[sizeof(buf1) - 1] != 0) { + printf("\tWARNING: Null termination in file not present as expected, enforcing it now..\r\n"); + buf1[sizeof(buf1) - 1] = '\0'; + } + + /* read out counter and replace val */ + int counter = buf1[0]; + printf("\tonboard eeprom: count: #%d, read values: %s\n", counter, (char *)buf1 + 1); + printf("\tAll %d bytes:\n\n\t", sizeof(buf1)); + + for (int i = 0; i < sizeof(buf1); i++) { + printf("0x%02x ", buf1[i]); + + if (i % 8 == 7) printf("\n\t"); + + if (i % 64 == 63) printf("\n\t"); + } + + /* end any open line */ + printf("\n\n"); + } + + close(fd); + + /* Let user know everything is ok */ + printf("\tOK: onboard eeprom passed all tests successfully\n"); + return ret; +} + +static int +baseboard_eeprom(int argc, char *argv[]) +{ + printf("\tbaseboard eeprom: test start\n"); + fflush(stdout); + + int fd; + uint8_t buf[128] = {'R', 'E', 'A', 'D', ' ', 'F', 'A', 'I', 'L', 'E', 'D', '\0'}; + int ret; + + fd = open("/dev/baseboard_eeprom", O_RDONLY | O_NONBLOCK); + + if (fd < 0) { + printf("\tbaseboard eeprom: open fail\n"); + return ERROR; + } + + /* read data */ + ret = read(fd, buf, sizeof(buf)); + /* set last char to string termination */ + buf[127] = '\0'; + + if (ret != sizeof(buf)) { + printf("\tbaseboard eeprom: ERROR: read fail\n", ret); + return ERROR; + + } else { + printf("\tbaseboard eeprom: string: %s\n", (char *)buf); + } + + close(fd); + + /* XXX more tests here */ + + /* Let user know everything is ok */ + printf("\tOK: baseboard eeprom passed all tests successfully\n"); + return ret; +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: test_eeproms + ****************************************************************************/ + +int test_eeproms(int argc, char *argv[]) +{ + unsigned i; + + printf("Running EEPROMs tests:\n\n"); + fflush(stdout); + + for (i = 0; eeproms[i].name; i++) { + printf(" eeprom: %s\n", eeproms[i].name); + eeproms[i].test(argc, argv); + fflush(stdout); + /* wait 100 ms to make sure buffer is emptied */ + usleep(100000); + } + + return 0; +} diff --git a/apps/px4/tests/test_float.c b/apps/px4/tests/test_float.c new file mode 100644 index 000000000..02e748e70 --- /dev/null +++ b/apps/px4/tests/test_float.c @@ -0,0 +1,282 @@ +/**************************************************************************** + * px4/sensors/test_gpio.c + * + * Copyright (C) 2012 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#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 <arch/board/drv_led.h> + +#include "tests.h" + +#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 + ****************************************************************************/ + +typedef union { + float f; + double d; + uint8_t b[8]; +} test_float_double_t; + +int test_float(int argc, char *argv[]) +{ + int ret = 0; + + printf("\n--- SINGLE PRECISION TESTS ---\n"); + printf("The single precision test involves calls to fabs(),\nif test fails check this function as well.\n\n"); + + float f1 = 1.55f; + + float sinf_zero = sinf(0.0f); + float sinf_one = sinf(1.0f); + float sqrt_two = sqrt(2.0f); + + if (sinf_zero == 0.0f) { + printf("\t success: sinf(0.0f) == 0.0f\n"); + + } else { + printf("\t FAIL: sinf(0.0f) != 0.0f, result: %f\n", sinf_zero); + ret = -4; + } + + fflush(stdout); + + if (fabs((sinf_one - 0.841470956802368164062500000000f)) < FLT_EPSILON) { + printf("\t success: sinf(1.0f) == 0.84147f\n"); + + } else { + printf("\t FAIL: sinf(1.0f) != 0.84147f, result: %f\n", sinf_one); + ret = -1; + } + + fflush(stdout); + + float asinf_one = asinf(1.0f); + + if (fabs((asinf_one - 1.570796251296997070312500000000f)) < FLT_EPSILON * 1.5f) { + printf("\t success: asinf(1.0f) == 1.57079f\n"); + + } else { + printf("\t FAIL: asinf(1.0f) != 1.57079f, result: %f\n", asinf_one); + ret = -1; + } + + fflush(stdout); + + float cosf_one = cosf(1.0f); + + if (fabs((cosf_one - 0.540302336215972900390625000000f)) < FLT_EPSILON) { + printf("\t success: cosf(1.0f) == 0.54030f\n"); + + } else { + printf("\t FAIL: cosf(1.0f) != 0.54030f, result: %f\n", cosf_one); + ret = -1; + } + + fflush(stdout); + + + float acosf_one = acosf(1.0f); + + if (fabs((acosf_one - 0.000000000000000000000000000000f)) < FLT_EPSILON) { + printf("\t success: acosf(1.0f) == 0.0f\n"); + + } else { + printf("\t FAIL: acosf(1.0f) != 0.0f, result: %f\n", acosf_one); + ret = -1; + } + + fflush(stdout); + + + float sinf_zero_one = sinf(0.1f); + + if (fabs(sinf_zero_one - 0.0998334166f) < FLT_EPSILON) { + printf("\t success: sinf(0.1f) == 0.09983f\n"); + + } else { + printf("\t FAIL: sinf(0.1f) != 0.09983f, result: %f\n", sinf_zero_one); + ret = -2; + } + + if (sqrt_two == 1.41421356f) { + printf("\t success: sqrt(2.0f) == 1.41421f\n"); + + } else { + printf("\t FAIL: sqrt(2.0f) != 1.41421f, result: %f\n", sinf_zero_one); + ret = -3; + } + + float atan2f_ones = atan2(1.0f, 1.0f); + + if (fabs(atan2f_ones - 0.785398163397448278999490867136f) < FLT_EPSILON) { + printf("\t success: atan2f(1.0f, 1.0f) == 0.78539f\n"); + + } else { + printf("\t FAIL: atan2f(1.0f, 1.0f) != 0.78539f, result: %f\n", atan2f_ones); + ret = -4; + } + + printf("\t testing printing: printf(0.553415f): %f\n", 0.553415f); + + + + + + printf("\n--- DOUBLE PRECISION TESTS ---\n"); + + double d1 = 1.0111; + double d2 = 2.0; + + double d1d2 = d1 * d2; + + if (d1d2 == 2.022200000000000219557705349871) { + printf("\t success: 1.0111 * 2.0 == 2.0222\n"); + + } else { + printf("\t FAIL: 1.0111 * 2.0 != 2.0222, result: %f\n", d1d2); + } + + fflush(stdout); + + // Assign value of f1 to d1 + d1 = f1; + + if (f1 == (float)d1) { + printf("\t success: (float) 1.55f == 1.55 (double)\n"); + + } else { + printf("\t FAIL: (float) 1.55f != 1.55 (double), result: %f\n", f1); + ret = -4; + } + + fflush(stdout); + + + double sin_zero = sin(0.0); + double sin_one = sin(1.0); + double atan2_ones = atan2(1.0, 1.0); + + if (sin_zero == 0.0) { + printf("\t success: sin(0.0) == 0.0\n"); + + } else { + printf("\t FAIL: sin(0.0) != 0.0, result: %f\n", sin_zero); + ret = -5; + } + + if (sin_one == 0.841470984807896504875657228695) { + printf("\t success: sin(1.0) == 0.84147098480\n"); + + } else { + printf("\t FAIL: sin(1.0) != 1.0, result: %f\n", sin_one); + ret = -6; + } + + if (atan2_ones != 0.785398) { + printf("\t success: atan2(1.0, 1.0) == 0.785398\n"); + + } else { + printf("\t FAIL: atan2(1.0, 1.0) != 0.785398, result: %f\n", atan2_ones); + ret = -7; + } + + printf("\t testing printing: printf(0.553415): %f\n", 0.553415); + + printf("\t testing pow() with magic value\n"); + printf("\t (44330.0 * (1.0 - pow((96286LL / 101325.0), 0.190295)));\n"); + fflush(stdout); + usleep(20000); + double powres = (44330.0 * (1.0 - pow((96286LL / 101325.0), 0.190295))); + printf("\t success: result: %f\n", (float)powres); + + + if (ret == 0) { + printf("\n SUCCESS: All float and double tests passed.\n"); + + } else { + printf("\n FAIL: One or more tests failed.\n"); + } + + printf("\n"); + + return ret; +} diff --git a/apps/px4/tests/test_gpio.c b/apps/px4/tests/test_gpio.c new file mode 100644 index 000000000..ab536d956 --- /dev/null +++ b/apps/px4/tests/test_gpio.c @@ -0,0 +1,115 @@ +/**************************************************************************** + * px4/sensors/test_gpio.c + * + * Copyright (C) 2012 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#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 <nuttx/analog/adc.h> + +#include "tests.h" + +#include <drivers/drv_gpio.h> + + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: test_gpio + ****************************************************************************/ + +int test_gpio(int argc, char *argv[]) +{ + int fd; + int ret = 0; + + fd = open(GPIO_DEVICE_PATH, 0); + + if (fd < 0) { + printf("GPIO: open fail\n"); + return ERROR; + } + + /* set all GPIOs to default state */ + ioctl(fd, GPIO_RESET, ~0); + + + /* XXX need to add some GPIO waving stuff here */ + + + /* Go back to default */ + ioctl(fd, GPIO_RESET, ~0); + + printf("\t GPIO test successful.\n"); + + return ret; +} diff --git a/apps/px4/tests/test_hrt.c b/apps/px4/tests/test_hrt.c new file mode 100644 index 000000000..41f207b7e --- /dev/null +++ b/apps/px4/tests/test_hrt.c @@ -0,0 +1,219 @@ +/**************************************************************************** + * px4/sensors/test_hrt.c + * + * Copyright (C) 2012 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#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 <time.h> +#include <unistd.h> + +#include <arch/board/board.h> +#include <arch/board/up_hrt.h> +#include <arch/board/drv_tone_alarm.h> + +#include <nuttx/spi.h> + +#include "tests.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +extern uint16_t ppm_buffer[]; +extern unsigned ppm_decoded_channels; +extern uint16_t ppm_edge_history[]; +extern uint16_t ppm_pulse_history[]; + +int test_ppm(int argc, char *argv[]) +{ +#ifdef CONFIG_HRT_PPM + unsigned i; + + printf("channels: %u\n", ppm_decoded_channels); + + for (i = 0; i < ppm_decoded_channels; i++) + printf(" %u\n", ppm_buffer[i]); + + printf("edges\n"); + + for (i = 0; i < 32; i++) + printf(" %u\n", ppm_edge_history[i]); + + printf("pulses\n"); + + for (i = 0; i < 32; i++) + printf(" %u\n", ppm_pulse_history[i]); + + fflush(stdout); +#else + printf("PPM not configured\n"); +#endif + return 0; +} + +int test_tone(int argc, char *argv[]) +{ +#ifdef CONFIG_TONE_ALARM + int fd, result; + unsigned long tone; + + fd = open("/dev/tone_alarm", O_WRONLY); + + if (fd < 0) { + printf("failed opening /dev/tone_alarm\n"); + goto out; + } + + tone = 1; + + if (argc == 2) + tone = atoi(argv[1]); + + if (tone == 0) { + result = ioctl(fd, TONE_SET_ALARM, 0); + + if (result < 0) { + printf("failed clearing alarms\n"); + goto out; + + } else { + printf("Alarm stopped.\n"); + } + + } else { + result = ioctl(fd, TONE_SET_ALARM, 0); + + if (result < 0) { + printf("failed clearing alarms\n"); + goto out; + } + + result = ioctl(fd, TONE_SET_ALARM, tone); + + if (result < 0) { + printf("failed setting alarm %lu\n", tone); + + } else { + printf("Alarm %lu (disable with: tests tone 0)\n", tone); + } + } + +out: + + if (fd >= 0) + close(fd); + +#endif + return 0; +} + +/**************************************************************************** + * Name: test_hrt + ****************************************************************************/ + +int test_hrt(int argc, char *argv[]) +{ + struct hrt_call call; + hrt_abstime prev, now; + int i; + struct timeval tv1, tv2; + + printf("start-time (hrt, sec/usec), end-time (hrt, sec/usec), microseconds per half second\n"); + + for (i = 0; i < 10; i++) { + prev = hrt_absolute_time(); + gettimeofday(&tv1, NULL); + usleep(500000); + now = hrt_absolute_time(); + gettimeofday(&tv2, NULL); + printf("%lu (%lu/%lu), %lu (%lu/%lu), %lu\n", + (unsigned long)prev, (unsigned long)tv1.tv_sec, (unsigned long)tv1.tv_usec, + (unsigned long)now, (unsigned long)tv2.tv_sec, (unsigned long)tv2.tv_usec, + (unsigned long)(hrt_absolute_time() - prev)); + fflush(stdout); + } + + usleep(1000000); + + printf("one-second ticks\n"); + + for (i = 0; i < 10; i++) { + hrt_call_after(&call, 1000000, NULL, NULL); + + while (!hrt_called(&call)) { + usleep(1000); + } + + printf("tick\n"); + fflush(stdout); + } + + return 0; +} diff --git a/apps/px4/tests/test_int.c b/apps/px4/tests/test_int.c new file mode 100644 index 000000000..40e030641 --- /dev/null +++ b/apps/px4/tests/test_int.c @@ -0,0 +1,151 @@ +/**************************************************************************** + * px4/sensors/test_gpio.c + * + * Copyright (C) 2012 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#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 <arch/board/drv_led.h> + +#include "tests.h" + +#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 + ****************************************************************************/ + +typedef union { + int32_t i; + int64_t l; + uint8_t b[8]; +} test_32_64_t; + +int test_int(int argc, char *argv[]) +{ + int ret = 0; + + printf("\n--- 64 BIT MATH TESTS ---\n"); + + int64_t large = 354156329598; + + int64_t calc = large * 5; + + if (calc == 1770781647990) { + printf("\t success: 354156329598 * 5 == %lld\n", calc); + + } else { + printf("\t FAIL: 354156329598 * 5 != %lld\n", calc); + ret = -1; + } + + fflush(stdout); + + + + + + printf("\n--- 32 BIT / 64 BIT MIXED MATH TESTS ---\n"); + + + int32_t small = 50; + int32_t large_int = 2147483647; // MAX INT value + + uint64_t small_times_large = large_int * (uint64_t)small; + + if (small_times_large == 107374182350) { + printf("\t success: 64bit calculation: 50 * 2147483647 (max int val) == %lld\n", small_times_large); + + } else { + printf("\t FAIL: 50 * 2147483647 != %lld, 64bit cast might fail\n", small_times_large); + ret = -1; + } + + fflush(stdout); + + if (ret == 0) { + printf("\n SUCCESS: All float and double tests passed.\n"); + + } else { + printf("\n FAIL: One or more tests failed.\n"); + } + + printf("\n"); + + return ret; +} diff --git a/apps/px4/tests/test_jig_voltages.c b/apps/px4/tests/test_jig_voltages.c new file mode 100644 index 000000000..51f9b9a5b --- /dev/null +++ b/apps/px4/tests/test_jig_voltages.c @@ -0,0 +1,217 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> +#include <nuttx/arch.h> + +#include <sys/types.h> + +#include <stdio.h> +#include <stdlib.h> +#include <unistd.h> +#include <fcntl.h> +#include <errno.h> +#include <debug.h> + +#include <nuttx/spi.h> + +#include "tests.h" + +#include <nuttx/analog/adc.h> + + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: test_gpio + ****************************************************************************/ + +int test_jig_voltages(int argc, char *argv[]) +{ + int fd0 = 0; + int ret = OK; + const int nchannels = 4; + + struct adc_msg4_s + { + uint8_t am_channel1; /* The 8-bit ADC Channel */ + int32_t am_data1; /* ADC convert result (4 bytes) */ + uint8_t am_channel2; /* The 8-bit ADC Channel */ + int32_t am_data2; /* ADC convert result (4 bytes) */ + uint8_t am_channel3; /* The 8-bit ADC Channel */ + int32_t am_data3; /* ADC convert result (4 bytes) */ + uint8_t am_channel4; /* The 8-bit ADC Channel */ + int32_t am_data4; /* ADC convert result (4 bytes) */ + }__attribute__((__packed__));; + + struct adc_msg4_s sample1[4]; + + size_t readsize; + ssize_t nbytes; + int i = 0; + int j = 0; + int errval; + + char name[11]; + sprintf(name, "/dev/adc%d", j); + fd0 = open(name, O_RDONLY | O_NONBLOCK); + if (fd0 < 0) + { + printf("ADC: %s open fail\n", name); + return ERROR; + } else { + printf("Opened %s successfully\n", name); + } + + + /* Expected values */ + int16_t expected_min[] = {2700, 2700, 2200, 2000}; + int16_t expected_max[] = {3000, 3000, 2500, 2200}; + char* check_res[nchannels]; + + /* first adc read round */ + readsize = 4 * sizeof(struct adc_msg_s); + + /* Empty all buffers */ + do { + nbytes = read(fd0, sample1, readsize); + } + while (nbytes > 0); + + up_udelay(20000);//microseconds + /* Take measurements */ + nbytes = read(fd0, sample1, readsize); + + /* Handle unexpected return values */ + + if (nbytes <= 0) + { + errval = errno; + if (errval != EINTR) + { + message("read %s failed: %d\n", + name, errval); + errval = 3; + goto errout_with_dev; + } + + message("\tInterrupted read...\n"); + } + else if (nbytes == 0) + { + message("\tNo data read, Ignoring\n"); + } + + /* Print the sample data on successful return */ + + else + { + int nsamples = nbytes / sizeof(struct adc_msg_s); + if (nsamples * sizeof(struct adc_msg_s) != nbytes) + { + message("\tread size=%d is not a multiple of sample size=%d, Ignoring\n", + nbytes, sizeof(struct adc_msg_s)); + } + else + { + /* Check values */ + check_res[0] = (expected_min[0] < sample1[i].am_data1 && expected_max[0] > sample1[i].am_data1) ? "OK" : "FAIL"; + check_res[1] = (expected_min[1] < sample1[i].am_data2 && expected_max[1] > sample1[i].am_data2) ? "OK" : "FAIL"; + check_res[2] = (expected_min[2] < sample1[i].am_data3 && expected_max[2] > sample1[i].am_data3) ? "OK" : "FAIL"; + check_res[3] = (expected_min[3] < sample1[i].am_data4 && expected_max[3] > sample1[i].am_data4) ? "OK" : "FAIL"; + + /* Accumulate result */ + ret += (expected_min[0] > sample1[i].am_data1 || expected_max[0] < sample1[i].am_data1) ? 1 : 0; + // XXX Chan 11 not connected on test setup + //ret += (expected_min[1] > sample1[i].am_data2 || expected_max[1] < sample1[i].am_data2) ? 1 : 0; + ret += (expected_min[2] > sample1[i].am_data3 || expected_max[2] < sample1[i].am_data3) ? 1 : 0; + ret += (expected_min[3] > sample1[i].am_data4 || expected_max[3] < sample1[i].am_data4) ? 1 : 0; + + message("Sample:"); + message("%d: channel: %d value: %d (allowed min: %d, allowed max: %d), result: %s\n", + i, sample1[i].am_channel1, sample1[i].am_data1, expected_min[0], expected_max[0], check_res[0]); + message("Sample:"); + message("%d: channel: %d value: %d (allowed min: %d, allowed max: %d), result: %s\n", + i, sample1[i].am_channel2, sample1[i].am_data2, expected_min[1], expected_max[1], check_res[1]); + message("Sample:"); + message("%d: channel: %d value: %d (allowed min: %d, allowed max: %d), result: %s\n", + i, sample1[i].am_channel3, sample1[i].am_data3, expected_min[2], expected_max[2], check_res[2]); + message("Sample:"); + message("%d: channel: %d value: %d (allowed min: %d, allowed max: %d), result: %s\n", + i, sample1[i].am_channel4, sample1[i].am_data4, expected_min[3], expected_max[3], check_res[3]); + + if (ret != OK) { + printf("\t ADC test FAILED. Some channels where out of allowed range. Check supply voltages.\n"); + goto errout_with_dev; + } + } + } + + printf("\t ADC test successful.\n"); + + errout_with_dev: + if (fd0 != 0) close(fd0); + + return ret; +} diff --git a/apps/px4/tests/test_led.c b/apps/px4/tests/test_led.c new file mode 100644 index 000000000..53615ccd8 --- /dev/null +++ b/apps/px4/tests/test_led.c @@ -0,0 +1,134 @@ +/**************************************************************************** + * px4/sensors/test_gpio.c + * + * Copyright (C) 2012 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#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 <arch/board/drv_led.h> + +#include "tests.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: test_led + ****************************************************************************/ + +int test_led(int argc, char *argv[]) +{ + int fd; + int ret = 0; + + fd = open("/dev/led", O_RDONLY | O_NONBLOCK); + + if (fd < 0) { + printf("\tLED: open fail\n"); + return ERROR; + } + + if (ioctl(fd, LED_ON, LED_BLUE) || + ioctl(fd, LED_ON, LED_AMBER)) { + + printf("\tLED: ioctl fail\n"); + return ERROR; + } + + /* let them blink for fun */ + + int i; + uint8_t ledon = 1; + + for (i = 0; i < 10; i++) { + if (ledon) { + ioctl(fd, LED_ON, LED_BLUE); + ioctl(fd, LED_OFF, LED_AMBER); + + } else { + ioctl(fd, LED_OFF, LED_BLUE); + ioctl(fd, LED_ON, LED_AMBER); + } + + ledon = !ledon; + usleep(60000); + } + + /* Go back to default */ + ioctl(fd, LED_ON, LED_BLUE); + ioctl(fd, LED_OFF, LED_AMBER); + + printf("\t LED test completed, no errors.\n"); + + return ret; +} diff --git a/apps/px4/tests/test_sensors.c b/apps/px4/tests/test_sensors.c new file mode 100644 index 000000000..bdb68f88b --- /dev/null +++ b/apps/px4/tests/test_sensors.c @@ -0,0 +1,501 @@ +/**************************************************************************** + * px4/sensors/test_sensors.c + * + * Copyright (C) 2012 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#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 <nuttx/spi.h> + +#include "tests.h" + +#include <arch/board/drv_lis331.h> +#include <arch/board/drv_bma180.h> +#include <arch/board/drv_l3gd20.h> +#include <arch/board/drv_hmc5883l.h> +#include <arch/board/drv_mpu6000.h> + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +//static int lis331(int argc, char *argv[]); +static int l3gd20(int argc, char *argv[]); +static int bma180(int argc, char *argv[]); +static int hmc5883l(int argc, char *argv[]); +static int ms5611(int argc, char *argv[]); +static int mpu6000(int argc, char *argv[]); + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +struct { + const char *name; + const char *path; + int (* test)(int argc, char *argv[]); +} sensors[] = { + {"l3gd20", "/dev/l3gd20", l3gd20}, + {"bma180", "/dev/bma180", bma180}, + {"hmc5883l", "/dev/hmc5883l", hmc5883l}, + {"ms5611", "/dev/ms5611", ms5611}, + {"mpu6000", "/dev/mpu6000", mpu6000}, +// {"lis331", "/dev/lis331", lis331}, + {NULL, NULL, NULL} +}; + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +//static int +//lis331(int argc, char *argv[]) +//{ +// int fd; +// int16_t buf[3]; +// int ret; +// +// fd = open("/dev/lis331", O_RDONLY); +// if (fd < 0) { +// printf("\tlis331: not present on PX4FMU v1.5 and later\n"); +// return ERROR; +// } +// +// if (ioctl(fd, LIS331_SETRATE, LIS331_RATE_50Hz) || +// ioctl(fd, LIS331_SETRANGE, LIS331_RANGE_4G)) { +// +// printf("LIS331: ioctl fail\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)); +// if (ret != sizeof(buf)) { +// printf("LIS331: read1 fail (%d)\n", ret); +// return ERROR; +// } +// +// /* read data - expect no samples (should not be ready again yet) */ +// ret = read(fd, buf, sizeof(buf)); +// if (ret != 0) { +// printf("LIS331: read2 fail (%d)\n", ret); +// return ERROR; +// } +// +// /* XXX more tests here */ +// +// return 0; +//} + +static int +l3gd20(int argc, char *argv[]) +{ + printf("\tL3GD20: test start\n"); + fflush(stdout); + + int fd; + int16_t buf[3] = {0, 0, 0}; + int ret; + + fd = open("/dev/l3gd20", O_RDONLY | O_NONBLOCK); + + if (fd < 0) { + printf("L3GD20: open fail\n"); + return ERROR; + } + +// if (ioctl(fd, L3GD20_SETRATE, L3GD20_RATE_760HZ_LP_50HZ) || +// ioctl(fd, L3GD20_SETRANGE, L3GD20_RANGE_500DPS)) { +// +// printf("L3GD20: ioctl fail\n"); +// return ERROR; +// } else { +// printf("\tconfigured..\n"); +// } +// +// /* wait at least 100ms, sensor should have data after no more than 2ms */ +// usleep(100000); + + + + /* read data - expect samples */ + ret = read(fd, buf, sizeof(buf)); + + if (ret != sizeof(buf)) { + printf("\tL3GD20: read1 fail (%d should have been %d)\n", ret, sizeof(buf)); + //return ERROR; + + } else { + printf("\tL3GD20 values #1: x:%d\ty:%d\tz:%d\n", buf[0], buf[1], buf[2]); + } + + /* wait at least 2 ms, sensor should have data after no more than 1.5ms */ + usleep(2000); + + /* read data - expect no samples (should not be ready again yet) */ + ret = read(fd, buf, sizeof(buf)); + + if (ret != sizeof(buf)) { + printf("\tL3GD20: read2 fail (%d)\n", ret); + close(fd); + return ERROR; + + } else { + printf("\tL3GD20 values #2: x:%d\ty:%d\tz:%d\n", buf[0], buf[1], buf[2]); + } + + /* empty sensor buffer */ + ret = 0; + + while (ret != sizeof(buf)) { + // Keep reading until successful + ret = read(fd, buf, sizeof(buf)); + } + + /* test if FIFO is operational */ + usleep(14800); // Expecting 10 measurements + + ret = 0; + int count = 0; + bool dataready = true; + + while (dataready) { + // Keep reading until successful + ret = read(fd, buf, sizeof(buf)); + + if (ret != sizeof(buf)) { + dataready = false; + + } else { + count++; + } + } + + printf("\tL3GD20: Drained FIFO with %d values (expected 8-12)\n", count); + + /* read data - expect no samples (should not be ready again yet) */ + ret = read(fd, buf, sizeof(buf)); + + if (ret != 0) { + printf("\tL3GD20: Note: read3 got data - there should not have been data ready\n", ret); +// return ERROR; + } + + close(fd); + + /* Let user know everything is ok */ + printf("\tOK: L3GD20 passed all tests successfully\n"); + return OK; +} + +static int +bma180(int argc, char *argv[]) +{ + printf("\tBMA180: test start\n"); + fflush(stdout); + + int fd; + int16_t buf[3] = {0, 0, 0}; + int ret; + + fd = open("/dev/bma180", O_RDONLY); + + if (fd < 0) { + printf("\tBMA180: open fail\n"); + return ERROR; + } + +// if (ioctl(fd, LIS331_SETRATE, LIS331_RATE_50Hz) || +// ioctl(fd, LIS331_SETRANGE, LIS331_RANGE_4G)) { +// +// printf("BMA180: ioctl fail\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)); + + if (ret != sizeof(buf)) { + printf("\tBMA180: read1 fail (%d)\n", ret); + close(fd); + return ERROR; + + } else { + printf("\tBMA180 values: x:%d\ty:%d\tz:%d\n", buf[0], buf[1], buf[2]); + } + + /* wait at least 10ms, sensor should have data after no more than 2ms */ + usleep(100000); + + ret = read(fd, buf, sizeof(buf)); + + if (ret != sizeof(buf)) { + printf("\tBMA180: read2 fail (%d)\n", ret); + close(fd); + return ERROR; + + } else { + printf("\tBMA180: x:%d\ty:%d\tz:%d\n", buf[0], buf[1], buf[2]); + } + + /* empty sensor buffer */ + ret = 0; + + while (ret != sizeof(buf)) { + // Keep reading until successful + ret = read(fd, buf, sizeof(buf)); + } + + ret = read(fd, buf, sizeof(buf)); + + if (ret != 0) { + printf("\tBMA180: Note: read3 got data - there should not have been data ready\n", ret); + } + + /* Let user know everything is ok */ + printf("\tOK: BMA180 passed all tests successfully\n"); + close(fd); + + return OK; +} + +static int +mpu6000(int argc, char *argv[]) +{ + printf("\tMPU-6000: test start\n"); + fflush(stdout); + + int fd; + int16_t buf[5] = { -1, 0, -1, 0, -1, 0}; + int ret; + + fd = open("/dev/mpu6000", O_RDONLY); + + if (fd < 0) { + printf("\tMPU-6000: open fail\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)); + + // if (ret != sizeof(buf)) { + // printf("\tMPU-6000: read1 fail (%d)\n", ret); + // return ERROR; + + // } 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]); + // } + + // /* wait at least 10ms, sensor should have data after no more than 2ms */ + // usleep(100000); + + // ret = read(fd, buf, sizeof(buf)); + + // if (ret != sizeof(buf)) { + // printf("\tMPU-6000: read2 fail (%d)\n", ret); + // return ERROR; + + // } 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]); + // } + + /* XXX more tests here */ + + /* Let user know everything is ok */ + printf("\tOK: MPU-6000 passed all tests successfully\n"); + + return OK; +} + +static int +ms5611(int argc, char *argv[]) +{ + printf("\tMS5611: test start\n"); + fflush(stdout); + + int fd; + float buf[3] = {0.0f, 0.0f, 0.0f}; + int ret; + + fd = open("/dev/ms5611", O_RDONLY); + + if (fd < 0) { + printf("\tMS5611: open fail\n"); + return ERROR; + } + + for (int i = 0; i < 5; i++) { + /* read data - expect samples */ + ret = read(fd, buf, sizeof(buf)); + + if (ret != sizeof(buf)) { + if ((uint8_t)ret == -EAGAIN || (uint8_t)ret == -EINPROGRESS || i < 3) { + /* waiting for device to become ready, this is not an error */ + } else { + printf("\tMS5611: read fail (%d)\n", ret); + close(fd); + return ERROR; + } + + } else { + + /* hack for float printing */ + int32_t pressure_int = buf[0]; + int32_t altitude_int = buf[1]; + int32_t temperature_int = buf[2]; + + printf("\tMS5611: pressure:%d.%03d mbar - altitude: %d.%02d meters - temp:%d.%02d deg celcius\n", pressure_int, (int)(buf[0] * 1000 - pressure_int * 1000), altitude_int, (int)(buf[1] * 100 - altitude_int * 100), temperature_int, (int)(buf[2] * 100 - temperature_int * 100)); + } + + /* wait at least 10ms, sensor should have data after no more than 6.5ms */ + usleep(10000); + } + + close(fd); + + /* Let user know everything is ok */ + printf("\tOK: MS5611 passed all tests successfully\n"); + + return OK; +} + +static int +hmc5883l(int argc, char *argv[]) +{ + printf("\tHMC5883L: test start\n"); + fflush(stdout); + + int fd; + int16_t buf[3] = {0, 0, 0}; + int ret; + + fd = open("/dev/hmc5883l", O_RDONLY); + + if (fd < 0) { + printf("\tHMC5883L: open fail\n"); + return ERROR; + } + + int i; + + for (i = 0; i < 5; i++) { + /* wait at least 7ms, sensor should have data after no more than 6.5ms */ + usleep(7000); + + /* read data - expect samples */ + ret = read(fd, buf, sizeof(buf)); + + if (ret != sizeof(buf)) { + printf("\tHMC5883L: read1 fail (%d) values: x:%d\ty:%d\tz:%d\n", ret, buf[0], buf[1], buf[2]); + close(fd); + return ERROR; + + } else { + printf("\tHMC5883L: x:%d\ty:%d\tz:%d\n", buf[0], buf[1], buf[2]); + } + } + + close(fd); + + /* Let user know everything is ok */ + printf("\tOK: HMC5883L passed all tests successfully\n"); + + return OK; +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: test_sensors + ****************************************************************************/ + +int test_sensors(int argc, char *argv[]) +{ + unsigned i; + + printf("Running sensors tests:\n\n"); + fflush(stdout); + + int ret = OK; + + for (i = 0; sensors[i].name; i++) { + printf(" sensor: %s\n", sensors[i].name); + + /* Flush and leave enough time for the flush to become effective */ + fflush(stdout); + usleep(50000); + /* Test the sensor - if the tests crash at this point, the right sensor name has been printed */ + + ret += sensors[i].test(argc, argv); + } + + return ret; +} diff --git a/apps/px4/tests/test_servo.c b/apps/px4/tests/test_servo.c new file mode 100644 index 000000000..f95760ca8 --- /dev/null +++ b/apps/px4/tests/test_servo.c @@ -0,0 +1,132 @@ +/**************************************************************************** + * px4/sensors/test_hrt.c + * + * Copyright (C) 2012 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#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 <time.h> + +#include <arch/board/board.h> +#include <drivers/drv_pwm_output.h> + +#include <nuttx/spi.h> + +#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; + servo_position_t data[PWM_OUTPUT_MAX_CHANNELS]; + servo_position_t pos; + + fd = open(PWM_OUTPUT_DEVICE_PATH, O_RDWR); + + if (fd < 0) { + printf("failed opening /dev/pwm_servo\n"); + goto out; + } + + result = read(fd, &data, sizeof(data)); + + if (result != sizeof(data)) { + printf("failed bulk-reading channel values\n"); + goto out; + } + + printf("Servo readback, pairs of values should match defaults\n"); + + for (unsigned i = 0; i < PWM_OUTPUT_MAX_CHANNELS; i++) { + result = ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&pos); + + if (result < 0) { + printf("failed reading channel %u\n", i); + goto out; + } + + printf("%u: %u %u\n", i, pos, data[i]); + + } + + printf("Servos arming at default values\n"); + result = ioctl(fd, PWM_SERVO_ARM, 0); + usleep(5000000); + printf("Advancing channel 0 to 1500\n"); + result = ioctl(fd, PWM_SERVO_SET(0), 1500); +out: + return 0; +} diff --git a/apps/px4/tests/test_sleep.c b/apps/px4/tests/test_sleep.c new file mode 100644 index 000000000..911a9c2e1 --- /dev/null +++ b/apps/px4/tests/test_sleep.c @@ -0,0 +1,103 @@ +/**************************************************************************** + * px4/sensors/test_gpio.c + * + * Copyright (C) 2012 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#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 <arch/board/drv_led.h> + +#include "tests.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: test_sleep + ****************************************************************************/ + +int test_sleep(int argc, char *argv[]) +{ + unsigned int nsleeps = 100; + printf("\t %d 100ms sleeps\n", nsleeps); + fflush(stdout); + + for (int i = 0; i < nsleeps; i++) { + usleep(100000); + //printf("\ttick\n"); + } + + printf("\t Sleep test successful.\n"); + + return OK; +} diff --git a/apps/px4/tests/test_time.c b/apps/px4/tests/test_time.c new file mode 100644 index 000000000..c128c73a3 --- /dev/null +++ b/apps/px4/tests/test_time.c @@ -0,0 +1,160 @@ +/**************************************************************************** + * px4/tests/test_time.c + * + * Copyright (C) 2012 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#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 "tests.h" + +#include <math.h> +#include <float.h> +#include <arch/board/up_hrt.h> + + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/* emulate hrt_absolute_time using the cycle counter */ +static hrt_abstime +cycletime(void) +{ + static uint64_t basetime; + static uint32_t lasttime; + uint32_t cycles; + + cycles = *(unsigned long *)0xe0001004; + + if (cycles < lasttime) + basetime += 0x100000000ULL; + + lasttime = cycles; + + return (basetime + cycles) / 168; +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: test_led + ****************************************************************************/ + +int test_time(int argc, char *argv[]) +{ + hrt_abstime h, c; + int64_t lowdelta, maxdelta = 0; + int64_t delta, deltadelta; + + /* enable the cycle counter */ + (*(unsigned long *)0xe000edfc) |= (1 << 24); /* DEMCR |= DEMCR_TRCENA */ + (*(unsigned long *)0xe0001000) |= 1; /* DWT_CTRL |= DWT_CYCCNT_ENA */ + + /* get an average delta between the two clocks - this should stay roughly the same */ + delta = 0; + + for (unsigned i = 0; i < 100; i++) { + uint32_t flags = irqsave(); + + h = hrt_absolute_time(); + c = cycletime(); + + irqrestore(flags); + + delta += h - c; + } + + lowdelta = abs(delta / 100); + + /* loop checking the time */ + for (unsigned i = 0; i < 100000; i++) { + + usleep(rand() * 10); + + uint32_t flags = irqsave(); + + c = cycletime(); + h = hrt_absolute_time(); + + irqrestore(flags); + + delta = abs(h - c); + deltadelta = abs(delta - lowdelta); + + if (deltadelta > maxdelta) + maxdelta = deltadelta; + + if (deltadelta > 1000) + fprintf(stderr, "h %llu c %llu d %lld\n", h, c, delta - lowdelta); + } + + printf("Maximum jitter %lld\n", maxdelta); + + return 0; +} diff --git a/apps/px4/tests/test_uart_baudchange.c b/apps/px4/tests/test_uart_baudchange.c new file mode 100644 index 000000000..06965bd3d --- /dev/null +++ b/apps/px4/tests/test_uart_baudchange.c @@ -0,0 +1,160 @@ +/**************************************************************************** + * px4/sensors/test_gpio.c + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Lorenz Meier <lm@inf.ethz.ch> + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#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 <termios.h> +#include <string.h> + +#include <arch/board/board.h> + +#include <arch/board/drv_led.h> + +#include "tests.h" + +#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_baudchange(int argc, char *argv[]) +{ + int uart2_nwrite = 0; + + /* assuming NuttShell is on UART1 (/dev/ttyS0) */ + int uart2 = open("/dev/ttyS1", O_RDWR | O_NONBLOCK | O_NOCTTY); // + + if (uart2 < 0) { + printf("ERROR opening UART2, aborting..\n"); + return uart2; + } + + struct termios uart2_config; + + struct termios uart2_config_original; + + int termios_state = 0; + +#define UART_BAUDRATE_RUNTIME_CONF +#ifdef UART_BAUDRATE_RUNTIME_CONF + + if ((termios_state = tcgetattr(uart2, &uart2_config)) < 0) { + printf("ERROR getting termios config for UART2: %d\n", termios_state); + exit(termios_state); + } + + memcpy(&uart2_config_original, &uart2_config, sizeof(struct termios)); + + /* Set baud rate */ + if (cfsetispeed(&uart2_config, B9600) < 0 || cfsetospeed(&uart2_config, B9600) < 0) { + printf("ERROR setting termios config for UART2: %d\n", termios_state); + exit(ERROR); + } + + if ((termios_state = tcsetattr(uart2, TCSANOW, &uart2_config)) < 0) { + printf("ERROR setting termios config for UART2\n"); + exit(termios_state); + } + + /* Set back to original settings */ + if ((termios_state = tcsetattr(uart2, TCSANOW, &uart2_config_original)) < 0) { + printf("ERROR setting termios config for UART2\n"); + exit(termios_state); + } + +#endif + + uint8_t sample_uart2[] = {'U', 'A', 'R', 'T', '2', ' ', '#', 0, '\n'}; + + int i, r; + + for (i = 0; i < 100; i++) { + /* uart2 -> */ + r = write(uart2, sample_uart2, sizeof(sample_uart2)); + + if (r > 0) + uart2_nwrite += r; + } + + close(uart2); + + printf("uart2_nwrite %d\n", uart2_nwrite); + + return OK; +} diff --git a/apps/px4/tests/test_uart_console.c b/apps/px4/tests/test_uart_console.c new file mode 100644 index 000000000..de1249b8c --- /dev/null +++ b/apps/px4/tests/test_uart_console.c @@ -0,0 +1,177 @@ +/**************************************************************************** + * px4/sensors/test_uart_console.c + * + * Copyright (C) 2012 Lorenz Meier. All rights reserved. + * Authors: Lorenz Meier <lm@inf.ethz.ch> + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#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 <arch/board/drv_led.h> + +#include "tests.h" + +#include <math.h> +#include <float.h> +#include <arch/board/up_hrt.h> + + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: test_led + ****************************************************************************/ + +static void *receive_loop(void *arg) +{ + int uart_usb = open("/dev/ttyACM0", O_RDONLY | O_NOCTTY); + + while (1) { + char c; + read(uart_usb, &c, 1); + printf("%c", c); + fflush(stdout); + } + + return NULL; +} + +int test_uart_console(int argc, char *argv[]) +{ + /* assuming NuttShell is on UART1 (/dev/ttyS0) */ + // + int uart_usb = open("/dev/ttyACM0", O_WRONLY | O_NOCTTY); + + if (uart_usb < 0) { + printf("ERROR opening /dev/ttyACM0. Do you need to run sercon first? Aborting..\n"); + return uart_usb; + } + + uint8_t sample_uart_usb[] = {'S', 'A', 'M', 'P', 'L', 'E', ' ', '\n'}; + + pthread_t receive_thread; + + pthread_create(&receive_thread, NULL, receive_loop, NULL); + + //wait for threads to complete: + pthread_join(receive_thread, NULL); + + for (int i = 0; i < 30; i++) { + write(uart_usb, sample_uart_usb, sizeof(sample_uart_usb)); + printf("."); + fflush(stdout); + sleep(1); + } + +// uint64_t start_time = hrt_absolute_time(); +// +//// while (true) +// for (int i = 0; i < 1000; i++) +// { +// //write(uart_usb, sample_uart_usb, sizeof(sample_uart_usb)); +// int nread = 0; +// char c; +// do { +// nread = read(uart_usb, &c, 1); +// if (nread > 0) +// { +// printf("%c", c); +// } +// } while (nread > 0); +// +// do { +// nread = read(uart_console, &c, 1); +// if (nread > 0) +// { +// if (c == 0x03) +// { +// close(uart_usb); +// close(uart_console); +// exit(OK); +// } +// else +// { +// write(uart_usb, &c, 1); +// } +// } +// } while (nread > 0); +// usleep(10000); +// } +// +// int interval = hrt_absolute_time() - start_time; + + close(uart_usb); +// close(uart_console); + + return 0; +} diff --git a/apps/px4/tests/test_uart_loopback.c b/apps/px4/tests/test_uart_loopback.c new file mode 100644 index 000000000..b2e07df1c --- /dev/null +++ b/apps/px4/tests/test_uart_loopback.c @@ -0,0 +1,197 @@ +/**************************************************************************** + * px4/sensors/test_gpio.c + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Lorenz Meier <lm@inf.ethz.ch> + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#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 <arch/board/drv_led.h> + +#include "tests.h" + +#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[]) +{ + + int uart5_nread = 0; + int uart2_nread = 0; + int uart5_nwrite = 0; + int uart2_nwrite = 0; + + int uart1 = open("/dev/ttyS0", O_RDWR | O_NOCTTY); // + + /* 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); // + + if (uart2 < 0) { + printf("ERROR opening UART2, aborting..\n"); + return uart2; + } + + if (uart5 < 0) { + printf("ERROR opening UART5, aborting..\n"); + exit(uart5); + } + + uint8_t sample_uart1[] = {'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}; + + int i, r; + + for (i = 0; i < 1000; i++) { +// printf("TEST #%d\n",i); + write(uart1, sample_uart1, sizeof(sample_uart1)); + + /* uart2 -> uart5 */ + r = write(uart2, sample_uart2, sizeof(sample_uart2)); + + if (r > 0) + uart2_nwrite += r; + +// printf("TEST #%d\n",i); + write(uart1, sample_uart1, sizeof(sample_uart1)); + + /* uart2 -> uart5 */ + r = write(uart5, sample_uart5, sizeof(sample_uart5)); + + if (r > 0) + uart5_nwrite += r; + +// printf("TEST #%d\n",i); + write(uart1, sample_uart1, sizeof(sample_uart1)); + + /* try to read back values */ + do { + r = read(uart5, sample_uart2, sizeof(sample_uart2)); + + if (r > 0) + uart5_nread += r; + } while (r > 0); + +// printf("TEST #%d\n",i); + write(uart1, sample_uart1, sizeof(sample_uart1)); + + do { + r = read(uart2, sample_uart5, sizeof(sample_uart5)); + + if (r > 0) + uart2_nread += r; + } while (r > 0); + +// printf("TEST #%d\n",i); +// write(uart1, sample_uart1, sizeof(sample_uart5)); + } + + for (i = 0; i < 200000; i++) { + + /* try to read back values */ + r = read(uart5, sample_uart2, sizeof(sample_uart2)); + + if (r > 0) + uart5_nread += r; + + r = read(uart2, sample_uart5, sizeof(sample_uart5)); + + if (r > 0) + uart2_nread += r; + + if ((uart2_nread == uart2_nwrite) && (uart5_nread == uart5_nwrite)) + break; + } + + + close(uart1); + close(uart2); + close(uart5); + + printf("uart2_nwrite %d\n", uart2_nwrite); + printf("uart5_nwrite %d\n", uart5_nwrite); + printf("uart2_nread %d\n", uart2_nread); + printf("uart5_nread %d\n", uart5_nread); + + + return 0; +} diff --git a/apps/px4/tests/test_uart_send.c b/apps/px4/tests/test_uart_send.c new file mode 100644 index 000000000..83d205440 --- /dev/null +++ b/apps/px4/tests/test_uart_send.c @@ -0,0 +1,133 @@ +/**************************************************************************** + * px4/sensors/test_gpio.c + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Lorenz Meier <lm@inf.ethz.ch> + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#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 <arch/board/drv_led.h> + +#include "tests.h" + +#include <math.h> +#include <float.h> +#include <arch/board/up_hrt.h> + + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: test_led + ****************************************************************************/ + +int test_uart_send(int argc, char *argv[]) +{ + /* input handling */ + char *uart_name = "/dev/ttyS3"; + + if (argc > 1) uart_name = argv[1]; + + /* assuming NuttShell is on UART1 (/dev/ttyS0) */ + int test_uart = open(uart_name, O_RDWR | O_NONBLOCK | O_NOCTTY); // + + if (test_uart < 0) { + printf("ERROR opening UART %s, aborting..\n", uart_name); + return test_uart; + + } else { + printf("Writing to UART %s\n", uart_name); + } + + char sample_test_uart[25];// = {'S', 'A', 'M', 'P', 'L', 'E', ' ', '\n'}; + + int i, r, n; + + uint64_t start_time = hrt_absolute_time(); + + for (i = 0; i < 30000; i++) { + n = sprintf(sample_test_uart, "SAMPLE #%d\n", i); + write(test_uart, sample_test_uart, n); + } + + int interval = hrt_absolute_time() - start_time; + + int bytes = i * sizeof(sample_test_uart); + + printf("Wrote %d bytes in %d ms on UART %s\n", bytes, interval / 1000, uart_name); + + close(test_uart); + + return 0; +} diff --git a/apps/px4/tests/tests.h b/apps/px4/tests/tests.h new file mode 100644 index 000000000..aa6dae1e0 --- /dev/null +++ b/apps/px4/tests/tests.h @@ -0,0 +1,99 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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. + * + ****************************************************************************/ + +#ifndef __APPS_PX4_TESTS_H +#define __APPS_PX4_TESTS_H + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +/**************************************************************************** + * Definitions + ****************************************************************************/ + +/* Debug ********************************************************************/ + +#ifdef CONFIG_CPP_HAVE_VARARGS +# ifdef CONFIG_DEBUG +# define message(...) lib_rawprintf(__VA_ARGS__) +# define msgflush() +# else +# define message(...) printf(__VA_ARGS__) +# define msgflush() fflush(stdout) +# endif +#else +# ifdef CONFIG_DEBUG +# define message lib_rawprintf +# define msgflush() +# else +# define message printf +# define msgflush() fflush(stdout) +# endif +#endif + +/**************************************************************************** + * Public Types + ****************************************************************************/ + +/**************************************************************************** + * Public Variables + ****************************************************************************/ + +/**************************************************************************** + * Public Function Prototypes + ****************************************************************************/ + +extern int test_sensors(int argc, char *argv[]); +extern int test_gpio(int argc, char *argv[]); +extern int test_hrt(int argc, char *argv[]); +extern int test_tone(int argc, char *argv[]); +extern int test_led(int argc, char *argv[]); +extern int test_adc(int argc, char *argv[]); +extern int test_int(int argc, char *argv[]); +extern int test_float(int argc, char *argv[]); +extern int test_eeproms(int argc, char *argv[]); +extern int test_ppm(int argc, char *argv[]); +extern int test_servo(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[]); +extern int test_uart_send(int argc, char *argv[]); +extern int test_sleep(int argc, char *argv[]); +extern int test_time(int argc, char *argv[]); +extern int test_uart_console(int argc, char *argv[]); +extern int test_jig_voltages(int argc, char *argv[]); + +#endif /* __APPS_PX4_TESTS_H */ diff --git a/apps/px4/tests/tests_main.c b/apps/px4/tests/tests_main.c new file mode 100644 index 000000000..704b5a237 --- /dev/null +++ b/apps/px4/tests/tests_main.c @@ -0,0 +1,339 @@ +/**************************************************************************** + * px4/sensors/tests_main.c + * + * Copyright (C) 2012 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <sys/types.h> + +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <unistd.h> +#include <fcntl.h> +#include <errno.h> +#include <debug.h> + +#include <arch/board/board.h> + +#include <nuttx/spi.h> + +#include <systemlib/perf_counter.h> + +#include "tests.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +static int test_help(int argc, char *argv[]); +static int test_all(int argc, char *argv[]); +static int test_perf(int argc, char *argv[]); +static int test_jig(int argc, char *argv[]); + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +struct { + const char *name; + int (* fn)(int argc, char *argv[]); + unsigned options; + int passed; +#define OPT_NOHELP (1<<0) +#define OPT_NOALLTEST (1<<1) +#define OPT_NOJIGTEST (1<<2) +} tests[] = { + {"led", test_led, 0, 0}, + {"int", test_int, 0, 0}, + {"float", test_float, 0, 0}, + {"sensors", test_sensors, 0, 0}, + {"gpio", test_gpio, OPT_NOJIGTEST | OPT_NOALLTEST, 0}, + {"hrt", test_hrt, OPT_NOJIGTEST | OPT_NOALLTEST, 0}, + {"ppm", test_ppm, OPT_NOJIGTEST | OPT_NOALLTEST, 0}, + {"servo", test_servo, OPT_NOJIGTEST | OPT_NOALLTEST, 0}, + {"adc", test_adc, OPT_NOJIGTEST, 0}, + {"jig_voltages", test_jig_voltages, OPT_NOALLTEST, 0}, + {"eeproms", test_eeproms, 0, 0}, + {"uart_loopback", test_uart_loopback, OPT_NOJIGTEST | OPT_NOALLTEST, 0}, + {"uart_baudchange", test_uart_baudchange, OPT_NOJIGTEST | OPT_NOALLTEST, 0}, + {"uart_send", test_uart_send, OPT_NOJIGTEST | OPT_NOALLTEST, 0}, + {"uart_console", test_uart_console, OPT_NOJIGTEST | OPT_NOALLTEST, 0}, + {"tone", test_tone, 0, 0}, + {"sleep", test_sleep, OPT_NOJIGTEST, 0}, + {"time", test_time, OPT_NOJIGTEST, 0}, + {"perf", test_perf, OPT_NOJIGTEST, 0}, + {"all", test_all, OPT_NOALLTEST | OPT_NOJIGTEST, 0}, + {"jig", test_jig, OPT_NOJIGTEST | OPT_NOALLTEST, 0}, + {"help", test_help, OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST, 0}, + {NULL, NULL, 0, 0} +}; + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +static int +test_help(int argc, char *argv[]) +{ + unsigned i; + + printf("Available tests:\n"); + + for (i = 0; tests[i].name; i++) + printf(" %s\n", tests[i].name); + + return 0; +} + +static int +test_all(int argc, char *argv[]) +{ + unsigned i; + char *args[2] = {"all", NULL}; + unsigned int failcount = 0; + + printf("\nRunning all tests...\n\n"); + + for (i = 0; tests[i].name; i++) { + /* Only run tests that are not excluded */ + if (!(tests[i].options & OPT_NOALLTEST)) { + printf(" [%s] \t\t\tSTARTING TEST\n", tests[i].name); + fflush(stdout); + + /* Execute test */ + if (tests[i].fn(1, args) != 0) { + fprintf(stderr, " [%s] \t\t\tFAIL\n", tests[i].name); + fflush(stderr); + failcount++; + + } else { + tests[i].passed = 1; + printf(" [%s] \t\t\tPASS\n", tests[i].name); + fflush(stdout); + } + } + } + + /* Print summary */ + printf("\n"); + int j; + + for (j = 0; j < 40; j++) { + printf("-"); + } + + printf("\n\n"); + + printf(" T E S T S U M M A R Y\n\n"); + + if (failcount == 0) { + printf(" ______ __ __ ______ __ __ \n"); + printf(" /\\ __ \\ /\\ \\ /\\ \\ /\\ __ \\ /\\ \\/ / \n"); + printf(" \\ \\ __ \\ \\ \\ \\____ \\ \\ \\____ \\ \\ \\/\\ \\ \\ \\ _\"-. \n"); + printf(" \\ \\_\\ \\_\\ \\ \\_____\\ \\ \\_____\\ \\ \\_____\\ \\ \\_\\ \\_\\ \n"); + printf(" \\/_/\\/_/ \\/_____/ \\/_____/ \\/_____/ \\/_/\\/_/ \n"); + printf("\n"); + printf(" All tests passed (%d of %d)\n", i, i); + + } else { + printf(" ______ ______ __ __ \n"); + printf(" /\\ ___\\ /\\ __ \\ /\\ \\ /\\ \\ \n"); + printf(" \\ \\ __\\ \\ \\ __ \\ \\ \\ \\ \\ \\ \\__\n"); + printf(" \\ \\_\\ \\ \\_\\ \\_\\ \\ \\_\\ \\ \\_____\\ \n"); + printf(" \\/_/ \\/_/\\/_/ \\/_/ \\/_____/ \n"); + printf("\n"); + printf(" Some tests failed (%d of %d)\n", failcount, i); + } + + printf("\n"); + + /* Print failed tests */ + if (failcount > 0) printf(" Failed tests:\n\n"); + + int k; + + for (k = 0; k < i; k++) { + if ((tests[k].passed == 0) && !(tests[k].options & OPT_NOALLTEST)) { + printf(" [%s] to obtain details, please re-run with\n\t nsh> tests %s\n\n", tests[k].name, tests[k].name); + } + } + + fflush(stdout); + + return 0; +} + +static int +test_perf(int argc, char *argv[]) +{ + perf_counter_t cc, ec; + + cc = perf_alloc(PC_COUNT, "test_count"); + ec = perf_alloc(PC_ELAPSED, "test_elapsed"); + + if ((cc == NULL) || (ec == NULL)) { + printf("perf: counter alloc failed\n"); + return 1; + } + + perf_begin(ec); + perf_count(cc); + perf_count(cc); + perf_count(cc); + perf_count(cc); + printf("perf: expect count of 4\n"); + perf_print_counter(cc); + perf_end(ec); + printf("perf: expect count of 1\n"); + perf_print_counter(ec); + printf("perf: expect at least two counters\n"); + perf_print_all(); + + perf_free(cc); + perf_free(ec); +} + +test_jig(int argc, char *argv[]) +{ + unsigned i; + char *args[2] = {"jig", NULL}; + unsigned int failcount = 0; + + printf("\nRunning all tests...\n\n"); + for (i = 0; tests[i].name; i++) { + /* Only run tests that are not excluded */ + if (!(tests[i].options & OPT_NOJIGTEST)) { + printf(" [%s] \t\t\tSTARTING TEST\n", tests[i].name); + fflush(stdout); + /* Execute test */ + if (tests[i].fn(1, args) != 0) { + fprintf(stderr, " [%s] \t\t\tFAIL\n", tests[i].name); + fflush(stderr); + failcount++; + } else { + tests[i].passed = 1; + printf(" [%s] \t\t\tPASS\n", tests[i].name); + fflush(stdout); + } + } + } + + /* Print summary */ + printf("\n"); + int j; + for (j = 0; j < 40; j++) + { + printf("-"); + } + printf("\n\n"); + + printf(" T E S T S U M M A R Y\n\n"); + if (failcount == 0) { + printf(" ______ __ __ ______ __ __ \n"); + printf(" /\\ __ \\ /\\ \\ /\\ \\ /\\ __ \\ /\\ \\/ / \n"); + printf(" \\ \\ __ \\ \\ \\ \\____ \\ \\ \\____ \\ \\ \\/\\ \\ \\ \\ _\"-. \n"); + printf(" \\ \\_\\ \\_\\ \\ \\_____\\ \\ \\_____\\ \\ \\_____\\ \\ \\_\\ \\_\\ \n"); + printf(" \\/_/\\/_/ \\/_____/ \\/_____/ \\/_____/ \\/_/\\/_/ \n"); + printf("\n"); + printf(" All tests passed (%d of %d)\n", i, i); + } else { + printf(" ______ ______ __ __ \n"); + printf(" /\\ ___\\ /\\ __ \\ /\\ \\ /\\ \\ \n"); + printf(" \\ \\ __\\ \\ \\ __ \\ \\ \\ \\ \\ \\ \\__\n"); + printf(" \\ \\_\\ \\ \\_\\ \\_\\ \\ \\_\\ \\ \\_____\\ \n"); + printf(" \\/_/ \\/_/\\/_/ \\/_/ \\/_____/ \n"); + printf("\n"); + printf(" Some tests failed (%d of %d)\n", failcount, i); + } + printf("\n"); + + /* Print failed tests */ + if (failcount > 0) printf(" Failed tests:\n\n"); + int k; + for (k = 0; k < i; k++) + { + if ((tests[k].passed == 0) && !(tests[k].options & OPT_NOJIGTEST)) + { + printf(" [%s] to obtain details, please re-run with\n\t nsh> tests %s\n\n", tests[k].name, tests[k].name); + } + } + fflush(stdout); + + return 0; +} + + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +__EXPORT int tests_main(int argc, char *argv[]); + +/**************************************************************************** + * Name: tests_main + ****************************************************************************/ + +int tests_main(int argc, char *argv[]) +{ + unsigned i; + + if (argc < 2) { + printf("tests: missing test name - 'tests help' for a list of tests\n"); + return 1; + } + + for (i = 0; tests[i].name; i++) { + if (!strcmp(tests[i].name, argv[1])) + return tests[i].fn(argc - 1, argv + 1); + } + + printf("tests: no test called '%s' - 'tests help' for a list of tests\n", argv[1]); + return ERROR; +} |