Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-06 16:49:51 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity

Merge branch 'master' of github.com:PX4/Firmware

Browse Source
This commit is contained in:
Lorenz Meier
2012-11-12 08:38:39 +01:00
parent b48fc5362c 7d8d7a76b9
commit c57be7a7a7
14 changed files with 1213 additions and 258 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
+18 -9
View File
@@ -58,6 +58,7 @@
#include <uORB/topics/debug_key_value.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/parameter_update.h>
#include <drivers/drv_hrt.h>
@@ -204,6 +205,8 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
memset(&raw, 0, sizeof(raw));
struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att));
struct vehicle_status_s state;
memset(&state, 0, sizeof(state));
uint64_t last_data = 0;
uint64_t last_measurement = 0;
@@ -216,6 +219,9 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
/* subscribe to param changes */
int sub_params = orb_subscribe(ORB_ID(parameter_update));
/* subscribe to system state*/
int sub_state = orb_subscribe(ORB_ID(vehicle_status));
/* advertise attitude */
orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);
@@ -226,13 +232,15 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
thread_running = true;
/* advertise debug value */
struct debug_key_value_s dbg = { .key = "", .value = 0.0f };
orb_advert_t pub_dbg = -1;
// struct debug_key_value_s dbg = { .key = "", .value = 0.0f };
// orb_advert_t pub_dbg = -1;
float sensor_update_hz[3] = {0.0f, 0.0f, 0.0f};
// XXX write this out to perf regs
/* keep track of sensor updates */
uint32_t sensor_last_count[3] = {0, 0, 0};
uint64_t sensor_last_timestamp[3] = {0, 0, 0};
float sensor_update_hz[3] = {0.0f, 0.0f, 0.0f};
struct attitude_estimator_ekf_params ekf_params;
@@ -259,8 +267,12 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
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 ekf] WARNING: Not getting sensor data - sensor app running?\n");
/* check if we're in HIL - not getting sensor data is fine then */
orb_copy(ORB_ID(vehicle_status), sub_state, &state);
if (!state.flag_hil_enabled) {
fprintf(stderr,
"[att ekf] WARNING: Not getting sensors - sensor app running?\n");
}
} else {
/* only update parameters if they changed */
@@ -347,9 +359,6 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
overloadcounter++;
}
int32_t z_k_sizes = 9;
// float u[4] = {0.0f, 0.0f, 0.0f, 0.0f};
static bool const_initialized = false;
/* initialize with good values once we have a reasonable dt estimate */
@@ -392,7 +401,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
continue;
}
uint64_t timing_diff = hrt_absolute_time() - timing_start;
// uint64_t timing_diff = hrt_absolute_time() - timing_start;
// /* print rotation matrix every 200th time */
if (printcounter % 200 == 0) {
apps/drivers/hil/Makefile
+42
View File
@@ -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 PX4FMU board
#
APPNAME = hil
PRIORITY = SCHED_PRIORITY_DEFAULT
STACKSIZE = 2048
include $(APPDIR)/mk/app.mk
apps/drivers/hil/hil.cpp
+853
View File
File diff suppressed because it is too large Load Diff
apps/drivers/px4fmu/fmu.cpp
+3 -2
View File
@@ -224,7 +224,7 @@ PX4FMU::init()
_task = task_spawn("fmuservo",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
1024,
2048,
(main_t)&PX4FMU::task_main_trampoline,
nullptr);
@@ -419,7 +419,8 @@ PX4FMU::ioctl(file *filp, int cmd, unsigned long arg)
{
int ret;
debug("ioctl 0x%04x 0x%08x", cmd, arg);
// XXX disabled, confusing users
//debug("ioctl 0x%04x 0x%08x", cmd, arg);
/* try it as a GPIO ioctl first */
ret = gpio_ioctl(filp, cmd, arg);
apps/mavlink/mavlink.c
+7 -13
View File
@@ -132,6 +132,7 @@ static struct mavlink_logbuffer lb;
static void mavlink_update_system(void);
static int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb);
static void usage(void);
int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_msg_id, int min_interval);
@@ -143,15 +144,10 @@ set_hil_on_off(bool hil_enabled)
/* Enable HIL */
if (hil_enabled && !mavlink_hil_enabled) {
//printf("\n HIL ON \n");
/* Advertise topics */
pub_hil_attitude = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude);
pub_hil_global_pos = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos);
printf("\n pub_hil_attitude :%i\n", pub_hil_attitude);
printf("\n pub_hil_global_pos :%i\n", pub_hil_global_pos);
mavlink_hil_enabled = true;
/* ramp up some HIL-related subscriptions */
@@ -166,15 +162,13 @@ set_hil_on_off(bool hil_enabled)
} else if (baudrate < 115200) {
/* 20 Hz */
hil_rate_interval = 50;
} else if (baudrate < 460800) {
/* 50 Hz */
hil_rate_interval = 20;
} else {
/* 100 Hz */
hil_rate_interval = 10;
}
orb_set_interval(mavlink_subs.spa_sub, hil_rate_interval);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, hil_rate_interval);
}
if (!hil_enabled && mavlink_hil_enabled) {
@@ -268,7 +262,7 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode)
}
static int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_msg_id, int min_interval)
int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_msg_id, int min_interval)
{
int ret = OK;
@@ -458,19 +452,19 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, uint8_t *ch, int length)
/*
* Internal function to give access to the channel status for each channel
*/
mavlink_status_t* mavlink_get_channel_status(uint8_t chan)
mavlink_status_t* mavlink_get_channel_status(uint8_t channel)
{
static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS];
return &m_mavlink_status[chan];
return &m_mavlink_status[channel];
}
/*
* Internal function to give access to the channel buffer for each channel
*/
mavlink_message_t* mavlink_get_channel_buffer(uint8_t chan)
mavlink_message_t* mavlink_get_channel_buffer(uint8_t channel)
{
static mavlink_message_t m_mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS];
return &m_mavlink_buffer[chan];
return &m_mavlink_buffer[channel];
}
void mavlink_update_system(void)
apps/mavlink/mavlink_receiver.c
+3 -74
View File
@@ -1,7 +1,7 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Lorenz Meier <lm@inf.ethz.ch>
* Author: 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
@@ -35,6 +35,8 @@
/**
* @file mavlink_receiver.c
* MAVLink protocol message receive and dispatch
*
* @author Lorenz Meier <lm@inf.ethz.ch>
*/
/* XXX trim includes */
@@ -216,8 +218,6 @@ handle_message(mavlink_message_t *msg)
if (msg->msgid == MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST) {
mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t quad_motors_setpoint;
mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(msg, &quad_motors_setpoint);
//printf("got message\n");
//printf("got MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT target_system=%u, sysid = %u\n", mavlink_system.sysid, quad_motors_setpoint.mode);
if (mavlink_system.sysid < 4) {
@@ -274,61 +274,6 @@ handle_message(mavlink_message_t *msg)
/* Publish */
orb_publish(ORB_ID(offboard_control_setpoint), offboard_control_sp_pub, &offboard_control_sp);
}
// /* change armed status if required */
// bool cmd_armed = (quad_motors_setpoint.mode & MAVLINK_OFFBOARD_CONTROL_FLAG_ARMED);
// bool cmd_generated = false;
// if (v_status.flag_control_offboard_enabled != cmd_armed) {
// vcmd.param1 = cmd_armed;
// vcmd.param2 = 0;
// vcmd.param3 = 0;
// vcmd.param4 = 0;
// vcmd.param5 = 0;
// vcmd.param6 = 0;
// vcmd.param7 = 0;
// vcmd.command = MAV_CMD_COMPONENT_ARM_DISARM;
// vcmd.target_system = mavlink_system.sysid;
// vcmd.target_component = MAV_COMP_ID_ALL;
// vcmd.source_system = msg->sysid;
// vcmd.source_component = msg->compid;
// vcmd.confirmation = 1;
// cmd_generated = true;
// }
// /* check if input has to be enabled */
// if ((v_status.flag_control_rates_enabled != (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_RATES)) ||
// (v_status.flag_control_attitude_enabled != (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_ATTITUDE)) ||
// (v_status.flag_control_velocity_enabled != (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_VELOCITY)) ||
// (v_status.flag_control_position_enabled != (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_POSITION))) {
// vcmd.param1 = (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_RATES);
// vcmd.param2 = (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_ATTITUDE);
// vcmd.param3 = (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_VELOCITY);
// vcmd.param4 = (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_POSITION);
// vcmd.param5 = 0;
// vcmd.param6 = 0;
// vcmd.param7 = 0;
// vcmd.command = PX4_CMD_CONTROLLER_SELECTION;
// vcmd.target_system = mavlink_system.sysid;
// vcmd.target_component = MAV_COMP_ID_ALL;
// vcmd.source_system = msg->sysid;
// vcmd.source_component = msg->compid;
// vcmd.confirmation = 1;
// cmd_generated = true;
// }
// if (cmd_generated) {
// /* check if topic is advertised */
// if (cmd_pub <= 0) {
// cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
// } else {
// /* create command */
// orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd);
// }
// }
}
}
@@ -340,8 +285,6 @@ handle_message(mavlink_message_t *msg)
* COMMAND_LONG message or a SET_MODE message
*/
// printf("\n HIL ENABLED?: %s \n",(mavlink_hil_enabled)?"true":"false");
if (mavlink_hil_enabled) {
if (msg->msgid == MAVLINK_MSG_ID_HIL_STATE) {
@@ -349,20 +292,6 @@ handle_message(mavlink_message_t *msg)
mavlink_hil_state_t hil_state;
mavlink_msg_hil_state_decode(msg, &hil_state);
// printf("\n HILSTATE : \n LAT: %i \n LON: %i \n ALT: %i \n "
// "ROLL %i \n PITCH %i \n YAW %i \n"
// "ROLLSPEED: %i \n PITCHSPEED: %i \n, YAWSPEED: %i \n",
// hil_state.lat/1000000, // 1e7
// hil_state.lon/1000000, // 1e7
// hil_state.alt/1000, // mm
// hil_state.roll, // float rad
// hil_state.pitch, // float rad
// hil_state.yaw, // float rad
// hil_state.rollspeed, // float rad/s
// hil_state.pitchspeed, // float rad/s
// hil_state.yawspeed); // float rad/s
hil_global_pos.lat = hil_state.lat;
hil_global_pos.lon = hil_state.lon;
hil_global_pos.alt = hil_state.alt / 1000.0f;
apps/mavlink/orb_listener.c
+88 -25
View File
@@ -1,7 +1,7 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Lorenz Meier <lm@inf.ethz.ch>
* Author: 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
@@ -35,6 +35,8 @@
/**
* @file orb_listener.c
* Monitors ORB topics and sends update messages as appropriate.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
*/
// XXX trim includes
@@ -419,7 +421,7 @@ l_actuator_outputs(struct listener *l)
/* copy actuator data into local buffer */
orb_copy(ids[l->arg], *l->subp, &act_outputs);
if (gcs_link)
if (gcs_link) {
mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000,
l->arg /* port number */,
act_outputs.output[0],
@@ -430,6 +432,90 @@ l_actuator_outputs(struct listener *l)
act_outputs.output[5],
act_outputs.output[6],
act_outputs.output[7]);
/* only send in HIL mode */
if (mavlink_hil_enabled) {
/* translate the current syste state to mavlink state and mode */
uint8_t mavlink_state = 0;
uint8_t mavlink_mode = 0;
get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode);
/* HIL message as per MAVLink spec */
/* scale / assign outputs depending on system type */
if (mavlink_system.type == MAV_TYPE_QUADROTOR) {
mavlink_msg_hil_controls_send(chan,
hrt_absolute_time(),
((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f,
((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f,
((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f,
((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f,
-1,
-1,
-1,
-1,
mavlink_mode,
0);
} else if (mavlink_system.type == MAV_TYPE_HEXAROTOR) {
mavlink_msg_hil_controls_send(chan,
hrt_absolute_time(),
((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f,
((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f,
((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f,
((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f,
((act_outputs.output[4] - 900.0f) / 600.0f) / 2.0f,
((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f,
-1,
-1,
mavlink_mode,
0);
} else if (mavlink_system.type == MAV_TYPE_OCTOROTOR) {
mavlink_msg_hil_controls_send(chan,
hrt_absolute_time(),
((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f,
((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f,
((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f,
((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f,
((act_outputs.output[4] - 900.0f) / 600.0f) / 2.0f,
((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f,
((act_outputs.output[6] - 900.0f) / 600.0f) / 2.0f,
((act_outputs.output[7] - 900.0f) / 600.0f) / 2.0f,
mavlink_mode,
0);
} else {
float rudder, throttle;
/* SCALING: PWM min: 900, PWM max: 2100, center: 1500 */
// XXX very ugly, needs rework
if (isfinite(act_outputs.output[3])
&& act_outputs.output[3] > 800 && act_outputs.output[3] < 2200) {
/* throttle is fourth output */
rudder = (act_outputs.output[2] - 1500.0f) / 600.0f;
throttle = (((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f);
} else {
/* only three outputs, put throttle on position 4 / index 3 */
rudder = 0;
throttle = (((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f);
}
mavlink_msg_hil_controls_send(chan,
hrt_absolute_time(),
(act_outputs.output[0] - 1500.0f) / 600.0f,
(act_outputs.output[1] - 1500.0f) / 600.0f,
rudder,
throttle,
(act_outputs.output[4] - 1500.0f) / 600.0f,
(act_outputs.output[5] - 1500.0f) / 600.0f,
(act_outputs.output[6] - 1500.0f) / 600.0f,
(act_outputs.output[7] - 1500.0f) / 600.0f,
mavlink_mode,
0);
}
}
}
}
void
@@ -482,29 +568,6 @@ l_vehicle_attitude_controls(struct listener *l)
"ctrl3 ",
actuators.control[3]);
}
/* Only send in HIL mode */
if (mavlink_hil_enabled) {
/* translate the current syste state to mavlink state and mode */
uint8_t mavlink_state = 0;
uint8_t mavlink_mode = 0;
get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode);
/* HIL message as per MAVLink spec */
mavlink_msg_hil_controls_send(chan,
hrt_absolute_time(),
actuators.control[0],
actuators.control[1],
actuators.control[2],
actuators.control[3],
0,
0,
0,
0,
mavlink_mode,
0);
}
}
void
apps/multirotor_pos_control/multirotor_pos_control.c
+4 -2
View File
@@ -32,8 +32,10 @@
*
****************************************************************************/
/*
* @file Implementation of AR.Drone 1.0 / 2.0 control interface
/**
* @file multirotor_pos_control.c
*
* Skeleton for multirotor position controller
*/
#include <nuttx/config.h>
apps/px4/tests/test_adc.c
+53 -116
View File
@@ -1,7 +1,6 @@
/****************************************************************************
* px4/sensors/test_gpio.c
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* 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
@@ -13,7 +12,7 @@
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name NuttX nor the names of its contributors may be
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
@@ -32,9 +31,10 @@
*
****************************************************************************/
/****************************************************************************
* Included Files
****************************************************************************/
/**
* @file test_adc.c
* Test for the analog to digital converter.
*/
#include <nuttx/config.h>
#include <nuttx/arch.h>
@@ -54,91 +54,46 @@
#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 fd0 = 0;
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];
#pragma pack(push,1)
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__));;
uint8_t am_channel1; /**< The 8-bit ADC Channel 1 */
int32_t am_data1; /**< ADC convert result 1 (4 bytes) */
uint8_t am_channel2; /**< The 8-bit ADC Channel 2 */
int32_t am_data2; /**< ADC convert result 2 (4 bytes) */
uint8_t am_channel3; /**< The 8-bit ADC Channel 3 */
int32_t am_data3; /**< ADC convert result 3 (4 bytes) */
uint8_t am_channel4; /**< The 8-bit ADC Channel 4 */
int32_t am_data4; /**< ADC convert result 4 (4 bytes) */
};
#pragma pack(pop)
struct adc_msg4_s sample1[4], sample2[4];
struct adc_msg4_s sample1;
size_t readsize;
ssize_t nbytes, nbytes2;
int i;
ssize_t nbytes;
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);
fd0 = open("/dev/adc0", O_RDONLY | O_NONBLOCK);
if (fd0 < 0) {
printf("ADC: %s open fail\n", name);
return ERROR;
if (fd0 <= 0) {
message("/dev/adc0 open fail: %d\n", errno);
return ERROR;
} else {
printf("Opened %s successfully\n", name);
}
} else {
message("opened /dev/adc0 successfully\n");
}
usleep(10000);
/* 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);
for (j = 0; j < 10; j++) {
/* sleep 20 milliseconds */
usleep(20000);
nbytes = read(fd0, &sample1, sizeof(sample1));
/* Handle unexpected return values */
@@ -146,62 +101,44 @@ int test_adc(int argc, char *argv[])
errval = errno;
if (errval != EINTR) {
message("read %s failed: %d\n",
name, errval);
message("reading /dev/adc0 failed: %d\n", errval);
errval = 3;
goto errout_with_dev;
}
message("\tInterrupted read...\n");
message("\tinterrupted read..\n");
} else if (nbytes == 0) {
message("\tNo data read, Ignoring\n");
message("\tno data read, ignoring.\n");
ret = ERROR;
}
/* 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));
if (nbytes != sizeof(sample1)) {
message("\tsample 1 size %d is not matching struct size %d, ignoring\n",
nbytes, sizeof(sample1));
ret = ERROR;
} 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);
}
message("CYCLE %d:\n", j);
message("channel: %d value: %d\n",
(int)sample1.am_channel1, sample1.am_data1);
message("channel: %d value: %d",
(int)sample1.am_channel2, sample1.am_data2);
message("channel: %d value: %d",
(int)sample1.am_channel3, sample1.am_data3);
message("channel: %d value: %d",
(int)sample1.am_channel4, sample1.am_data4);
}
}
fflush(stdout);
}
printf("\t ADC test successful.\n");
message("\t ADC test successful.");
errout_with_dev:
apps/px4/tests/test_sensors.c
+130 -11
View File
@@ -1,7 +1,7 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Lorenz Meier <lm@inf.ethz.ch>
* Author: 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
@@ -36,7 +36,7 @@
* @file test_sensors.c
* Tests the onboard sensors.
*
* The sensors app must not be running when performing this test.
* @author Lorenz Meier <lm@inf.ethz.ch>
*/
#include <nuttx/config.h>
@@ -56,7 +56,10 @@
#include "tests.h"
#include <drivers/drv_gyro.h>
#include <drivers/drv_accel.h>
#include <drivers/drv_mag.h>
#include <drivers/drv_baro.h>
/****************************************************************************
* Pre-processor Definitions
@@ -70,8 +73,10 @@
* Private Function Prototypes
****************************************************************************/
//static int lis331(int argc, char *argv[]);
static int mpu6000(int argc, char *argv[]);
static int accel(int argc, char *argv[]);
static int gyro(int argc, char *argv[]);
static int mag(int argc, char *argv[]);
static int baro(int argc, char *argv[]);
/****************************************************************************
* Private Data
@@ -82,7 +87,10 @@ struct {
const char *path;
int (* test)(int argc, char *argv[]);
} sensors[] = {
{"mpu6000", "/dev/accel", mpu6000},
{"accel", "/dev/accel", accel},
{"gyro", "/dev/gyro", gyro},
{"mag", "/dev/mag", mag},
{"baro", "/dev/baro", baro},
{NULL, NULL, NULL}
};
@@ -95,9 +103,9 @@ struct {
****************************************************************************/
static int
mpu6000(int argc, char *argv[])
accel(int argc, char *argv[])
{
printf("\tMPU-6000: test start\n");
printf("\tACCEL: test start\n");
fflush(stdout);
int fd;
@@ -107,7 +115,7 @@ mpu6000(int argc, char *argv[])
fd = open("/dev/accel", O_RDONLY);
if (fd < 0) {
printf("\tMPU-6000: open fail, run <mpu6000 start> first.\n");
printf("\tACCEL: open fail, run <mpu6000 start> or <lsm303 start> or <bma180 start> first.\n");
return ERROR;
}
@@ -118,11 +126,11 @@ mpu6000(int argc, char *argv[])
ret = read(fd, &buf, sizeof(buf));
if (ret < 3) {
printf("\tMPU-6000: read1 fail (%d)\n", ret);
printf("\tACCEL: read1 fail (%d)\n", ret);
return ERROR;
} else {
printf("\tMPU-6000 values: acc: x:%8.4f\ty:%8.4f\tz:%8.4f\n", (double)buf.x, (double)buf.y, (double)buf.z);//\tgyro: r:%d\tp:%d\ty:%d\n", buf[0], buf[1], buf[2], buf[3], buf[4], buf[5]);
printf("\tACCEL accel: x:%8.4f\ty:%8.4f\tz:%8.4f m/s^2\n", (double)buf.x, (double)buf.y, (double)buf.z);
}
// /* wait at least 10ms, sensor should have data after no more than 2ms */
@@ -141,7 +149,118 @@ mpu6000(int argc, char *argv[])
/* XXX more tests here */
/* Let user know everything is ok */
printf("\tOK: MPU-6000 passed all tests successfully\n");
printf("\tOK: ACCEL passed all tests successfully\n");
return OK;
}
static int
gyro(int argc, char *argv[])
{
printf("\tGYRO: test start\n");
fflush(stdout);
int fd;
struct gyro_report buf;
int ret;
fd = open("/dev/gyro", O_RDONLY);
if (fd < 0) {
printf("\tGYRO: open fail, run <l3gd20 start> or <mpu6000 start> first.\n");
return ERROR;
}
/* wait at least 5 ms, sensor should have data after that */
usleep(5000);
/* read data - expect samples */
ret = read(fd, &buf, sizeof(buf));
if (ret < 3) {
printf("\tGYRO: read fail (%d)\n", ret);
return ERROR;
} else {
printf("\tGYRO rates: x:%8.4f\ty:%8.4f\tz:%8.4f rad/s\n", (double)buf.x, (double)buf.y, (double)buf.z);
}
/* Let user know everything is ok */
printf("\tOK: GYRO passed all tests successfully\n");
return OK;
}
static int
mag(int argc, char *argv[])
{
printf("\tMAG: test start\n");
fflush(stdout);
int fd;
struct mag_report buf;
int ret;
fd = open("/dev/mag", O_RDONLY);
if (fd < 0) {
printf("\tMAG: open fail, run <hmc5883 start> or <lsm303 start> first.\n");
return ERROR;
}
/* wait at least 5 ms, sensor should have data after that */
usleep(5000);
/* read data - expect samples */
ret = read(fd, &buf, sizeof(buf));
if (ret < 3) {
printf("\tMAG: read fail (%d)\n", ret);
return ERROR;
} else {
printf("\tMAG values: x:%8.4f\ty:%8.4f\tz:%8.4f\n", (double)buf.x, (double)buf.y, (double)buf.z);
}
/* Let user know everything is ok */
printf("\tOK: MAG passed all tests successfully\n");
return OK;
}
static int
baro(int argc, char *argv[])
{
printf("\tBARO: test start\n");
fflush(stdout);
int fd;
struct baro_report buf;
int ret;
fd = open("/dev/baro", O_RDONLY);
if (fd < 0) {
printf("\tBARO: open fail, run <ms5611 start> or <lps331 start> first.\n");
return ERROR;
}
/* wait at least 5 ms, sensor should have data after that */
usleep(5000);
/* read data - expect samples */
ret = read(fd, &buf, sizeof(buf));
if (ret < 3) {
printf("\tBARO: read fail (%d)\n", ret);
return ERROR;
} else {
printf("\tBARO pressure: %8.4f mbar\talt: %8.4f m\ttemp: %8.4f deg C\n", (double)buf.pressure, (double)buf.altitude, (double)buf.temperature);
}
/* Let user know everything is ok */
printf("\tOK: BARO passed all tests successfully\n");
return OK;
}
apps/sensors/sensors.cpp
+2 -2
View File
@@ -34,9 +34,9 @@
/**
* @file sensors.cpp
* @author Lorenz Meier <lm@inf.ethz.ch>
*
* Sensor readout process.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
*/
#include <nuttx/config.h>
apps/systemlib/mixer/mixer_group.cpp
+1
View File
@@ -168,6 +168,7 @@ mixer_load_simple(Mixer::ControlCallback control_cb, uintptr_t cb_handle, int fd
/* let's assume we're going to read a simple mixer */
mixinfo = (mixer_simple_s *)malloc(MIXER_SIMPLE_SIZE(inputs));
mixinfo->control_count = inputs;
/* first, get the output scaler */
ret = mixer_getline(fd, buf, sizeof(buf));
apps/uORB/topics/subsystem_info.h
+8 -4
View File
@@ -1,9 +1,9 @@
/****************************************************************************
*
* Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
* Author: @author Lorenz Meier <lm@inf.ethz.ch>
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: Lorenz Meier <lm@inf.ethz.ch>
* Thomas Gubler <thomasgubler@student.ethz.ch>
* Julian Oes <joes@student.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -37,6 +37,10 @@
/**
* @file subsystem_info.h
* Definition of the subsystem info topic.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
*/
#ifndef TOPIC_SUBSYSTEM_INFO_H_
nuttx/configs/px4fmu/nsh/appconfig
+1
View File
@@ -96,6 +96,7 @@ CONFIGURED_APPS += drivers/stm32
CONFIGURED_APPS += drivers/led
CONFIGURED_APPS += drivers/stm32/tone_alarm
CONFIGURED_APPS += drivers/px4fmu
CONFIGURED_APPS += drivers/hil
# Testing stuff
CONFIGURED_APPS += px4/sensors_bringup