Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-21 21:55:34 +08:00
Code Issues Actions 5 Packages Projects Releases Wiki Activity

Added temperature measurement, added led system command

Browse Source
This commit is contained in:
Lorenz Meier
2012-09-01 19:55:48 +02:00
parent 1cebdf6fb2
commit cf62c892f9
5 changed files with 262 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/drivers/drv_accel.h
+7 -4
View File
@@ -52,14 +52,17 @@
*/
struct accel_report {
uint64_t timestamp;
float x;
float y;
float z;
float range_m_s2;
float x; /**< acceleration in the NED X board axis in m/s^2 */
float y; /**< acceleration in the NED Y board axis in m/s^2 */
float z; /**< acceleration in the NED Z board axis in m/s^2 */
float temperature; /**< temperature in degrees celsius */
float range_m_s2; /**< range in m/s^2 (+- this value) */
float scaling;
int16_t x_raw;
int16_t y_raw;
int16_t z_raw;
int16_t temperature_raw;
};
/** accel scaling factors; Vout = (Vin * Vscale) + Voffset */
apps/drivers/drv_gyro.h
+5 -3
View File
@@ -52,15 +52,17 @@
*/
struct gyro_report {
uint64_t timestamp;
float x;
float y;
float z;
float x; /**< angular velocity in the NED X board axis in rad/s */
float y; /**< angular velocity in the NED Y board axis in rad/s */
float z; /**< angular velocity in the NED Z board axis in rad/s */
float temperature; /**< temperature in degrees celcius */
float range_rad_s;
float scaling;
int16_t x_raw;
int16_t y_raw;
int16_t z_raw;
int16_t temperature_raw;
};
/** gyro scaling factors; Vout = (Vin * Vscale) + Voffset */
apps/drivers/mpu6000/mpu6000.cpp
+10
View File
@@ -877,6 +877,9 @@ MPU6000::measure()
_accel_report.scaling = _accel_range_scale;
_accel_report.range_m_s2 = _accel_range_m_s2;
_accel_report.temperature_raw = report.temp;
_accel_report.temperature = (report.temp) / 361.0f + 35.0f;
_gyro_report.x_raw = report.gyro_x;
_gyro_report.y_raw = report.gyro_y;
_gyro_report.z_raw = report.gyro_z;
@@ -887,6 +890,9 @@ MPU6000::measure()
_gyro_report.scaling = _gyro_range_scale;
_gyro_report.range_rad_s = _gyro_range_rad_s;
_gyro_report.temperature_raw = report.temp;
_gyro_report.temperature = (report.temp) / 361.0f + 35.0f;
/* notify anyone waiting for data */
poll_notify(POLLIN);
_gyro->parent_poll_notify();
@@ -1041,6 +1047,10 @@ test()
warnx("gyro range: %8.4f rad/s (%d deg/s)", (double)g_report.range_rad_s,
(int)((g_report.range_rad_s / M_PI_F) * 180.0f+0.5f));
warnx("temp: \t%8.4f\tdeg celsius", (double)a_report.temperature);
warnx("temp: \t%d\traw 0x%0x", (short)a_report.temperature_raw, (unsigned short)a_report.temperature_raw);
/* XXX add poll-rate tests here too */
reset();
apps/systemcmds/led/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.
#
############################################################################
#
# Makefile to build ardrone interface
#
APPNAME = led
PRIORITY = SCHED_PRIORITY_MAX - 15
STACKSIZE = 2048
include $(APPDIR)/mk/app.mk
apps/systemcmds/led/led.c
+198
View File
@@ -0,0 +1,198 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @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
* 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 led.c
* Plain, stupid led outputs
*/
#include <nuttx/config.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdbool.h>
#include <unistd.h>
#include <math.h>
#include <fcntl.h>
#include <errno.h>
#include <debug.h>
#include <termios.h>
#include <time.h>
#include <sys/prctl.h>
#include <arch/board/up_hrt.h>
#include <arch/board/drv_led.h>
__EXPORT int led_main(int argc, char *argv[]);
static bool thread_should_exit = false; /**< Deamon exit flag */
static bool thread_running = false; /**< Deamon status flag */
static int led_task; /**< Handle of deamon task / thread */
static int leds;
static int led_init()
{
leds = open("/dev/led", O_RDONLY | O_NONBLOCK);
if (leds < 0) {
errx(1, "[led] LED: open fail\n");
}
if (ioctl(leds, LED_ON, LED_BLUE) || ioctl(leds, LED_ON, LED_AMBER)) {
errx(1, "[led] LED: ioctl fail\n");
}
return 0;
}
static void led_deinit()
{
close(leds);
}
static int led_toggle(int led)
{
static int last_blue = LED_ON;
static int last_amber = LED_ON;
if (led == LED_BLUE) last_blue = (last_blue == LED_ON) ? LED_OFF : LED_ON;
if (led == LED_AMBER) last_amber = (last_amber == LED_ON) ? LED_OFF : LED_ON;
return ioctl(leds, ((led == LED_BLUE) ? last_blue : last_amber), led);
}
static int led_on(int led)
{
return ioctl(leds, LED_ON, led);
}
static int led_off(int led)
{
return ioctl(leds, LED_OFF, led);
}
/**
* Mainloop of led.
*/
int led_thread_main(int argc, char *argv[]);
/**
* Print the correct usage.
*/
static void usage(const char *reason);
static void
usage(const char *reason)
{
if (reason)
fprintf(stderr, "%s\n", reason);
fprintf(stderr, "usage: led {start|stop|status} [-d <UART>]\n\n");
exit(1);
}
/**
* The deamon app only briefly exists to start
* the background job. The stack size assigned in the
* Makefile does only apply to this management task.
*
* The actual stack size should be set in the call
* to task_create().
*/
int led_main(int argc, char *argv[])
{
if (argc < 1)
usage("missing command");
if (!strcmp(argv[1], "start")) {
if (thread_running) {
printf("led already running\n");
/* this is not an error */
exit(0);
}
thread_should_exit = false;
led_task = task_create("led", SCHED_PRIORITY_MAX - 15, 4096, led_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL);
thread_running = true;
exit(0);
}
if (!strcmp(argv[1], "stop")) {
thread_should_exit = true;
exit(0);
}
if (!strcmp(argv[1], "status")) {
if (thread_running) {
printf("\tled is running\n");
} else {
printf("\tled not started\n");
}
exit(0);
}
usage("unrecognized command");
exit(1);
}
int led_thread_main(int argc, char *argv[])
{
/* welcome user */
printf("[led] Control started, taking over motors\n");
/* open leds */
led_init();
unsigned int rate = 200;
while (!thread_should_exit) {
/* swell blue led */
/* 200 Hz base loop */
usleep(1000000 / rate);
}
/* close leds */
led_deinit();
printf("[led] ending now...\n\n");
fflush(stdout);
thread_running = false;
return OK;
}