fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-05 04:00:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Thomas Gubler
2012-10-21 15:13:39 +02:00
parent b9510b89bb 73521cbc66
commit 7fcb51703b
125 changed files with 4471 additions and 4560 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.gitignore
+1
View File
@@ -38,3 +38,4 @@ Firmware.sublime-workspace
nsh_romfsimg.h
cscope.out
.configX-e
nuttx-export.zip
ROMFS/scripts/rc.FMU_quad_x
+1 -1
View File
@@ -9,7 +9,7 @@ echo "[init] eeprom"
eeprom start
if [ -f /eeprom/parameters ]
then
eeprom load_param /eeprom/parameters
param load
fi
echo "[init] sensors"
ROMFS/scripts/rc.PX4IOAR
+1 -1
View File
@@ -19,7 +19,7 @@ echo "[init] eeprom"
eeprom start
if [ -f /eeprom/parameters ]
then
eeprom load_param /eeprom/parameters
param load
fi
#
ROMFS/scripts/rc.standalone
+10
View File
@@ -10,6 +10,16 @@ echo "[init] doing standalone PX4FMU startup..."
#
uorb start
#
# Init the EEPROM
#
echo "[init] eeprom"
eeprom start
if [ -f /eeprom/parameters ]
then
param load
fi
#
# Start the sensors.
#
ROMFS/scripts/rcS
+1
View File
@@ -33,6 +33,7 @@ fi
usleep 500
#
# Look for an init script on the microSD card.
#
apps/ChangeLog.txt
+26 -3
View File
@@ -279,14 +279,14 @@
* apps/*/Make.defs: Numerous fixes needed to use the automated
configuration (from Richard Cochran).
6.22 2012-xx-xx Gregory Nutt <gnutt@nuttx.org>
6.22 2012-09-29 Gregory Nutt <gnutt@nuttx.org>
* apps/netutils/thttpd/thttpd_cgi.c: Missing NULL in argv[]
list (contributed by Kate).
* apps/nshlib/nsh_parse.c: CONFIG_NSH_DISABLE_WGET not CONFIG_NSH_DISABLE_GET
in one location (found by Kate).
* apps/examples/ostest/prioinherit.c: Limit the number of test
threds to no more than 3 of each priority. Bad things happen
threads to no more than 3 of each priority. Bad things happen
when the existing logic tried to created several hundred test
treads!
* apps/nshlib/nsh.h: Both CONFIG_LIBC_STRERROR and CONFIG_NSH_STRERROR
@@ -341,7 +341,30 @@
* apps/netutils/webserver/httpd.c: Add support for Keep-alive connections
(from Kate).
* apps/NxWidget/Kconfig: This is a kludge. I created this NxWidgets
directory that ONLY contains Kconfig. NxWidgets does not like in
directory that ONLY contains Kconfig. NxWidgets does not live in
either the nuttx/ or the apps/ source trees. This kludge makes it
possible to configure NxWidgets/NxWM without too much trouble (with
the tradeoff being a kind ugly structure and some maintenance issues).
* apps/examples/Make.defs: Missing support for apps/examples/watchdog.
* apps/NxWidgets/Kconfig: Add option to turn on the memory monitor
feature of the NxWidgets/NxWM unit tests.
6.23 2012-xx-xx Gregory Nutt <gnutt@nuttx.org>
* vsn: Moved all NSH commands from vsn/ to system/. Deleted the vsn/
directory.
* Makefile: Change order of includes when CONFIG_NEWCONFIG=y. In
that case, namedapp must be included first so that the namedapp
context is established first. If the namedapp context is established
later, it will overwrite any existing namedapp_list.h and nameapp_proto.h
files.
* CONFIG_EXAMPLES_*: To make things consistent, changed all occurrences
of CONFIG_EXAMPLE_* to CONFIG_EXAMPLES_*.
* Kconfig: Fleshed out apps/examples/adc/Kconfig and apps/examples/wget/Kconfig.
There are still a LOT of empty, stub Kconfig files.
* Kconfig: Fleshed out apps/examples/buttons/Kconfig. There are still a LOT
of empty, stub Kconfig files.
* apps/netutils/webserver/httpd.c: Fix a bug that I introduced in
recent check-ins (Darcy Gong).
* apps/netutils/webclient/webclient.c: Fix another but that I introduced
when I was trying to add correct handling for loss of connection (Darcy Gong)
apps/Kconfig
-4
View File
@@ -34,7 +34,3 @@ endmenu
menu "System NSH Add-Ons"
source "$APPSDIR/system/Kconfig"
endmenu
menu "VSN board Add-Ons"
source "$APPSDIR/vsn/Kconfig"
endmenu
apps/Makefile
+5 -4
View File
@@ -46,10 +46,10 @@ APPDIR = ${shell pwd}
# appears in this directory as .config)
# SUBDIRS is the list of all directories containing Makefiles. It is used
# only for cleaning. namedapp must always be the first in the list. This
# list can be extended by the .config file as well
# list can be extended by the .config file as well.
CONFIGURED_APPS =
#SUBDIRS = examples graphics interpreters modbus namedapp nshlib netutils system vsn
#SUBDIRS = examples graphics interpreters modbus namedapp nshlib netutils system
ALL_SUBDIRS = $(dir $(shell /usr/bin/find . -name Makefile))
SUBDIRS = namedapp/ $(filter-out ./ ./namedapp/ ./examples/,$(ALL_SUBDIRS))
@@ -73,15 +73,16 @@ SUBDIRS = namedapp/ $(filter-out ./ ./namedapp/ ./examples/,$(ALL_SUBDIRS))
ifeq ($(CONFIG_NUTTX_NEWCONFIG),y)
# namedapp/Make.defs must be included first
-include namedapp/Make.defs
-include examples/Make.defs
-include graphics/Make.defs
-include interpreters/Make.defs
-include modbus/Make.defs
-include namedapp/Make.defs
-include netutils/Make.defs
-include nshlib/Make.defs
-include system/Make.defs
-include vsn/Make.defs
# INSTALLED_APPS is the list of currently available application directories. It
# is the same as CONFIGURED_APPS, but filtered to exclude any non-existent
apps/README.txt
+1 -1
View File
@@ -107,7 +107,7 @@ NuttX is configured. .config is included in the toplevel apps/Makefile.
As a minimum, this configuration file must define files to add to the
CONFIGURED_APPS list like:
CONFIGURED_APPS += examples/hello vsn/poweroff
CONFIGURED_APPS += examples/hello system/poweroff
Named Start-Up main() function
------------------------------
apps/ardrone_interface/ardrone_interface.c
+1 -1
View File
@@ -115,7 +115,7 @@ int ardrone_interface_main(int argc, char *argv[])
thread_should_exit = false;
ardrone_interface_task = task_spawn("ardrone_interface",
SCHED_RR,
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 15,
4096,
ardrone_interface_thread_main,
apps/attitude_estimator_ekf/Makefile
+1
View File
@@ -36,6 +36,7 @@ PRIORITY = SCHED_PRIORITY_DEFAULT
STACKSIZE = 2048
CSRCS = attitude_estimator_ekf_main.c \
attitude_estimator_ekf_params.c \
codegen/eye.c \
codegen/attitudeKalmanfilter.c \
codegen/mrdivide.c \
apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
+189 -160
View File
@@ -58,25 +58,20 @@
#include <uORB/topics/debug_key_value.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/parameter_update.h>
#include <arch/board/up_hrt.h>
#include <systemlib/systemlib.h>
#include "codegen/attitudeKalmanfilter_initialize.h"
#include "codegen/attitudeKalmanfilter.h"
#include "attitude_estimator_ekf_params.h"
__EXPORT int attitude_estimator_ekf_main(int argc, char *argv[]);
// #define N_STATES 6
// #define PROJECTION_INITIALIZE_COUNTER_LIMIT 5000
// #define REPROJECTION_COUNTER_LIMIT 125
static unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
static float dt = 1.0f;
/* 0, 0, -9.81, 1, 1, 1, wo (gyro offset), w */
/* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */
static float z_k[9]; /**< Measurement vector */
static float x_aposteriori_k[12]; /**< */
@@ -94,6 +89,7 @@ static float P_aposteriori_k[144] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0
0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 100.0f, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 0, 100.0f,
};
static float P_aposteriori[144] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0,
@@ -107,11 +103,14 @@ static float P_aposteriori[144] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 100.0f, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 0, 100.0f,
}; /**< init: diagonal matrix with big values */
// static float knownConst[15] = {1, 1, 1, 1, 1, 0.04, 4, 0.1, 70, 70, -2000, 9.81, 1, 4, 1}; /**< knownConst has the following entries [PrvaA,PrvarM,PrvarWO,PrvarW||MsvarA,MsvarM,MsvarW] */
/* output euler angles */
static float euler[3] = {0.0f, 0.0f, 0.0f};
static float Rot_matrix[9] = {1.f, 0, 0,
0, 1.f, 0,
0, 0, 1.f
}; /**< init: identity matrix */
0, 1.f, 0,
0, 0, 1.f
}; /**< init: identity matrix */
static bool thread_should_exit = false; /**< Deamon exit flag */
@@ -160,8 +159,8 @@ int attitude_estimator_ekf_main(int argc, char *argv[])
thread_should_exit = false;
attitude_estimator_ekf_task = task_spawn("attitude_estimator_ekf",
SCHED_RR,
SCHED_PRIORITY_DEFAULT,
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
20000,
attitude_estimator_ekf_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
@@ -221,7 +220,10 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
/* 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);
orb_set_interval(sub_raw, 4);
/* subscribe to param changes */
int sub_params = orb_subscribe(ORB_ID(parameter_update));
/* advertise attitude */
orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);
@@ -236,21 +238,26 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
struct debug_key_value_s dbg = { .key = "", .value = 0.0f };
orb_advert_t pub_dbg = orb_advertise(ORB_ID(debug_key_value), &dbg);
/* 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};
/* process noise covariance */
float q[12];
/* measurement noise covariance */
float r[9];
/* output euler angles */
float euler[3] = {0.0f, 0.0f, 0.0f};
struct attitude_estimator_ekf_params ekf_params;
struct attitude_estimator_ekf_param_handles ekf_param_handles;
/* initialize parameter handles */
parameters_init(&ekf_param_handles);
/* Main loop*/
while (!thread_should_exit) {
struct pollfd fds[1] = {
struct pollfd fds[2] = {
{ .fd = sub_raw, .events = POLLIN },
{ .fd = sub_params, .events = POLLIN }
};
int ret = poll(fds, 1, 1000);
int ret = poll(fds, 2, 1000);
if (ret < 0) {
/* XXX this is seriously bad - should be an emergency */
@@ -258,152 +265,174 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
/* XXX this means no sensor data - should be critical or emergency */
printf("[attitude estimator ekf] WARNING: Not getting sensor data - sensor app running?\n");
} else {
/* only update parameters if they changed */
if (fds[1].revents & POLLIN) {
/* read from param to clear updated flag */
struct parameter_update_s update;
orb_copy(ORB_ID(parameter_update), sub_params, &update);
orb_copy(ORB_ID(sensor_combined), sub_raw, &raw);
/* update parameters */
parameters_update(&ekf_param_handles, &ekf_params);
}
/* Calculate data time difference in seconds */
dt = (raw.timestamp - last_measurement) / 1000000.0f;
last_measurement = raw.timestamp;
/* only run filter if sensor values changed */
if (fds[0].revents & POLLIN) {
/* Fill in gyro measurements */
z_k[0] = raw.gyro_rad_s[0];
z_k[1] = raw.gyro_rad_s[1];
z_k[2] = raw.gyro_rad_s[2];
/* get latest measurements */
orb_copy(ORB_ID(sensor_combined), sub_raw, &raw);
/* scale from 14 bit to m/s2 */
z_k[3] = raw.accelerometer_m_s2[0];
z_k[4] = raw.accelerometer_m_s2[1];
z_k[5] = raw.accelerometer_m_s2[2];
/* Calculate data time difference in seconds */
dt = (raw.timestamp - last_measurement) / 1000000.0f;
last_measurement = raw.timestamp;
uint8_t update_vect[3] = {0, 0, 0};
z_k[6] = raw.magnetometer_ga[0];
z_k[7] = raw.magnetometer_ga[1];
z_k[8] = raw.magnetometer_ga[2];
uint64_t now = hrt_absolute_time();
unsigned int time_elapsed = now - last_run;
last_run = now;
if (time_elapsed > loop_interval_alarm) {
//TODO: add warning, cpu overload here
if (overloadcounter == 20) {
printf("CPU OVERLOAD DETECTED IN ATTITUDE ESTIMATOR EKF (%lu > %lu)\n", time_elapsed, loop_interval_alarm);
overloadcounter = 0;
/* Fill in gyro measurements */
if (sensor_last_count[0] != raw.gyro_counter) {
update_vect[0] = 1;
sensor_last_count[0] = raw.gyro_counter;
sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]);
sensor_last_timestamp[0] = raw.timestamp;
}
overloadcounter++;
z_k[0] = raw.gyro_rad_s[0];
z_k[1] = raw.gyro_rad_s[1];
z_k[2] = raw.gyro_rad_s[2];
/* update accelerometer measurements */
if (sensor_last_count[1] != raw.accelerometer_counter) {
update_vect[1] = 1;
sensor_last_count[1] = raw.accelerometer_counter;
sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]);
sensor_last_timestamp[1] = raw.timestamp;
}
z_k[3] = raw.accelerometer_m_s2[0];
z_k[4] = raw.accelerometer_m_s2[1];
z_k[5] = raw.accelerometer_m_s2[2];
/* update magnetometer measurements */
if (sensor_last_count[2] != raw.magnetometer_counter) {
update_vect[2] = 1;
sensor_last_count[2] = raw.magnetometer_counter;
sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]);
sensor_last_timestamp[2] = raw.timestamp;
}
z_k[6] = raw.magnetometer_ga[0];
z_k[7] = raw.magnetometer_ga[1];
z_k[8] = raw.magnetometer_ga[2];
uint64_t now = hrt_absolute_time();
unsigned int time_elapsed = now - last_run;
last_run = now;
if (time_elapsed > loop_interval_alarm) {
//TODO: add warning, cpu overload here
// if (overloadcounter == 20) {
// printf("CPU OVERLOAD DETECTED IN ATTITUDE ESTIMATOR EKF (%lu > %lu)\n", time_elapsed, loop_interval_alarm);
// overloadcounter = 0;
// }
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 */
if (!const_initialized /*&& dt < 0.05 && dt > 0.005*/)
{
dt = 0.005f;
parameters_update(&ekf_param_handles, &ekf_params);
x_aposteriori_k[0] = z_k[0];
x_aposteriori_k[1] = z_k[1];
x_aposteriori_k[2] = z_k[2];
x_aposteriori_k[3] = 0.0f;
x_aposteriori_k[4] = 0.0f;
x_aposteriori_k[5] = 0.0f;
x_aposteriori_k[6] = z_k[3];
x_aposteriori_k[7] = z_k[4];
x_aposteriori_k[8] = z_k[5];
x_aposteriori_k[9] = z_k[6];
x_aposteriori_k[10] = z_k[7];
x_aposteriori_k[11] = z_k[8];
const_initialized = true;
}
/* do not execute the filter if not initialized */
if (!const_initialized) {
continue;
}
dt = 0.004f;
uint64_t timing_start = hrt_absolute_time();
// attitudeKalmanfilter(dt, update_vect, z_k, &z_k_sizes, u, x_aposteriori_k, P_aposteriori_k, knownConst, euler,
// Rot_matrix, x_aposteriori, P_aposteriori);
attitudeKalmanfilter(update_vect, dt, z_k, x_aposteriori_k, P_aposteriori_k, ekf_params.q, ekf_params.r,
euler, Rot_matrix, x_aposteriori, P_aposteriori);
/* swap values for next iteration */
memcpy(P_aposteriori_k, P_aposteriori, sizeof(P_aposteriori_k));
memcpy(x_aposteriori_k, x_aposteriori, sizeof(x_aposteriori_k));
uint64_t timing_diff = hrt_absolute_time() - timing_start;
// /* print rotation matrix every 200th time */
if (printcounter % 200 == 0) {
// printf("x apo:\n%8.4f\t%8.4f\t%8.4f\n%8.4f\t%8.4f\t%8.4f\n%8.4f\t%8.4f\t%8.4f\n",
// x_aposteriori[0], x_aposteriori[1], x_aposteriori[2],
// x_aposteriori[3], x_aposteriori[4], x_aposteriori[5],
// x_aposteriori[6], x_aposteriori[7], x_aposteriori[8]);
// }
//printf("EKF attitude iteration: %d, runtime: %d us, dt: %d us (%d Hz)\n", loopcounter, (int)timing_diff, (int)(dt * 1000000.0f), (int)(1.0f / dt));
//printf("roll: %8.4f\tpitch: %8.4f\tyaw:%8.4f\n", (double)euler[0], (double)euler[1], (double)euler[2]);
//printf("update rates gyro: %8.4f\taccel: %8.4f\tmag:%8.4f\n", (double)sensor_update_hz[0], (double)sensor_update_hz[1], (double)sensor_update_hz[2]);
// printf("\n%d\t%d\t%d\n%d\t%d\t%d\n%d\t%d\t%d\n", (int)(Rot_matrix[0] * 100), (int)(Rot_matrix[1] * 100), (int)(Rot_matrix[2] * 100),
// (int)(Rot_matrix[3] * 100), (int)(Rot_matrix[4] * 100), (int)(Rot_matrix[5] * 100),
// (int)(Rot_matrix[6] * 100), (int)(Rot_matrix[7] * 100), (int)(Rot_matrix[8] * 100));
}
// int i = printcounter % 9;
// // for (int i = 0; i < 9; i++) {
// char name[10];
// sprintf(name, "xapo #%d", i);
// memcpy(dbg.key, name, sizeof(dbg.key));
// dbg.value = x_aposteriori[i];
// orb_publish(ORB_ID(debug_key_value), pub_dbg, &dbg);
printcounter++;
if (last_data > 0 && raw.timestamp - last_data > 12000) printf("[attitude estimator ekf] sensor data missed! (%llu)\n", raw.timestamp - last_data);
last_data = raw.timestamp;
/* send out */
att.timestamp = raw.timestamp;
att.roll = euler[0];
att.pitch = euler[1];
att.yaw = euler[2];
// XXX replace with x_apo after fix to filter
att.rollspeed = raw.gyro_rad_s[0]; //x_aposteriori[0];
att.pitchspeed = raw.gyro_rad_s[1]; //x_aposteriori[1];
att.yawspeed = raw.gyro_rad_s[2]; //x_aposteriori[2];
/* copy offsets */
memcpy(&att.rate_offsets, &(x_aposteriori[3]), sizeof(att.rate_offsets));
/* copy rotation matrix */
memcpy(&att.R, Rot_matrix, sizeof(Rot_matrix));
att.R_valid = true;
// Broadcast
orb_publish(ORB_ID(vehicle_attitude), pub_att, &att);
}
uint8_t update_vect[3] = {1, 1, 1};
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 */
if (!const_initialized /*&& dt < 0.05 && dt > 0.005*/)
{
dt = 0.005f;
q[0] = 1e1f;
q[1] = 1e1f;
q[2] = 1e1f;
/* process noise gyro offset covariance */
q[3] = 1e-4f;
q[4] = 1e-4f;
q[5] = 1e-4f;
q[6] = 1e-1f;
q[7] = 1e-1f;
q[8] = 1e-1f;
q[9] = 1e-1f;
q[10] = 1e-1f;
q[11] = 1e-1f;
r[0]= 1e-2f;
r[1]= 1e-2f;
r[2]= 1e-2f;
r[3]= 1e-1f;
r[4]= 1e-1f;
r[5]= 1e-1f;
r[6]= 1e-1f;
r[7]= 1e-1f;
r[8]= 1e-1f;
x_aposteriori_k[0] = z_k[0];
x_aposteriori_k[1] = z_k[1];
x_aposteriori_k[2] = z_k[2];
x_aposteriori_k[3] = 0.0f;
x_aposteriori_k[4] = 0.0f;
x_aposteriori_k[5] = 0.0f;
x_aposteriori_k[6] = z_k[3];
x_aposteriori_k[7] = z_k[4];
x_aposteriori_k[8] = z_k[5];
x_aposteriori_k[9] = z_k[6];
x_aposteriori_k[10] = z_k[7];
x_aposteriori_k[11] = z_k[8];
const_initialized = true;
}
/* do not execute the filter if not initialized */
if (!const_initialized) {
continue;
}
dt = 0.004f;
uint64_t timing_start = hrt_absolute_time();
// attitudeKalmanfilter(dt, update_vect, z_k, &z_k_sizes, u, x_aposteriori_k, P_aposteriori_k, knownConst, euler,
// Rot_matrix, x_aposteriori, P_aposteriori);
attitudeKalmanfilter(update_vect, dt, z_k, x_aposteriori_k, P_aposteriori_k, q, r,
euler, Rot_matrix, x_aposteriori, P_aposteriori);
/* swap values for next iteration */
memcpy(P_aposteriori_k, P_aposteriori, sizeof(P_aposteriori_k));
memcpy(x_aposteriori_k, x_aposteriori, sizeof(x_aposteriori_k));
uint64_t timing_diff = hrt_absolute_time() - timing_start;
// /* print rotation matrix every 200th time */
// if (printcounter % 200 == 0) {
// // printf("x apo:\n%8.4f\t%8.4f\t%8.4f\n%8.4f\t%8.4f\t%8.4f\n%8.4f\t%8.4f\t%8.4f\n",
// // x_aposteriori[0], x_aposteriori[1], x_aposteriori[2],
// // x_aposteriori[3], x_aposteriori[4], x_aposteriori[5],
// // x_aposteriori[6], x_aposteriori[7], x_aposteriori[8]);
// // }
// printf("EKF attitude iteration: %d, runtime: %d us, dt: %d us (%d Hz)\n", loopcounter, (int)timing_diff, (int)(dt * 1000000.0f), (int)(1.0f / dt));
// printf("roll: %8.4f\tpitch: %8.4f\tyaw:%8.4f\n", (double)euler[0], (double)euler[1], (double)euler[2]);
// // printf("\n%d\t%d\t%d\n%d\t%d\t%d\n%d\t%d\t%d\n", (int)(Rot_matrix[0] * 100), (int)(Rot_matrix[1] * 100), (int)(Rot_matrix[2] * 100),
// // (int)(Rot_matrix[3] * 100), (int)(Rot_matrix[4] * 100), (int)(Rot_matrix[5] * 100),
// // (int)(Rot_matrix[6] * 100), (int)(Rot_matrix[7] * 100), (int)(Rot_matrix[8] * 100));
// }
// int i = printcounter % 9;
// // for (int i = 0; i < 9; i++) {
// char name[10];
// sprintf(name, "xapo #%d", i);
// memcpy(dbg.key, name, sizeof(dbg.key));
// dbg.value = x_aposteriori[i];
// orb_publish(ORB_ID(debug_key_value), pub_dbg, &dbg);
printcounter++;
if (last_data > 0 && raw.timestamp - last_data > 12000) printf("[attitude estimator ekf] sensor data missed! (%llu)\n", raw.timestamp - last_data);
last_data = raw.timestamp;
/* send out */
att.timestamp = raw.timestamp;
att.roll = euler[0];
att.pitch = euler[1];
att.yaw = euler[2];
att.rollspeed = x_aposteriori[0];
att.pitchspeed = x_aposteriori[1];
att.yawspeed = x_aposteriori[2];
// Broadcast
orb_publish(ORB_ID(vehicle_attitude), pub_att, &att);
}
loopcounter++;
apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c
+131
View File
@@ -0,0 +1,131 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: Tobias Naegeli <naegelit@student.ethz.ch>
* 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 attitude_estimator_ekf_params.c
*
* Parameters for EKF filter
*/
#include "attitude_estimator_ekf_params.h"
/* Extended Kalman Filter covariances */
/* gyro process noise */
PARAM_DEFINE_FLOAT(EKF_ATT_Q0, 1e1f);
PARAM_DEFINE_FLOAT(EKF_ATT_Q1, 1e1f);
PARAM_DEFINE_FLOAT(EKF_ATT_Q2, 1e1f);
/* gyro offsets process noise */
PARAM_DEFINE_FLOAT(EKF_ATT_Q3, 1e-4f);
PARAM_DEFINE_FLOAT(EKF_ATT_Q4, 1e-4f);
PARAM_DEFINE_FLOAT(EKF_ATT_Q5, 1e-4f);
/* accelerometer process noise */
PARAM_DEFINE_FLOAT(EKF_ATT_Q6, 1e-1f);
PARAM_DEFINE_FLOAT(EKF_ATT_Q7, 1e-1f);
PARAM_DEFINE_FLOAT(EKF_ATT_Q8, 1e-1f);
/* magnetometer process noise */
PARAM_DEFINE_FLOAT(EKF_ATT_Q9, 1e-1f);
PARAM_DEFINE_FLOAT(EKF_ATT_Q10, 1e-1f);
PARAM_DEFINE_FLOAT(EKF_ATT_Q11, 1e-1f);
/* gyro measurement noise */
PARAM_DEFINE_FLOAT(EKF_ATT_R0, 1e-2f);
PARAM_DEFINE_FLOAT(EKF_ATT_R1, 1e-2f);
PARAM_DEFINE_FLOAT(EKF_ATT_R2, 1e-2f);
/* accelerometer measurement noise */
PARAM_DEFINE_FLOAT(EKF_ATT_R3, 1e-1f);
PARAM_DEFINE_FLOAT(EKF_ATT_R4, 1e-1f);
PARAM_DEFINE_FLOAT(EKF_ATT_R5, 1e-1f);
/* magnetometer measurement noise */
PARAM_DEFINE_FLOAT(EKF_ATT_R6, 1e-1f);
PARAM_DEFINE_FLOAT(EKF_ATT_R7, 1e-1f);
PARAM_DEFINE_FLOAT(EKF_ATT_R8, 1e-1f);
int parameters_init(struct attitude_estimator_ekf_param_handles *h)
{
/* PID parameters */
h->q0 = param_find("EKF_ATT_Q0");
h->q1 = param_find("EKF_ATT_Q1");
h->q2 = param_find("EKF_ATT_Q2");
h->q3 = param_find("EKF_ATT_Q3");
h->q4 = param_find("EKF_ATT_Q4");
h->q5 = param_find("EKF_ATT_Q5");
h->q6 = param_find("EKF_ATT_Q6");
h->q7 = param_find("EKF_ATT_Q7");
h->q8 = param_find("EKF_ATT_Q8");
h->q9 = param_find("EKF_ATT_Q9");
h->q10 = param_find("EKF_ATT_Q10");
h->q11 = param_find("EKF_ATT_Q11");
h->r0 = param_find("EKF_ATT_R0");
h->r1 = param_find("EKF_ATT_R1");
h->r2 = param_find("EKF_ATT_R2");
h->r3 = param_find("EKF_ATT_R3");
h->r4 = param_find("EKF_ATT_R4");
h->r5 = param_find("EKF_ATT_R5");
h->r6 = param_find("EKF_ATT_R6");
h->r7 = param_find("EKF_ATT_R7");
h->r8 = param_find("EKF_ATT_R8");
return OK;
}
int parameters_update(const struct attitude_estimator_ekf_param_handles *h, struct attitude_estimator_ekf_params *p)
{
param_get(h->q0, &(p->q[0]));
param_get(h->q1, &(p->q[1]));
param_get(h->q2, &(p->q[2]));
param_get(h->q3, &(p->q[3]));
param_get(h->q4, &(p->q[4]));
param_get(h->q5, &(p->q[5]));
param_get(h->q6, &(p->q[6]));
param_get(h->q7, &(p->q[7]));
param_get(h->q8, &(p->q[8]));
param_get(h->q9, &(p->q[9]));
param_get(h->q10, &(p->q[10]));
param_get(h->q11, &(p->q[11]));
param_get(h->r0, &(p->r[0]));
param_get(h->r1, &(p->r[1]));
param_get(h->r2, &(p->r[2]));
param_get(h->r3, &(p->r[3]));
param_get(h->r4, &(p->r[4]));
param_get(h->r5, &(p->r[5]));
param_get(h->r6, &(p->r[6]));
param_get(h->r7, &(p->r[7]));
param_get(h->r8, &(p->r[8]));
return OK;
}
nuttx/configs/px4fmu/include/drv_ms5611.h → apps/attitude_estimator_ekf/attitude_estimator_ekf_params.h
+34 -46
View File
@@ -1,5 +1,8 @@
/*
* Copyright (C) 2012 Lorenz Meier. All rights reserved.
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: Tobias Naegeli <naegelit@student.ethz.ch>
* 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
@@ -11,7 +14,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 of the author or the names of 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.
*
@@ -27,50 +30,35 @@
* 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.
*/
/*
* Driver for the Meas Spec MS5611 barometric pressure sensor
*/
#include <sys/ioctl.h>
#define _MS5611BASE 0x6A00
#define MS5611C(_x) _IOC(_MS5611BASE, _x)
/*
* Sets the sensor internal sampling rate, and if a buffer
* has been configured, the rate at which entries will be
* added to the buffer.
*/
#define MS5611_SETRATE MS5611C(1)
/* set rate (configuration A register */
#define MS5611_RATE_0_75HZ (0 << 2) /* 0.75 Hz */
/*
* Sets the sensor internal range.
*/
#define MS5611_SETRANGE MS5611C(2)
#define MS5611_RANGE_0_88GA (0 << 5)
/*
* Sets the address of a shared MS5611_buffer
* structure that is maintained by the driver.
*
* If zero is passed as the address, disables
* the buffer updating.
*/
#define MS5611_SETBUFFER MS5611C(3)
****************************************************************************/
struct ms5611_buffer {
uint32_t size; /* number of entries in the samples[] array */
uint32_t next; /* the next entry that will be populated */
struct {
uint32_t pressure;
uint16_t temperature;
} samples[];
/*
* @file attitude_estimator_ekf_params.h
*
* Parameters for EKF filter
*/
#include <systemlib/param/param.h>
struct attitude_estimator_ekf_params {
float r[9];
float q[12];
};
extern int ms5611_attach(struct i2c_dev_s *i2c);
struct attitude_estimator_ekf_param_handles {
param_t r0, r1, r2, r3, r4, r5, r6, r7, r8;
param_t q0, q1, q2, q3, q4, q5, q6, q7, q8, q9, q10, q11;
};
/**
* Initialize all parameter handles and values
*
*/
int parameters_init(struct attitude_estimator_ekf_param_handles *h);
/**
* Update all parameters
*
*/
int parameters_update(const struct attitude_estimator_ekf_param_handles *h, struct attitude_estimator_ekf_params *p);
apps/commander/commander.c
+7 -11
View File
@@ -293,7 +293,7 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
struct sensor_combined_s raw;
/* 30 seconds */
const uint64_t calibration_interval_us = 45 * 1000000;
int calibration_interval_ms = 30 * 1000;
unsigned int calibration_counter = 0;
float mag_max[3] = {-FLT_MAX, -FLT_MAX, -FLT_MAX};
@@ -312,10 +312,10 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
warn("WARNING: failed to set scale / offsets for mag");
close(fd);
mavlink_log_info(mavlink_fd, "[commander] Please rotate around all axes.");
mavlink_log_info(mavlink_fd, "[commander] Please rotate around X");
uint64_t calibration_start = hrt_absolute_time();
while ((hrt_absolute_time() - calibration_start) < calibration_interval_us) {
while ((hrt_absolute_time() - calibration_start)/1000 < calibration_interval_ms) {
/* wait blocking for new data */
struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } };
@@ -348,11 +348,13 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
calibration_counter++;
} else {
/* any poll failure for 1s is a reason to abort */
mavlink_log_info(mavlink_fd, "[commander] mag calibration aborted, please retry.");
mavlink_log_info(mavlink_fd, "[commander] mag cal canceled");
break;
}
}
mavlink_log_info(mavlink_fd, "[commander] mag calibration done");
/* disable calibration mode */
status->flag_preflight_mag_calibration = false;
state_machine_publish(status_pub, status, mavlink_fd);
@@ -950,7 +952,7 @@ int commander_main(int argc, char *argv[])
thread_should_exit = false;
deamon_task = task_spawn("commander",
SCHED_RR,
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 50,
4096,
commander_thread_main,
@@ -1307,18 +1309,12 @@ int commander_thread_main(int argc, char *argv[])
//printf("RC: y:%i/t:%i s:%i chans: %i\n", rc_yaw_scale, rc_throttle_scale, mode_switch_rc_value, rc.chan_count);
if (sp_man.override_mode_switch > STICK_ON_OFF_LIMIT) {
current_status.flag_control_attitude_enabled = true;
current_status.flag_control_rates_enabled = false;
update_state_machine_mode_manual(stat_pub, &current_status, mavlink_fd);
} else if (sp_man.override_mode_switch < -STICK_ON_OFF_LIMIT) {
current_status.flag_control_attitude_enabled = true;
current_status.flag_control_rates_enabled = false;
update_state_machine_mode_auto(stat_pub, &current_status, mavlink_fd);
} else {
current_status.flag_control_attitude_enabled = true;
current_status.flag_control_rates_enabled = false;
update_state_machine_mode_stabilized(stat_pub, &current_status, mavlink_fd);
}
apps/commander/state_machine_helper.c
+5
View File
@@ -504,6 +504,7 @@ void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *c
current_status->flag_control_manual_enabled = true;
/* enable attitude control per default */
current_status->flag_control_attitude_enabled = true;
current_status->flag_control_rates_enabled = false; // XXX
if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) {
@@ -517,6 +518,8 @@ void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_
int old_mode = current_status->flight_mode;
current_status->flight_mode = VEHICLE_FLIGHT_MODE_STABILIZED;
current_status->flag_control_manual_enabled = true;
current_status->flag_control_attitude_enabled = true;
current_status->flag_control_rates_enabled = false; // XXX
if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) {
@@ -530,6 +533,8 @@ void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *cur
int old_mode = current_status->flight_mode;
current_status->flight_mode = VEHICLE_FLIGHT_MODE_AUTO;
current_status->flag_control_manual_enabled = true;
current_status->flag_control_attitude_enabled = true;
current_status->flag_control_rates_enabled = false; // XXX
if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_STABILIZED) {
apps/drivers/bma180/bma180.cpp
+2 -2
View File
@@ -410,8 +410,8 @@ BMA180::ioctl(struct file *filp, int cmd, unsigned long arg)
/* set default/max polling rate */
case SENSOR_POLLRATE_MAX:
case SENSOR_POLLRATE_DEFAULT:
/* XXX 500Hz is just a wild guess */
return ioctl(filp, SENSORIOCSPOLLRATE, 500);
/* With internal low pass filters enabled, 250 Hz is sufficient */
return ioctl(filp, SENSORIOCSPOLLRATE, 250);
/* adjust to a legal polling interval in Hz */
default: {
apps/drivers/device/cdev.cpp
+2 -2
View File
@@ -74,7 +74,7 @@ static int cdev_poll(struct file *filp, struct pollfd *fds, bool setup);
* Note that we use the GNU extension syntax here because we don't get designated
* initialisers in gcc 4.6.
*/
static const struct file_operations cdev_fops = {
const struct file_operations CDev::fops = {
open : cdev_open,
close : cdev_close,
read : cdev_read,
@@ -118,7 +118,7 @@ CDev::init()
goto out;
// now register the driver
ret = register_driver(_devname, &cdev_fops, 0666, (void *)this);
ret = register_driver(_devname, &fops, 0666, (void *)this);
if (ret != OK)
goto out;
apps/drivers/device/device.h
+6
View File
@@ -286,6 +286,12 @@ public:
bool is_open() { return _open_count > 0; }
protected:
/**
* Pointer to the default cdev file operations table; useful for
* registering clone devices etc.
*/
static const struct file_operations fops;
/**
* Check the current state of the device for poll events from the
* perspective of the file.
apps/drivers/l3gd20/l3gd20.cpp
+2 -2
View File
@@ -420,8 +420,8 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
/* set default/max polling rate */
case SENSOR_POLLRATE_MAX:
case SENSOR_POLLRATE_DEFAULT:
/* XXX 500Hz is just a wild guess */
return ioctl(filp, SENSORIOCSPOLLRATE, 500);
/* With internal low pass filters enabled, 250 Hz is sufficient */
return ioctl(filp, SENSORIOCSPOLLRATE, 250);
/* adjust to a legal polling interval in Hz */
default: {
apps/examples/Make.defs
+5 -1
View File
@@ -58,7 +58,7 @@ ifeq ($(CONFIG_EXAMPLES_DHCPD),y)
CONFIGURED_APPS += examples/dhcpd
endif
ifeq ($(CONFIG_EXAMPLE_DISCOVER),y)
ifeq ($(CONFIG_EXAMPLES_DISCOVER),y)
CONFIGURED_APPS += examples/discover
endif
@@ -218,6 +218,10 @@ ifeq ($(CONFIG_EXAMPLES_USBTERM),y)
CONFIGURED_APPS += examples/usbterm
endif
ifeq ($(CONFIG_EXAMPLES_WATCHDOG),y)
CONFIGURED_APPS += examples/watchdog
endif
ifeq ($(CONFIG_EXAMPLES_WGET),y)
CONFIGURED_APPS += examples/wget
endif
apps/examples/README.txt
+57 -57
View File
@@ -60,20 +60,20 @@ examples/buttons
This is a simple configuration that may be used to test the board-
specific button interfaces. Configuration options:
CONFIG_ARCH_BUTTONS - Must be defined for button support
CONFIG_EXAMPLE_BUTTONS_MIN - Lowest button number (MIN=0)
CONFIG_EXAMPLE_BUTTONS_MAX - Highest button number (MAX=7)
CONFIG_ARCH_BUTTONS - Must be defined for button support
CONFIG_EXAMPLES_BUTTONS_MIN - Lowest button number (MIN=0)
CONFIG_EXAMPLES_BUTTONS_MAX - Highest button number (MAX=7)
CONFIG_ARCH_IRQBUTTONS - Must be defined for interrupting button support
CONFIG_EXAMPLE_IRQBUTTONS_MIN - Lowest interrupting button number (MIN=0)
CONFIG_EXAMPLE_IRQBUTTONS_MAX - Highest interrupting button number (MAX=7)
CONFIG_ARCH_IRQBUTTONS - Must be defined for interrupting button support
CONFIG_EXAMPLES_IRQBUTTONS_MIN - Lowest interrupting button number (MIN=0)
CONFIG_EXAMPLES_IRQBUTTONS_MAX - Highest interrupting button number (MAX=7)
Name strings for buttons:
CONFIG_EXAMPLE_BUTTONS_NAME0, CONFIG_EXAMPLE_BUTTONS_NAME1,
CONFIG_EXAMPLE_BUTTONS_NAME2, CONFIG_EXAMPLE_BUTTONS_NAME3,
CONFIG_EXAMPLE_BUTTONS_NAME4, CONFIG_EXAMPLE_BUTTONS_NAME5,
CONFIG_EXAMPLE_BUTTONS_NAME6, CONFIG_EXAMPLE_BUTTONS_NAME7,
CONFIG_EXAMPLES_BUTTONS_NAME0, CONFIG_EXAMPLES_BUTTONS_NAME1,
CONFIG_EXAMPLES_BUTTONS_NAME2, CONFIG_EXAMPLES_BUTTONS_NAME3,
CONFIG_EXAMPLES_BUTTONS_NAME4, CONFIG_EXAMPLES_BUTTONS_NAME5,
CONFIG_EXAMPLES_BUTTONS_NAME6, CONFIG_EXAMPLES_BUTTONS_NAME7,
Additional architecture-/board- specific configuration settings may also
be required.
@@ -260,10 +260,10 @@ examples/dhcpd
configuration settings)
CONFIG_NET_BROADCAST=y - UDP broadcast support is needed.
CONFIG_EXAMPLE_DHCPD_NOMAC - (May be defined to use software assigned MAC)
CONFIG_EXAMPLE_DHCPD_IPADDR - Target IP address
CONFIG_EXAMPLE_DHCPD_DRIPADDR - Default router IP addess
CONFIG_EXAMPLE_DHCPD_NETMASK - Network mask
CONFIG_EXAMPLES_DHCPD_NOMAC - (May be defined to use software assigned MAC)
CONFIG_EXAMPLES_DHCPD_IPADDR - Target IP address
CONFIG_EXAMPLES_DHCPD_DRIPADDR - Default router IP addess
CONFIG_EXAMPLES_DHCPD_NETMASK - Network mask
See also CONFIG_NETUTILS_DHCPD_* settings described elsewhere
and used in netutils/dhcpd/dhcpd.c. These settings are required
@@ -291,11 +291,11 @@ examples/discover
NuttX configuration settings:
CONFIG_EXAMPLE_DISCOVER_DHCPC - DHCP Client
CONFIG_EXAMPLE_DISCOVER_NOMAC - Use canned MAC address
CONFIG_EXAMPLE_DISCOVER_IPADDR - Target IP address
CONFIG_EXAMPLE_DISCOVER_DRIPADDR - Router IP address
CONFIG_EXAMPLE_DISCOVER_NETMASK - Network Mask
CONFIG_EXAMPLES_DISCOVER_DHCPC - DHCP Client
CONFIG_EXAMPLES_DISCOVER_NOMAC - Use canned MAC address
CONFIG_EXAMPLES_DISCOVER_IPADDR - Target IP address
CONFIG_EXAMPLES_DISCOVER_DRIPADDR - Router IP address
CONFIG_EXAMPLES_DISCOVER_NETMASK - Network Mask
examples/ftpc
^^^^^^^^^^^^^
@@ -367,12 +367,12 @@ examples/ftpd
If CONFIG_EXAMPLES_FTPD_NONETINIT is not defined, then the following may
be specified to customized the network configuration:
CONFIG_EXAMPLE_FTPD_NOMAC - If the hardware has no MAC address of its
CONFIG_EXAMPLES_FTPD_NOMAC - If the hardware has no MAC address of its
own, define this =y to provide a bogus address for testing.
CONFIG_EXAMPLE_FTPD_IPADDR - The target IP address. Default 10.0.0.2
CONFIG_EXAMPLE_FTPD_DRIPADDR - The default router address. Default
CONFIG_EXAMPLES_FTPD_IPADDR - The target IP address. Default 10.0.0.2
CONFIG_EXAMPLES_FTPD_DRIPADDR - The default router address. Default
10.0.0.1
CONFIG_EXAMPLE_FTPD_NETMASK - The network mask. Default: 255.255.255.0
CONFIG_EXAMPLES_FTPD_NETMASK - The network mask. Default: 255.255.255.0
Other required configuration settings: Of course TCP networking support
is required. But here are a couple that are less obvious:
@@ -465,15 +465,15 @@ examples/igmp
does not do much of value -- Much more is needed in order to verify
the IGMP features!
* CONFIG_EXAMPLE_IGMP_NOMAC
* CONFIG_EXAMPLES_IGMP_NOMAC
Set if the hardware has no MAC address; one will be assigned
* CONFIG_EXAMPLE_IGMP_IPADDR
* CONFIG_EXAMPLES_IGMP_IPADDR
Target board IP address
* CONFIG_EXAMPLE_IGMP_DRIPADDR
* CONFIG_EXAMPLES_IGMP_DRIPADDR
Default router address
* CONFIG_EXAMPLE_IGMP_NETMASK
* CONFIG_EXAMPLES_IGMP_NETMASK
Network mask
* CONFIG_EXAMPLE_IGMP_GRPADDR
* CONFIG_EXAMPLES_IGMP_GRPADDR
Multicast group address
Applications using this example will need to provide an appconfig
@@ -1023,10 +1023,10 @@ examples/poll
CONFIG_NSOCKET_DESCRIPTORS - Defined to be greater than 0
CONFIG_NET_NTCP_READAHEAD_BUFFERS - Defined to be greater than zero
CONFIG_EXAMPLE_POLL_NOMAC - (May be defined to use software assigned MAC)
CONFIG_EXAMPLE_POLL_IPADDR - Target IP address
CONFIG_EXAMPLE_POLL_DRIPADDR - Default router IP addess
CONFIG_EXAMPLE_POLL_NETMASK - Network mask
CONFIG_EXAMPLES_POLL_NOMAC - (May be defined to use software assigned MAC)
CONFIG_EXAMPLES_POLL_IPADDR - Target IP address
CONFIG_EXAMPLES_POLL_DRIPADDR - Default router IP addess
CONFIG_EXAMPLES_POLL_NETMASK - Network mask
In order to for select to work with incoming connections, you
must also select:
@@ -1163,14 +1163,14 @@ examples/sendmail
Settings unique to this example include:
CONFIG_EXAMPLE_SENDMAIL_NOMAC - May be defined to use software assigned MAC (optional)
CONFIG_EXAMPLE_SENDMAIL_IPADDR - Target IP address (required)
CONFIG_EXAMPLE_SENDMAIL_DRIPADDR - Default router IP addess (required)
CONFIG_EXAMPLE_SENDMAILT_NETMASK - Network mask (required)
CONFIG_EXAMPLE_SENDMAIL_RECIPIENT - The recipient of the email (required)
CONFIG_EXAMPLE_SENDMAIL_SENDER - Optional. Default: "nuttx-testing@example.com"
CONFIG_EXAMPLE_SENDMAIL_SUBJECT - Optional. Default: "Testing SMTP from NuttX"
CONFIG_EXAMPLE_SENDMAIL_BODY - Optional. Default: "Test message sent by NuttX"
CONFIG_EXAMPLES_SENDMAIL_NOMAC - May be defined to use software assigned MAC (optional)
CONFIG_EXAMPLES_SENDMAIL_IPADDR - Target IP address (required)
CONFIG_EXAMPLES_SENDMAIL_DRIPADDR - Default router IP addess (required)
CONFIG_EXAMPLES_SENDMAILT_NETMASK - Network mask (required)
CONFIG_EXAMPLES_SENDMAIL_RECIPIENT - The recipient of the email (required)
CONFIG_EXAMPLES_SENDMAIL_SENDER - Optional. Default: "nuttx-testing@example.com"
CONFIG_EXAMPLES_SENDMAIL_SUBJECT - Optional. Default: "Testing SMTP from NuttX"
CONFIG_EXAMPLES_SENDMAIL_BODY - Optional. Default: "Test message sent by NuttX"
NOTE: This test has not been verified on the NuttX target environment.
As of this writing, unit-tested in the Cygwin/Linux host environment.
@@ -1213,12 +1213,12 @@ examples/telnetd
Default: SCHED_PRIORITY_DEFAULT
CONFIG_EXAMPLES_TELNETD_CLIENTSTACKSIZE - Stack size allocated for the
Telnet client. Default: 2048
CONFIG_EXAMPLE_TELNETD_NOMAC - If the hardware has no MAC address of its
CONFIG_EXAMPLES_TELNETD_NOMAC - If the hardware has no MAC address of its
own, define this =y to provide a bogus address for testing.
CONFIG_EXAMPLE_TELNETD_IPADDR - The target IP address. Default 10.0.0.2
CONFIG_EXAMPLE_TELNETD_DRIPADDR - The default router address. Default
CONFIG_EXAMPLES_TELNETD_IPADDR - The target IP address. Default 10.0.0.2
CONFIG_EXAMPLES_TELNETD_DRIPADDR - The default router address. Default
10.0.0.1
CONFIG_EXAMPLE_TELNETD_NETMASK - The network mask. Default: 255.255.255.0
CONFIG_EXAMPLES_TELNETD_NETMASK - The network mask. Default: 255.255.255.0
The appconfig file (apps/.config) should include:
@@ -1240,9 +1240,9 @@ examples/thttpd
CGI programs. see configs/README.txt for most THTTPD settings.
In addition to those, this example accepts:
CONFIG_EXAMPLE_THTTPD_NOMAC - (May be defined to use software assigned MAC)
CONFIG_EXAMPLE_THTTPD_DRIPADDR - Default router IP addess
CONFIG_EXAMPLE_THTTPD_NETMASK - Network mask
CONFIG_EXAMPLES_THTTPD_NOMAC - (May be defined to use software assigned MAC)
CONFIG_EXAMPLES_THTTPD_DRIPADDR - Default router IP addess
CONFIG_EXAMPLES_THTTPD_NETMASK - Network mask
Applications using this example will need to provide an appconfig
file in the configuration directory with instruction to build applications
@@ -1335,11 +1335,11 @@ examples/uip
This is a port of uIP tiny webserver example application. Settings
specific to this example include:
CONFIG_EXAMPLE_UIP_NOMAC - (May be defined to use software assigned MAC)
CONFIG_EXAMPLE_UIP_IPADDR - Target IP address
CONFIG_EXAMPLE_UIP_DRIPADDR - Default router IP addess
CONFIG_EXAMPLE_UIP_NETMASK - Network mask
CONFIG_EXAMPLE_UIP_DHCPC - Select to get IP address via DHCP
CONFIG_EXAMPLES_UIP_NOMAC - (May be defined to use software assigned MAC)
CONFIG_EXAMPLES_UIP_IPADDR - Target IP address
CONFIG_EXAMPLES_UIP_DRIPADDR - Default router IP addess
CONFIG_EXAMPLES_UIP_NETMASK - Network mask
CONFIG_EXAMPLES_UIP_DHCPC - Select to get IP address via DHCP
If you use DHCPC, then some special configuration network options are
required. These include:
@@ -1637,11 +1637,11 @@ examples/wget
A simple web client example. It will obtain a file from a server using the HTTP
protocol. Settings unique to this example include:
CONFIG_EXAMPLE_WGET_URL - The URL of the file to get
CONFIG_EXAMPLE_WGET_NOMAC - (May be defined to use software assigned MAC)
CONFIG_EXAMPLE_WGET_IPADDR - Target IP address
CONFIG_EXAMPLE_WGET_DRIPADDR - Default router IP addess
CONFIG_EXAMPLE_WGET_NETMASK - Network mask
CONFIG_EXAMPLES_WGET_URL - The URL of the file to get
CONFIG_EXAMPLES_WGET_NOMAC - (May be defined to use software assigned MAC)
CONFIG_EXAMPLES_WGET_IPADDR - Target IP address
CONFIG_EXAMPLES_WGET_DRIPADDR - Default router IP addess
CONFIG_EXAMPLES_WGET_NETMASK - Network mask
This example uses netutils/webclient. Additional configuration settings apply
to that code as follows (but built-in defaults are probably OK):
apps/examples/adc/Kconfig
+24
View File
@@ -6,8 +6,32 @@
config EXAMPLES_ADC
bool "ADC example"
default n
depends on ADC
---help---
Enable the ADC example
if EXAMPLES_ADC
config EXAMPLES_ADC_DEVPATH
string "ADC device path"
default "/dev/adc0"
---help---
The default path to the ADC device. Default: /dev/adc0
config EXAMPLES_ADC_NSAMPLES
int "Number of Sample Groups"
default 0
depends on !NSH_BUILTIN_APPS
---help---
If NSH_BUILTIN_APPS is defined, then the number of samples is provided
on the command line and this value is ignored. Otherwise, this number
of samples is collected and the program terminates. Default: 0 (samples
are collected indefinitely).
config EXAMPLES_ADC_GROUPSIZE
int "Number of Samples per Group"
default 4
---help---
The number of samples to read at once. Default: 4
endif
apps/examples/adc/adc_main.c
+11 -3
View File
@@ -57,6 +57,14 @@
* Pre-processor Definitions
****************************************************************************/
/* Use CONFIG_EXAMPLES_ADC_NSAMPLES == 0 to mean to collect samples
* indefinitely.
*/
#ifndef CONFIG_EXAMPLES_ADC_NSAMPLES
# define CONFIG_EXAMPLES_ADC_NSAMPLES 0
#endif
/****************************************************************************
* Private Types
****************************************************************************/
@@ -249,7 +257,7 @@ int adc_main(int argc, char *argv[])
adc_devpath(&g_adcstate, CONFIG_EXAMPLES_ADC_DEVPATH);
#ifdef CONFIG_EXAMPLES_ADC_NSAMPLES
#if CONFIG_EXAMPLES_ADC_NSAMPLES > 0
g_adcstate.count = CONFIG_EXAMPLES_ADC_NSAMPLES;
#else
g_adcstate.count = 1;
@@ -267,7 +275,7 @@ int adc_main(int argc, char *argv[])
* samples that we collect before returning. Otherwise, we never return
*/
#if defined(CONFIG_NSH_BUILTIN_APPS) || defined(CONFIG_EXAMPLES_ADC_NSAMPLES)
#if defined(CONFIG_NSH_BUILTIN_APPS) || CONFIG_EXAMPLES_ADC_NSAMPLES > 0
message("adc_main: g_adcstate.count: %d\n", g_adcstate.count);
#endif
@@ -290,7 +298,7 @@ int adc_main(int argc, char *argv[])
#if defined(CONFIG_NSH_BUILTIN_APPS)
for (; g_adcstate.count > 0; g_adcstate.count--)
#elif defined(CONFIG_EXAMPLES_ADC_NSAMPLES)
#elif CONFIG_EXAMPLES_ADC_NSAMPLES > 0
for (g_adcstate.count = 0; g_adcstate.count < CONFIG_EXAMPLES_ADC_NSAMPLES; g_adcstate.count++)
#else
for (;;)
apps/examples/buttons/Kconfig
+51 -1
View File
@@ -7,7 +7,57 @@ config EXAMPLES_BUTTONS
bool "Buttons example"
default n
---help---
Enable the buttons example
Enable the buttons example. May require ARCH_BUTTONS on some boards.
if EXAMPLES_BUTTONS
config EXAMPLES_BUTTONS_MIN
int "Lowest Button Number"
default 0
config EXAMPLES_BUTTONS_MAX
int "Highest Button Number"
default 7
if ARCH_IRQBUTTONS
config EXAMPLES_IRQBUTTONS_MIN
int "Lowest Interrupting Button Number"
default 0
config EXAMPLES_IRQBUTTONS_MAX
int "Highest Interrupting Button Number"
default 7
config EXAMPLES_BUTTONS_NAME0
string "Button 0 Name"
default "Button 0"
config EXAMPLES_BUTTONS_NAME1
string "Button 1 Name"
default "Button 1"
config EXAMPLES_BUTTONS_NAME2
string "Button 2 Name"
default "Button 2"
config EXAMPLES_BUTTONS_NAME3
string "Button 3 Name"
default "Button 3"
config EXAMPLES_BUTTONS_NAME4
string "Button 4 Name"
default "Button 4"
config EXAMPLES_BUTTONS_NAME5
string "Button 5 Name"
default "Button 5"
config EXAMPLES_BUTTONS_NAME6
string "Button 6 Name"
default "Button 6"
config EXAMPLES_BUTTONS_NAME7
string "Button 7 Name"
default "Button 7"
endif
endif
apps/examples/buttons/buttons_main.c
+44 -44
View File
@@ -61,60 +61,60 @@
# error "CONFIG_ARCH_BUTTONS is not defined in the configuration"
#endif
#ifndef CONFIG_EXAMPLE_BUTTONS_NAME0
# define CONFIG_EXAMPLE_BUTTONS_NAME0 "BUTTON0"
#ifndef CONFIG_EXAMPLES_BUTTONS_NAME0
# define CONFIG_EXAMPLES_BUTTONS_NAME0 "BUTTON0"
#endif
#ifndef CONFIG_EXAMPLE_BUTTONS_NAME1
# define CONFIG_EXAMPLE_BUTTONS_NAME1 "BUTTON1"
#ifndef CONFIG_EXAMPLES_BUTTONS_NAME1
# define CONFIG_EXAMPLES_BUTTONS_NAME1 "BUTTON1"
#endif
#ifndef CONFIG_EXAMPLE_BUTTONS_NAME2
# define CONFIG_EXAMPLE_BUTTONS_NAME2 "BUTTON2"
#ifndef CONFIG_EXAMPLES_BUTTONS_NAME2
# define CONFIG_EXAMPLES_BUTTONS_NAME2 "BUTTON2"
#endif
#ifndef CONFIG_EXAMPLE_BUTTONS_NAME3
# define CONFIG_EXAMPLE_BUTTONS_NAME3 "BUTTON3"
#ifndef CONFIG_EXAMPLES_BUTTONS_NAME3
# define CONFIG_EXAMPLES_BUTTONS_NAME3 "BUTTON3"
#endif
#ifndef CONFIG_EXAMPLE_BUTTONS_NAME4
# define CONFIG_EXAMPLE_BUTTONS_NAME4 "BUTTON4"
#ifndef CONFIG_EXAMPLES_BUTTONS_NAME4
# define CONFIG_EXAMPLES_BUTTONS_NAME4 "BUTTON4"
#endif
#ifndef CONFIG_EXAMPLE_BUTTONS_NAME5
# define CONFIG_EXAMPLE_BUTTONS_NAME5 "BUTTON5"
#ifndef CONFIG_EXAMPLES_BUTTONS_NAME5
# define CONFIG_EXAMPLES_BUTTONS_NAME5 "BUTTON5"
#endif
#ifndef CONFIG_EXAMPLE_BUTTONS_NAME6
# define CONFIG_EXAMPLE_BUTTONS_NAME6 "BUTTON6"
#ifndef CONFIG_EXAMPLES_BUTTONS_NAME6
# define CONFIG_EXAMPLES_BUTTONS_NAME6 "BUTTON6"
#endif
#ifndef CONFIG_EXAMPLE_BUTTONS_NAME7
# define CONFIG_EXAMPLE_BUTTONS_NAME7 "BUTTON7"
#ifndef CONFIG_EXAMPLES_BUTTONS_NAME7
# define CONFIG_EXAMPLES_BUTTONS_NAME7 "BUTTON7"
#endif
#define BUTTON_MIN 0
#define BUTTON_MAX 7
#ifndef CONFIG_EXAMPLE_BUTTONS_MIN
# define CONFIG_EXAMPLE_BUTTONS_MIN BUTTON_MIN
#ifndef CONFIG_EXAMPLES_BUTTONS_MIN
# define CONFIG_EXAMPLES_BUTTONS_MIN BUTTON_MIN
#endif
#ifndef CONFIG_EXAMPLE_BUTTONS_MAX
# define CONFIG_EXAMPLE_BUTTONS_MAX BUTTON_MAX
#ifndef CONFIG_EXAMPLES_BUTTONS_MAX
# define CONFIG_EXAMPLES_BUTTONS_MAX BUTTON_MAX
#endif
#if CONFIG_EXAMPLE_BUTTONS_MIN > CONFIG_EXAMPLE_BUTTONS_MAX
# error "CONFIG_EXAMPLE_BUTTONS_MIN > CONFIG_EXAMPLE_BUTTONS_MAX"
#if CONFIG_EXAMPLES_BUTTONS_MIN > CONFIG_EXAMPLES_BUTTONS_MAX
# error "CONFIG_EXAMPLES_BUTTONS_MIN > CONFIG_EXAMPLES_BUTTONS_MAX"
#endif
#if CONFIG_EXAMPLE_BUTTONS_MAX > 7
# error "CONFIG_EXAMPLE_BUTTONS_MAX > 7"
#if CONFIG_EXAMPLES_BUTTONS_MAX > 7
# error "CONFIG_EXAMPLES_BUTTONS_MAX > 7"
#endif
#ifndef CONFIG_EXAMPLE_IRQBUTTONS_MIN
# define CONFIG_EXAMPLE_IRQBUTTONS_MIN CONFIG_EXAMPLE_BUTTONS_MIN
#ifndef CONFIG_EXAMPLES_IRQBUTTONS_MIN
# define CONFIG_EXAMPLES_IRQBUTTONS_MIN CONFIG_EXAMPLES_BUTTONS_MIN
#endif
#ifndef CONFIG_EXAMPLE_IRQBUTTONS_MAX
# define CONFIG_EXAMPLE_IRQBUTTONS_MAX CONFIG_EXAMPLE_BUTTONS_MAX
#ifndef CONFIG_EXAMPLES_IRQBUTTONS_MAX
# define CONFIG_EXAMPLES_IRQBUTTONS_MAX CONFIG_EXAMPLES_BUTTONS_MAX
#endif
#if CONFIG_EXAMPLE_IRQBUTTONS_MIN > CONFIG_EXAMPLE_IRQBUTTONS_MAX
# error "CONFIG_EXAMPLE_IRQBUTTONS_MIN > CONFIG_EXAMPLE_IRQBUTTONS_MAX"
#if CONFIG_EXAMPLES_IRQBUTTONS_MIN > CONFIG_EXAMPLES_IRQBUTTONS_MAX
# error "CONFIG_EXAMPLES_IRQBUTTONS_MIN > CONFIG_EXAMPLES_IRQBUTTONS_MAX"
#endif
#if CONFIG_EXAMPLE_IRQBUTTONS_MAX > 7
# error "CONFIG_EXAMPLE_IRQBUTTONS_MAX > 7"
#if CONFIG_EXAMPLES_IRQBUTTONS_MAX > 7
# error "CONFIG_EXAMPLES_IRQBUTTONS_MAX > 7"
#endif
#ifndef MIN
@@ -124,8 +124,8 @@
# define MAX(a,b) (a > b ? a : b)
#endif
#define MIN_BUTTON MIN(CONFIG_EXAMPLE_BUTTONS_MIN, CONFIG_EXAMPLE_IRQBUTTONS_MIN)
#define MAX_BUTTON MAX(CONFIG_EXAMPLE_BUTTONS_MAX, CONFIG_EXAMPLE_IRQBUTTONS_MAX)
#define MIN_BUTTON MIN(CONFIG_EXAMPLES_BUTTONS_MIN, CONFIG_EXAMPLES_IRQBUTTONS_MIN)
#define MAX_BUTTON MAX(CONFIG_EXAMPLES_BUTTONS_MAX, CONFIG_EXAMPLES_IRQBUTTONS_MAX)
#define NUM_BUTTONS (MAX_BUTTON - MIN_BUTTON + 1)
#define BUTTON_INDEX(b) ((b)-MIN_BUTTON)
@@ -187,7 +187,7 @@ static const struct button_info_s g_buttoninfo[NUM_BUTTONS] =
{
#if MIN_BUTTON < 1
{
CONFIG_EXAMPLE_BUTTONS_NAME0,
CONFIG_EXAMPLES_BUTTONS_NAME0,
#ifdef CONFIG_ARCH_IRQBUTTONS
button0_handler
#endif
@@ -195,7 +195,7 @@ static const struct button_info_s g_buttoninfo[NUM_BUTTONS] =
#endif
#if MIN_BUTTON < 2 && MAX_BUTTON > 0
{
CONFIG_EXAMPLE_BUTTONS_NAME1,
CONFIG_EXAMPLES_BUTTONS_NAME1,
#ifdef CONFIG_ARCH_IRQBUTTONS
button1_handler
#endif
@@ -203,7 +203,7 @@ static const struct button_info_s g_buttoninfo[NUM_BUTTONS] =
#endif
#if MIN_BUTTON < 3 && MAX_BUTTON > 1
{
CONFIG_EXAMPLE_BUTTONS_NAME2,
CONFIG_EXAMPLES_BUTTONS_NAME2,
#ifdef CONFIG_ARCH_IRQBUTTONS
button2_handler
#endif
@@ -211,7 +211,7 @@ static const struct button_info_s g_buttoninfo[NUM_BUTTONS] =
#endif
#if MIN_BUTTON < 4 && MAX_BUTTON > 2
{
CONFIG_EXAMPLE_BUTTONS_NAME3,
CONFIG_EXAMPLES_BUTTONS_NAME3,
#ifdef CONFIG_ARCH_IRQBUTTONS
button3_handler
#endif
@@ -219,7 +219,7 @@ static const struct button_info_s g_buttoninfo[NUM_BUTTONS] =
#endif
#if MIN_BUTTON < 5 && MAX_BUTTON > 3
{
CONFIG_EXAMPLE_BUTTONS_NAME4,
CONFIG_EXAMPLES_BUTTONS_NAME4,
#ifdef CONFIG_ARCH_IRQBUTTONS
button4_handler
#endif
@@ -227,7 +227,7 @@ static const struct button_info_s g_buttoninfo[NUM_BUTTONS] =
#endif
#if MIN_BUTTON < 6 && MAX_BUTTON > 4
{
CONFIG_EXAMPLE_BUTTONS_NAME5,
CONFIG_EXAMPLES_BUTTONS_NAME5,
#ifdef CONFIG_ARCH_IRQBUTTONS
button5_handler
#endif
@@ -235,7 +235,7 @@ static const struct button_info_s g_buttoninfo[NUM_BUTTONS] =
#endif
#if MIN_BUTTON < 7 && MAX_BUTTON > 5
{
CONFIG_EXAMPLE_BUTTONS_NAME6,
CONFIG_EXAMPLES_BUTTONS_NAME6,
#ifdef CONFIG_ARCH_IRQBUTTONS
button6_handler
#endif
@@ -243,7 +243,7 @@ static const struct button_info_s g_buttoninfo[NUM_BUTTONS] =
#endif
#if MAX_BUTTON > 6
{
CONFIG_EXAMPLE_BUTTONS_NAME7,
CONFIG_EXAMPLES_BUTTONS_NAME7,
#ifdef CONFIG_ARCH_IRQBUTTONS
button7_handler
#endif
@@ -419,7 +419,7 @@ int buttons_main(int argc, char *argv[])
/* Register to recieve button interrupts */
#ifdef CONFIG_ARCH_IRQBUTTONS
for (i = CONFIG_EXAMPLE_IRQBUTTONS_MIN; i <= CONFIG_EXAMPLE_IRQBUTTONS_MAX; i++)
for (i = CONFIG_EXAMPLES_IRQBUTTONS_MIN; i <= CONFIG_EXAMPLES_IRQBUTTONS_MAX; i++)
{
xcpt_t oldhandler = up_irqbutton(i, g_buttoninfo[BUTTON_INDEX(i)].handler);
@@ -488,7 +488,7 @@ int buttons_main(int argc, char *argv[])
/* Un-register button handlers */
#if defined(CONFIG_ARCH_IRQBUTTONS) && defined(CONFIG_NSH_BUILTIN_APPS)
for (i = CONFIG_EXAMPLE_IRQBUTTONS_MIN; i <= CONFIG_EXAMPLE_IRQBUTTONS_MAX; i++)
for (i = CONFIG_EXAMPLES_IRQBUTTONS_MIN; i <= CONFIG_EXAMPLES_IRQBUTTONS_MAX; i++)
{
(void)up_irqbutton(i, NULL);
}
apps/examples/poll/net_listener.c
+5 -5
View File
@@ -290,14 +290,14 @@ static inline bool net_mksocket(struct net_listener_s *nls)
static void net_configure(void)
{
struct in_addr addr;
#if defined(CONFIG_EXAMPLE_POLL_NOMAC)
#if defined(CONFIG_EXAMPLES_POLL_NOMAC)
uint8_t mac[IFHWADDRLEN];
#endif
/* Configure uIP */
/* Many embedded network interfaces must have a software assigned MAC */
#ifdef CONFIG_EXAMPLE_POLL_NOMAC
#ifdef CONFIG_EXAMPLES_POLL_NOMAC
mac[0] = 0x00;
mac[1] = 0xe0;
mac[2] = 0xde;
@@ -309,17 +309,17 @@ static void net_configure(void)
/* Set up our host address */
addr.s_addr = HTONL(CONFIG_EXAMPLE_POLL_IPADDR);
addr.s_addr = HTONL(CONFIG_EXAMPLES_POLL_IPADDR);
uip_sethostaddr("eth0", &addr);
/* Set up the default router address */
addr.s_addr = HTONL(CONFIG_EXAMPLE_POLL_DRIPADDR);
addr.s_addr = HTONL(CONFIG_EXAMPLES_POLL_DRIPADDR);
uip_setdraddr("eth0", &addr);
/* Setup the subnet mask */
addr.s_addr = HTONL(CONFIG_EXAMPLE_POLL_NETMASK);
addr.s_addr = HTONL(CONFIG_EXAMPLES_POLL_NETMASK);
uip_setnetmask("eth0", &addr);
}
apps/examples/poll/net_reader.c
+5 -5
View File
@@ -83,14 +83,14 @@
static void net_configure(void)
{
struct in_addr addr;
#if defined(CONFIG_EXAMPLE_POLL_NOMAC)
#if defined(CONFIG_EXAMPLES_POLL_NOMAC)
uint8_t mac[IFHWADDRLEN];
#endif
/* Configure uIP */
/* Many embedded network interfaces must have a software assigned MAC */
#ifdef CONFIG_EXAMPLE_POLL_NOMAC
#ifdef CONFIG_EXAMPLES_POLL_NOMAC
mac[0] = 0x00;
mac[1] = 0xe0;
mac[2] = 0xde;
@@ -102,17 +102,17 @@ static void net_configure(void)
/* Set up our host address */
addr.s_addr = HTONL(CONFIG_EXAMPLE_POLL_IPADDR);
addr.s_addr = HTONL(CONFIG_EXAMPLES_POLL_IPADDR);
uip_sethostaddr("eth0", &addr);
/* Set up the default router address */
addr.s_addr = HTONL(CONFIG_EXAMPLE_POLL_DRIPADDR);
addr.s_addr = HTONL(CONFIG_EXAMPLES_POLL_DRIPADDR);
uip_setdraddr("eth0", &addr);
/* Setup the subnet mask */
addr.s_addr = HTONL(CONFIG_EXAMPLE_POLL_NETMASK);
addr.s_addr = HTONL(CONFIG_EXAMPLES_POLL_NETMASK);
uip_setnetmask("eth0", &addr);
}
apps/examples/px4_deamon_app/px4_deamon_app.c
+5 -2
View File
@@ -92,12 +92,11 @@ int px4_deamon_app_main(int argc, char *argv[])
thread_should_exit = false;
deamon_task = task_spawn("deamon",
SCHED_RR,
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
4096,
px4_deamon_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
thread_running = true;
exit(0);
}
@@ -123,6 +122,8 @@ int px4_deamon_thread_main(int argc, char *argv[]) {
printf("[deamon] starting\n");
thread_running = true;
while (!thread_should_exit) {
printf("Hello Deamon!\n");
sleep(10);
@@ -130,5 +131,7 @@ int px4_deamon_thread_main(int argc, char *argv[]) {
printf("[deamon] exiting.\n");
thread_running = false;
return 0;
}
apps/fixedwing_control/fixedwing_control.c
+2 -2
View File
@@ -31,7 +31,7 @@
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
// Workflow test comment - DEW
/**
* @file fixedwing_control.c
* Implementation of a fixed wing attitude and position controller.
@@ -416,7 +416,7 @@ int fixedwing_control_main(int argc, char *argv[])
thread_should_exit = false;
deamon_task = task_spawn("fixedwing_control",
SCHED_RR,
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 20,
4096,
fixedwing_control_thread_main,
apps/gps/gps.c
+1 -1
View File
@@ -143,7 +143,7 @@ int gps_main(int argc, char *argv[])
thread_should_exit = false;
deamon_task = task_spawn("gps",
SCHED_RR,
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
4096,
gps_thread_main,
apps/gps/ubx.c
+2
View File
@@ -781,6 +781,8 @@ void *ubx_watchdog_loop(void *args)
} else {
/* gps healthy */
ubx_success_count++;
ubx_healthy = true;
ubx_fail_count = 0;
if (!ubx_healthy && ubx_success_count == UBX_HEALTH_SUCCESS_COUNTER_LIMIT) {
//printf("[gps] ublox UBX module status ok (baud=%d)\r\n", current_gps_speed);
apps/mavlink/mavlink.c
+62 -26
View File
@@ -68,6 +68,7 @@
#include <uORB/topics/offboard_control_setpoint.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_vicon_position.h>
#include <uORB/topics/vehicle_global_position_setpoint.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/optical_flow.h>
@@ -134,6 +135,8 @@ static struct vehicle_command_s vcmd;
static struct actuator_armed_s armed;
static struct vehicle_vicon_position_s vicon_position;
static orb_advert_t pub_hil_global_pos = -1;
static orb_advert_t cmd_pub = -1;
static orb_advert_t flow_pub = -1;
@@ -191,8 +194,10 @@ static struct mavlink_subscriptions {
static struct mavlink_publications {
orb_advert_t offboard_control_sp_pub;
orb_advert_t vicon_position_pub;
} mavlink_pubs = {
.offboard_control_sp_pub = -1
.offboard_control_sp_pub = -1,
.vicon_position_pub = -1
};
@@ -884,7 +889,7 @@ static void *uorb_receiveloop(void *arg)
/* copy rc channels into local buffer */
orb_copy(ORB_ID(rc_channels), rc_sub, &rc);
/* Channels are sent in MAVLink main loop at a fixed interval */
mavlink_msg_rc_channels_raw_send(chan, rc.timestamp / 1000, 0, rc.chan[0].raw, rc.chan[1].raw, rc.chan[2].raw, rc.chan[3].raw,
mavlink_msg_rc_channels_raw_send(chan, rc.timestamp / 1000, 0, rc.chan[0].raw, rc.chan[1].raw, rc.chan[2].raw, rc.chan[3].raw,
rc.chan[4].raw, rc.chan[5].raw, rc.chan[6].raw, rc.chan[7].raw, rc.rssi);
}
@@ -1240,9 +1245,26 @@ void handleMessage(mavlink_message_t *msg)
/* 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);
}
}
/* Handle Vicon position estimates */
if (msg->msgid == MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE) {
mavlink_vicon_position_estimate_t pos;
mavlink_msg_vicon_position_estimate_decode(msg, &pos);
vicon_position.x = pos.x;
vicon_position.y = pos.y;
vicon_position.z = pos.z;
if (mavlink_pubs.vicon_position_pub <= 0) {
mavlink_pubs.vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position);
} else {
orb_publish(ORB_ID(vehicle_vicon_position), mavlink_pubs.vicon_position_pub, &vicon_position);
}
/* create command */
orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd);
}
/* Handle quadrotor motor setpoints */
@@ -1250,7 +1272,8 @@ void handleMessage(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 MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT target_system=%u, sysid = %u\n", quad_motors_setpoint.target_system, mavlink_system.sysid);
//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) {
/*
@@ -1260,19 +1283,28 @@ void handleMessage(mavlink_message_t *msg)
uint8_t ml_mode = 0;
bool ml_armed = false;
if (quad_motors_setpoint.mode & MAVLINK_OFFBOARD_CONTROL_FLAG_ARMED) {
ml_armed = true;
}
// if (quad_motors_setpoint.mode & MAVLINK_OFFBOARD_CONTROL_FLAG_ARMED) {
// ml_armed = true;
// }
switch (quad_motors_setpoint.mode) {
case 0:
ml_armed = false;
break;
case 1:
ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_RATES;
break;
ml_armed = true;
break;
case 2:
ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE;
break;
ml_armed = true;
break;
case 3:
ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY;
break;
@@ -1284,7 +1316,14 @@ void handleMessage(mavlink_message_t *msg)
offboard_control_sp.p1 = quad_motors_setpoint.roll[mavlink_system.sysid] / (float)INT16_MAX;
offboard_control_sp.p2 = quad_motors_setpoint.pitch[mavlink_system.sysid] / (float)INT16_MAX;
offboard_control_sp.p3= quad_motors_setpoint.yaw[mavlink_system.sysid] / (float)INT16_MAX;
offboard_control_sp.p4 = quad_motors_setpoint.thrust[mavlink_system.sysid] / (float)UINT16_MAX;
offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid]/(float)UINT16_MAX;
//offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid] ;
if (quad_motors_setpoint.thrust[mavlink_system.sysid] ==0){
ml_armed = false;
}
offboard_control_sp.armed = ml_armed;
offboard_control_sp.mode = ml_mode;
@@ -1418,6 +1457,8 @@ void handleMessage(mavlink_message_t *msg)
memset(&rc_hil, 0, sizeof(rc_hil));
static orb_advert_t rc_pub = 0;
rc_hil.timestamp = hrt_absolute_time();
rc_hil.chan_count = 4;
rc_hil.chan[0].raw = 1500 + man.x / 2;
rc_hil.chan[1].raw = 1500 + man.y / 2;
rc_hil.chan[2].raw = 1500 + man.r / 2;
@@ -1431,6 +1472,7 @@ void handleMessage(mavlink_message_t *msg)
struct manual_control_setpoint_s mc;
static orb_advert_t mc_pub = 0;
mc.timestamp = rc_hil.timestamp;
mc.roll = man.x / 1000.0f;
mc.pitch = man.y / 1000.0f;
mc.yaw = man.r / 1000.0f;
@@ -1695,14 +1737,6 @@ int mavlink_thread_main(int argc, char *argv[])
/* all subscriptions are now active, set up initial guess about rate limits */
if (baudrate >= 460800) {
/* 200 Hz / 5 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 5);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 5);
/* 200 Hz / 5 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 5);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 3);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 5);
/* 5 Hz */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 200);
} else if (baudrate >= 230400) {
/* 200 Hz / 5 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 20);
@@ -1716,13 +1750,13 @@ int mavlink_thread_main(int argc, char *argv[])
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 100);
} else if (baudrate >= 115200) {
/* 50 Hz / 20 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 20);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 20);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 200);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 200);
/* 20 Hz / 50 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 20);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 50);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 200);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 200);
/* 10 Hz / 100 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 100);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 200);
/* 1 Hz */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 1000);
} else if (baudrate >= 57600) {
@@ -1799,7 +1833,9 @@ int mavlink_thread_main(int argc, char *argv[])
mavlink_pm_queued_send();
/* sleep quarter the time */
usleep(25000);
mavlink_pm_queued_send();
if (baudrate > 57600) {
mavlink_pm_queued_send();
}
/* sleep 10 ms */
usleep(10000);
@@ -1869,7 +1905,7 @@ int mavlink_main(int argc, char *argv[])
thread_should_exit = false;
mavlink_task = task_spawn("mavlink",
SCHED_RR,
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
6000,
mavlink_thread_main,
apps/multirotor_att_control/multirotor_att_control_main.c
+74 -105
View File
@@ -82,63 +82,6 @@ static orb_advert_t actuator_pub;
static struct vehicle_status_s state;
/**
* Perform rate control right after gyro reading
*/
static void *rate_control_thread_main(void *arg)
{
prctl(PR_SET_NAME, "mc rate control", getpid());
struct actuator_controls_s actuators;
int gyro_sub = orb_subscribe(ORB_ID(sensor_gyro));
int rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
struct pollfd fds = { .fd = gyro_sub, .events = POLLIN };
struct gyro_report gyro_report;
struct vehicle_rates_setpoint_s rates_sp;
memset(&rates_sp, 0, sizeof(rates_sp));
float gyro_lp[3] = {0.0f, 0.0f, 0.0f};
while (!thread_should_exit) {
/* rate control at maximum rate */
/* wait for a sensor update, check for exit condition every 1000 ms */
int ret = poll(&fds, 1, 1000);
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("[mc att control] WARNING: Not getting gyro data, no rate control\n");
} else {
/* get data */
orb_copy(ORB_ID(sensor_gyro), gyro_sub, &gyro_report);
bool rates_sp_valid = false;
orb_check(rates_sp_sub, &rates_sp_valid);
if (rates_sp_valid) {
orb_copy(ORB_ID(vehicle_rates_setpoint), rates_sp_sub, &rates_sp);
}
/* perform local lowpass */
/* apply controller */
if (state.flag_control_rates_enabled) {
/* lowpass gyros */
// XXX
gyro_lp[0] = gyro_report.x;
gyro_lp[1] = gyro_report.y;
gyro_lp[2] = gyro_report.z;
multirotor_control_rates(&rates_sp, gyro_lp, &actuators);
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
}
}
}
return NULL;
}
static int
mc_thread_main(int argc, char *argv[])
{
@@ -185,6 +128,7 @@ mc_thread_main(int argc, char *argv[])
actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators);
orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
orb_advert_t rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
int rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
/* register the perf counter */
perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "multirotor_att_control");
@@ -192,13 +136,6 @@ mc_thread_main(int argc, char *argv[])
/* welcome user */
printf("[multirotor_att_control] starting\n");
/* ready, spawn pthread */
pthread_attr_t rate_control_attr;
pthread_attr_init(&rate_control_attr);
pthread_attr_setstacksize(&rate_control_attr, 2048);
pthread_t rate_control_thread;
pthread_create(&rate_control_thread, &rate_control_attr, rate_control_thread_main, NULL);
while (!thread_should_exit) {
/* wait for a sensor update, check for exit condition every 500 ms */
@@ -228,19 +165,52 @@ mc_thread_main(int argc, char *argv[])
/** STEP 1: Define which input is the dominating control input */
if (state.flag_control_offboard_enabled) {
/* offboard inputs */
if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES) {
rates_sp.roll = offboard_sp.p1;
rates_sp.pitch = offboard_sp.p2;
rates_sp.yaw = offboard_sp.p3;
rates_sp.thrust = offboard_sp.p4;
printf("thrust_rate=%8.4f\n",offboard_sp.p4);
rates_sp.timestamp = hrt_absolute_time();
orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
} else if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE) {
att_sp.roll_body = offboard_sp.p1;
att_sp.pitch_body = offboard_sp.p2;
att_sp.yaw_body = offboard_sp.p3;
att_sp.thrust = offboard_sp.p4;
printf("thrust_att=%8.4f\n",offboard_sp.p4);
att_sp.timestamp = hrt_absolute_time();
/* STEP 2: publish the result to the vehicle actuators */
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
}
if (state.flag_control_manual_enabled) {
/* decide wether we want rate or position input */
}
else if (state.flag_control_manual_enabled) {
/* manual inputs, from RC control or joystick */
att_sp.roll_body = manual.roll;
att_sp.pitch_body = manual.pitch;
att_sp.yaw_body = manual.yaw; // XXX Hack, remove, switch to yaw rate controller
/* set yaw rate */
rates_sp.yaw = manual.yaw;
att_sp.thrust = manual.throttle;
att_sp.timestamp = hrt_absolute_time();
if (state.flag_control_rates_enabled && !state.flag_control_attitude_enabled) {
rates_sp.roll = manual.roll;
rates_sp.pitch = manual.pitch;
rates_sp.yaw = manual.yaw;
rates_sp.thrust = manual.throttle;
rates_sp.timestamp = hrt_absolute_time();
}
if (state.flag_control_attitude_enabled) {
att_sp.roll_body = manual.roll;
att_sp.pitch_body = manual.pitch;
att_sp.yaw_body = manual.yaw; // XXX Hack, remove, switch to yaw rate controller
/* set yaw rate */
rates_sp.yaw = manual.yaw;
att_sp.thrust = manual.throttle;
att_sp.timestamp = hrt_absolute_time();
}
/* STEP 2: publish the result to the vehicle actuators */
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
if (motor_test_mode) {
att_sp.roll_body = 0.0f;
att_sp.pitch_body = 0.0f;
@@ -251,39 +221,40 @@ mc_thread_main(int argc, char *argv[])
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
}
} else if (state.flag_control_offboard_enabled) {
/* offboard inputs */
if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES) {
rates_sp.roll = offboard_sp.p1;
rates_sp.pitch = offboard_sp.p2;
rates_sp.yaw = offboard_sp.p3;
rates_sp.thrust = offboard_sp.p4;
rates_sp.timestamp = hrt_absolute_time();
orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
} else if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE) {
att_sp.roll_body = offboard_sp.p1;
att_sp.pitch_body = offboard_sp.p2;
att_sp.yaw_body = offboard_sp.p3;
att_sp.thrust = offboard_sp.p4;
att_sp.timestamp = hrt_absolute_time();
/* STEP 2: publish the result to the vehicle actuators */
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
}
/* decide wether we want rate or position input */
}
/** STEP 3: Identify the controller setup to run and set up the inputs correctly */
/* run attitude controller */
if (state.flag_control_attitude_enabled && !state.flag_control_rates_enabled) {
multirotor_control_attitude(&att_sp, &att, NULL, &actuators);
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
} else if (state.flag_control_attitude_enabled && state.flag_control_rates_enabled) {
multirotor_control_attitude(&att_sp, &att, &rates_sp, NULL);
orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
}
/* run attitude controller */
if (state.flag_control_attitude_enabled && !state.flag_control_rates_enabled) {
multirotor_control_attitude(&att_sp, &att, NULL, &actuators);
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
} else if (state.flag_control_attitude_enabled && state.flag_control_rates_enabled) {
multirotor_control_attitude(&att_sp, &att, &rates_sp, NULL);
orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
}
if (state.flag_control_rates_enabled) {
float gyro[3] = {0.0f, 0.0f, 0.0f};
/* get current rate setpoint */
bool rates_sp_valid = false;
orb_check(rates_sp_sub, &rates_sp_valid);
if (rates_sp_valid) {
orb_copy(ORB_ID(vehicle_rates_setpoint), rates_sp_sub, &rates_sp);
}
/* apply controller */
gyro[0] = att.rollspeed;
gyro[1] = att.pitchspeed;
gyro[2] = att.yawspeed;
multirotor_control_rates(&rates_sp, gyro, &actuators);
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
}
perf_end(mc_loop_perf);
}
@@ -305,8 +276,6 @@ mc_thread_main(int argc, char *argv[])
perf_print_counter(mc_loop_perf);
perf_free(mc_loop_perf);
pthread_join(rate_control_thread, NULL);
fflush(stdout);
exit(0);
}
@@ -352,9 +321,9 @@ int multirotor_att_control_main(int argc, char *argv[])
thread_should_exit = false;
mc_task = task_spawn("multirotor_att_control",
SCHED_RR,
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 15,
2048,
6000,
mc_thread_main,
NULL);
exit(0);
apps/multirotor_att_control/multirotor_rate_control.c
+23 -86
View File
@@ -56,18 +56,21 @@
// PARAM_DEFINE_FLOAT(MC_YAWRATE_LIM, 0.1f);
PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 0.2f); /* 0.15 F405 Flamewheel */
PARAM_DEFINE_FLOAT(MC_ATTRATE_I, 0.0f);
PARAM_DEFINE_FLOAT(MC_ATTRATE_AWU, 0.05f);
PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.0f);
//PARAM_DEFINE_FLOAT(MC_ATTRATE_I, 0.0f);
//PARAM_DEFINE_FLOAT(MC_ATTRATE_AWU, 0.05f);
PARAM_DEFINE_FLOAT(MC_ATTRATE_LIM, 8.0f); /**< roughly < 500 deg/s limit */
struct mc_rate_control_params {
float yawrate_p;
float yawrate_d;
float yawrate_i;
float yawrate_awu;
float yawrate_lim;
float attrate_p;
float attrate_d;
float attrate_i;
float attrate_awu;
float attrate_lim;
@@ -79,11 +82,13 @@ struct mc_rate_control_param_handles {
param_t yawrate_p;
param_t yawrate_i;
param_t yawrate_d;
param_t yawrate_awu;
param_t yawrate_lim;
param_t attrate_p;
param_t attrate_i;
param_t attrate_d;
param_t attrate_awu;
param_t attrate_lim;
};
@@ -106,11 +111,13 @@ static int parameters_init(struct mc_rate_control_param_handles *h)
/* PID parameters */
h->yawrate_p = param_find("MC_YAWRATE_P");
h->yawrate_i = param_find("MC_YAWRATE_I");
h->yawrate_d = param_find("MC_YAWRATE_D");
h->yawrate_awu = param_find("MC_YAWRATE_AWU");
h->yawrate_lim = param_find("MC_YAWRATE_LIM");
h->attrate_p = param_find("MC_ATTRATE_P");
h->attrate_i = param_find("MC_ATTRATE_I");
h->attrate_d = param_find("MC_ATTRATE_D");
h->attrate_awu = param_find("MC_ATTRATE_AWU");
h->attrate_lim = param_find("MC_ATTRATE_LIM");
@@ -121,11 +128,13 @@ static int parameters_update(const struct mc_rate_control_param_handles *h, stru
{
param_get(h->yawrate_p, &(p->yawrate_p));
param_get(h->yawrate_i, &(p->yawrate_i));
param_get(h->yawrate_d, &(p->yawrate_d));
param_get(h->yawrate_awu, &(p->yawrate_awu));
param_get(h->yawrate_lim, &(p->yawrate_lim));
param_get(h->attrate_p, &(p->attrate_p));
param_get(h->attrate_i, &(p->attrate_i));
param_get(h->attrate_d, &(p->attrate_d));
param_get(h->attrate_awu, &(p->attrate_awu));
param_get(h->attrate_lim, &(p->attrate_lim));
@@ -135,17 +144,14 @@ static int parameters_update(const struct mc_rate_control_param_handles *h, stru
void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
const float rates[], struct actuator_controls_s *actuators)
{
static float roll_control_last=0;
static float pitch_control_last=0;
static uint64_t last_run = 0;
const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
last_run = hrt_absolute_time();
static int motor_skip_counter = 0;
// static PID_t yaw_pos_controller;
static PID_t yaw_speed_controller;
static PID_t pitch_controller;
static PID_t roll_controller;
static struct mc_rate_control_params p;
static struct mc_rate_control_param_handles h;
@@ -155,102 +161,33 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
if (initialized == false) {
parameters_init(&h);
parameters_update(&h, &p);
pid_init(&yaw_speed_controller, p.yawrate_p, 0, p.yawrate_i, p.yawrate_awu,
PID_MODE_DERIVATIV_SET);
pid_init(&pitch_controller, p.attrate_p, p.attrate_i, 0, p.attrate_awu,
PID_MODE_DERIVATIV_SET);
pid_init(&roll_controller, p.attrate_p, p.attrate_i, 0, p.attrate_awu,
PID_MODE_DERIVATIV_SET);
initialized = true;
}
/* load new parameters with lower rate */
if (motor_skip_counter % 250 == 0) {
if (motor_skip_counter % 2500 == 0) {
/* update parameters from storage */
parameters_update(&h, &p);
/* apply parameters */
pid_set_parameters(&yaw_speed_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu);
pid_set_parameters(&pitch_controller, p.attrate_p, p.attrate_i, 0, p.attrate_awu);
pid_set_parameters(&roll_controller, p.attrate_p, p.attrate_i, 0, p.attrate_awu);
printf("p.yawrate_p: %8.4f\n", (double)p.yawrate_p);
}
/* calculate current control outputs */
/* control pitch (forward) output */
float pitch_control = pid_calculate(&pitch_controller, rate_sp->pitch,
rates[1], 0.0f, deltaT);
float pitch_control = p.attrate_p * deltaT *(rate_sp->pitch-rates[1])-p.attrate_d*(pitch_control_last);
pitch_control_last=pitch_control;
/* control roll (left/right) output */
float roll_control = pid_calculate(&roll_controller, rate_sp->roll,
rates[0], 0.0f, deltaT);
float roll_control = p.attrate_p * deltaT * (rate_sp->roll-rates[0])-p.attrate_d*(roll_control_last);
roll_control_last=roll_control;
/* control yaw rate */
float yaw_rate_control = pid_calculate(&yaw_speed_controller, rate_sp->yaw, rates[2], 0.0f, deltaT);
/*
* compensate the vertical loss of thrust
* when thrust plane has an angle.
* start with a factor of 1.0 (no change)
*/
float zcompensation = 1.0f;
// if (fabsf(att->roll) > 0.3f) {
// zcompensation *= 1.04675160154f;
// } else {
// zcompensation *= 1.0f / cosf(att->roll);
// }
// if (fabsf(att->pitch) > 0.3f) {
// zcompensation *= 1.04675160154f;
// } else {
// zcompensation *= 1.0f / cosf(att->pitch);
// }
float motor_thrust = 0.0f;
motor_thrust = rate_sp->thrust;
/* compensate thrust vector for roll / pitch contributions */
motor_thrust *= zcompensation;
/* limit yaw rate output */
if (yaw_rate_control > p.yawrate_lim) {
yaw_rate_control = p.yawrate_lim;
yaw_speed_controller.saturated = 1;
}
if (yaw_rate_control < -p.yawrate_lim) {
yaw_rate_control = -p.yawrate_lim;
yaw_speed_controller.saturated = 1;
}
if (pitch_control > p.attrate_lim) {
pitch_control = p.attrate_lim;
pitch_controller.saturated = 1;
}
if (pitch_control < -p.attrate_lim) {
pitch_control = -p.attrate_lim;
pitch_controller.saturated = 1;
}
if (roll_control > p.attrate_lim) {
roll_control = p.attrate_lim;
roll_controller.saturated = 1;
}
if (roll_control < -p.attrate_lim) {
roll_control = -p.attrate_lim;
roll_controller.saturated = 1;
}
float yaw_rate_control = p.yawrate_p * deltaT * (rate_sp->yaw-rates[2] );
actuators->control[0] = roll_control;
actuators->control[1] = pitch_control;
actuators->control[2] = yaw_rate_control;
actuators->control[3] = motor_thrust;
actuators->control[3] = rate_sp->thrust;
motor_skip_counter++;
}
apps/multirotor_pos_control/multirotor_pos_control.c
+119 -21
View File
@@ -49,24 +49,94 @@
#include <time.h>
#include <sys/prctl.h>
#include <arch/board/up_hrt.h>
#include "ardrone_control.h"
#include "attitude_control.h"
#include "rate_control.h"
#include "ardrone_motor_control.h"
#include "position_control.h"
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_vicon_position.h>
#include <systemlib/systemlib.h>
#include "multirotor_pos_control_params.h"
static bool thread_should_exit = false; /**< Deamon exit flag */
static bool thread_running = false; /**< Deamon status flag */
static int deamon_task; /**< Handle of deamon task / thread */
__EXPORT int multirotor_pos_control_main(int argc, char *argv[]);
static bool thread_should_exit;
static bool thread_running = false;
static int mpc_task;
/**
* Mainloop of position controller.
*/
static int multirotor_pos_control_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: deamon {start|stop|status} [-p <additional params>]\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_spawn().
*/
int multirotor_pos_control_main(int argc, char *argv[])
{
if (argc < 1)
usage("missing command");
if (!strcmp(argv[1], "start")) {
if (thread_running) {
printf("multirotor pos control already running\n");
/* this is not an error */
exit(0);
}
thread_should_exit = false;
deamon_task = task_spawn("multirotor pos control",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 60,
4096,
multirotor_pos_control_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
exit(0);
}
if (!strcmp(argv[1], "stop")) {
thread_should_exit = true;
exit(0);
}
if (!strcmp(argv[1], "status")) {
if (thread_running) {
printf("\tmultirotor pos control app is running\n");
} else {
printf("\tmultirotor pos control app not started\n");
}
exit(0);
}
usage("unrecognized command");
exit(1);
}
static int
mpc_thread_main(int argc, char *argv[])
multirotor_pos_control_thread_main(int argc, char *argv[])
{
/* welcome user */
printf("[multirotor pos control] Control started, taking over position control\n");
@@ -76,7 +146,7 @@ mpc_thread_main(int argc, char *argv[])
struct vehicle_attitude_s att;
//struct vehicle_global_position_setpoint_s global_pos_sp;
struct vehicle_local_position_setpoint_s local_pos_sp;
struct vehicle_local_position_s local_pos;
struct vehicle_vicon_position_s local_pos;
struct manual_control_setpoint_s manual;
struct vehicle_attitude_setpoint_s att_sp;
@@ -84,13 +154,23 @@ mpc_thread_main(int argc, char *argv[])
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
int state_sub = orb_subscribe(ORB_ID(vehicle_status));
int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
int local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
int local_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position));
//int global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
int local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
/* publish attitude setpoint */
orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
thread_running = true;
int loopcounter = 0;
struct multirotor_position_control_params p;
struct multirotor_position_control_param_handles h;
parameters_init(&h);
parameters_update(&h, &p);
while (1) {
/* get a local copy of the vehicle state */
orb_copy(ORB_ID(vehicle_status), state_sub, &state);
@@ -99,15 +179,34 @@ mpc_thread_main(int argc, char *argv[])
/* get a local copy of attitude */
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
/* get a local copy of local position */
orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos);
orb_copy(ORB_ID(vehicle_vicon_position), local_pos_sub, &local_pos);
/* get a local copy of local position setpoint */
orb_copy(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, &local_pos_sp);
if (state.state_machine == SYSTEM_STATE_AUTO) {
position_control(&state, &manual, &att, &local_pos, &local_pos_sp, &att_sp);
if (loopcounter == 500) {
parameters_update(&h, &p);
loopcounter = 0;
}
// if (state.state_machine == SYSTEM_STATE_AUTO) {
// XXX IMPLEMENT POSITION CONTROL HERE
float dT = 1.0f / 50.0f;
float x_setpoint = 0.0f;
/* local pos is the Vicon position */
att_sp.pitch_body = (local_pos.x - x_setpoint) * p.p * dT;
att_sp.roll_body = 0.0f;
att_sp.yaw_body = 0.0f;
att_sp.thrust = 0.4f;
att_sp.timestamp = hrt_absolute_time();
/* publish new attitude setpoint */
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
} else if (state.state_machine == SYSTEM_STATE_STABILIZE) {
// } else if (state.state_machine == SYSTEM_STATE_STABILIZED) {
/* set setpoint to current position */
// XXX select pos reset channel on remote
/* reset setpoint to current position (position hold) */
@@ -117,19 +216,18 @@ mpc_thread_main(int argc, char *argv[])
// local_pos_sp.z = local_pos.z;
// local_pos_sp.yaw = att.yaw;
// }
}
// }
/* run at approximately 50 Hz */
usleep(20000);
loopcounter++;
counter++;
}
/* close uarts */
close(ardrone_write);
ar_multiplexing_deinit(gpios);
printf("[multirotor pos control] ending now...\n");
thread_running = false;
printf("[multirotor pos control] ending now...\r\n");
fflush(stdout);
return 0;
}
apps/multirotor_pos_control/multirotor_pos_control_params.c
+62
View File
@@ -0,0 +1,62 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: Tobias Naegeli <naegelit@student.ethz.ch>
* 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 multirotor_position_control_params.c
*
* Parameters for EKF filter
*/
#include "multirotor_pos_control_params.h"
/* Extended Kalman Filter covariances */
/* controller parameters */
PARAM_DEFINE_FLOAT(MC_POS_P, 0.2f);
int parameters_init(struct multirotor_position_control_param_handles *h)
{
/* PID parameters */
h->p = param_find("MC_POS_P");
return OK;
}
int parameters_update(const struct multirotor_position_control_param_handles *h, struct multirotor_position_control_params *p)
{
param_get(h->p, &(p->p));
return OK;
}
apps/multirotor_pos_control/multirotor_pos_control_params.h
+66
View File
@@ -0,0 +1,66 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: Tobias Naegeli <naegelit@student.ethz.ch>
* 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 multirotor_position_control_params.h
*
* Parameters for position controller
*/
#include <systemlib/param/param.h>
struct multirotor_position_control_params {
float p;
float i;
float d;
};
struct multirotor_position_control_param_handles {
param_t p;
param_t i;
param_t d;
};
/**
* Initialize all parameter handles and values
*
*/
int parameters_init(struct multirotor_position_control_param_handles *h);
/**
* Update all parameters
*
*/
int parameters_update(const struct multirotor_position_control_param_handles *h, struct multirotor_position_control_params *p);
apps/multirotor_pos_control/position_control.c
+183 -183
View File
@@ -1,235 +1,235 @@
/****************************************************************************
*
* Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
* Author: @author Lorenz Meier <lm@inf.ethz.ch>
* @author Laurens Mackay <mackayl@student.ethz.ch>
* @author Tobias Naegeli <naegelit@student.ethz.ch>
* @author Martin Rutschmann <rutmarti@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.
*
****************************************************************************/
// /****************************************************************************
// *
// * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
// * Author: @author Lorenz Meier <lm@inf.ethz.ch>
// * @author Laurens Mackay <mackayl@student.ethz.ch>
// * @author Tobias Naegeli <naegelit@student.ethz.ch>
// * @author Martin Rutschmann <rutmarti@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 multirotor_position_control.c
* Implementation of the position control for a multirotor VTOL
*/
// /**
// * @file multirotor_position_control.c
// * Implementation of the position control for a multirotor VTOL
// */
#include <stdio.h>
#include <stdlib.h>
#include <stdio.h>
#include <stdint.h>
#include <math.h>
#include <stdbool.h>
#include <float.h>
#include <systemlib/pid/pid.h>
// #include <stdio.h>
// #include <stdlib.h>
// #include <stdio.h>
// #include <stdint.h>
// #include <math.h>
// #include <stdbool.h>
// #include <float.h>
// #include <systemlib/pid/pid.h>
#include "multirotor_position_control.h"
// #include "multirotor_position_control.h"
void control_multirotor_position(const struct vehicle_state_s *vstatus, const struct vehicle_manual_control_s *manual,
const struct vehicle_attitude_s *att, const struct vehicle_local_position_s *local_pos,
const struct vehicle_local_position_setpoint_s *local_pos_sp, struct vehicle_attitude_setpoint_s *att_sp)
{
static PID_t distance_controller;
// void control_multirotor_position(const struct vehicle_state_s *vstatus, const struct vehicle_manual_control_s *manual,
// const struct vehicle_attitude_s *att, const struct vehicle_local_position_s *local_pos,
// const struct vehicle_local_position_setpoint_s *local_pos_sp, struct vehicle_attitude_setpoint_s *att_sp)
// {
// static PID_t distance_controller;
static int read_ret;
static global_data_position_t position_estimated;
// static int read_ret;
// static global_data_position_t position_estimated;
static uint16_t counter;
// static uint16_t counter;
static bool initialized;
static uint16_t pm_counter;
// static bool initialized;
// static uint16_t pm_counter;
static float lat_next;
static float lon_next;
// static float lat_next;
// static float lon_next;
static float pitch_current;
// static float pitch_current;
static float thrust_total;
// static float thrust_total;
if (initialized == false) {
// if (initialized == false) {
pid_init(&distance_controller,
global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P],
global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I],
global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D],
global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU],
PID_MODE_DERIVATIV_CALC, 150);//150
// pid_init(&distance_controller,
// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P],
// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I],
// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D],
// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU],
// PID_MODE_DERIVATIV_CALC, 150);//150
// pid_pos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_LIM];
// pid_pos_z_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_Z_LIM];
// // pid_pos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_LIM];
// // pid_pos_z_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_Z_LIM];
thrust_total = 0.0f;
// thrust_total = 0.0f;
/* Position initialization */
/* Wait for new position estimate */
do {
read_ret = read_lock_position(&position_estimated);
} while (read_ret != 0);
// /* Position initialization */
// /* Wait for new position estimate */
// do {
// read_ret = read_lock_position(&position_estimated);
// } while (read_ret != 0);
lat_next = position_estimated.lat;
lon_next = position_estimated.lon;
// lat_next = position_estimated.lat;
// lon_next = position_estimated.lon;
/* attitude initialization */
global_data_lock(&global_data_attitude->access_conf);
pitch_current = global_data_attitude->pitch;
global_data_unlock(&global_data_attitude->access_conf);
// /* attitude initialization */
// global_data_lock(&global_data_attitude->access_conf);
// pitch_current = global_data_attitude->pitch;
// global_data_unlock(&global_data_attitude->access_conf);
initialized = true;
}
// initialized = true;
// }
/* load new parameters with 10Hz */
if (counter % 50 == 0) {
if (global_data_trylock(&global_data_parameter_storage->access_conf) == 0) {
/* check whether new parameters are available */
if (global_data_parameter_storage->counter > pm_counter) {
pid_set_parameters(&distance_controller,
global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P],
global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I],
global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D],
global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU]);
// /* load new parameters with 10Hz */
// if (counter % 50 == 0) {
// if (global_data_trylock(&global_data_parameter_storage->access_conf) == 0) {
// /* check whether new parameters are available */
// if (global_data_parameter_storage->counter > pm_counter) {
// pid_set_parameters(&distance_controller,
// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P],
// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I],
// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D],
// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU]);
//
// pid_pos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_LIM];
// pid_pos_z_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_Z_LIM];
// //
// // pid_pos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_LIM];
// // pid_pos_z_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_Z_LIM];
pm_counter = global_data_parameter_storage->counter;
printf("Position controller changed pid parameters\n");
}
}
// pm_counter = global_data_parameter_storage->counter;
// printf("Position controller changed pid parameters\n");
// }
// }
global_data_unlock(&global_data_parameter_storage->access_conf);
}
// global_data_unlock(&global_data_parameter_storage->access_conf);
// }
/* Wait for new position estimate */
do {
read_ret = read_lock_position(&position_estimated);
} while (read_ret != 0);
// /* Wait for new position estimate */
// do {
// read_ret = read_lock_position(&position_estimated);
// } while (read_ret != 0);
/* Get next waypoint */ //TODO: add local copy
// /* Get next waypoint */ //TODO: add local copy
if (0 == global_data_trylock(&global_data_position_setpoint->access_conf)) {
lat_next = global_data_position_setpoint->x;
lon_next = global_data_position_setpoint->y;
global_data_unlock(&global_data_position_setpoint->access_conf);
}
// if (0 == global_data_trylock(&global_data_position_setpoint->access_conf)) {
// lat_next = global_data_position_setpoint->x;
// lon_next = global_data_position_setpoint->y;
// global_data_unlock(&global_data_position_setpoint->access_conf);
// }
/* Get distance to waypoint */
float distance_to_waypoint = get_distance_to_next_waypoint(position_estimated.lat , position_estimated.lon, lat_next, lon_next);
// if(counter % 5 == 0)
// printf("distance_to_waypoint: %.4f\n", distance_to_waypoint);
// /* Get distance to waypoint */
// float distance_to_waypoint = get_distance_to_next_waypoint(position_estimated.lat , position_estimated.lon, lat_next, lon_next);
// // if(counter % 5 == 0)
// // printf("distance_to_waypoint: %.4f\n", distance_to_waypoint);
/* Get bearing to waypoint (direction on earth surface to next waypoint) */
float bearing = get_bearing_to_next_waypoint(position_estimated.lat, position_estimated.lon, lat_next, lon_next);
// /* Get bearing to waypoint (direction on earth surface to next waypoint) */
// float bearing = get_bearing_to_next_waypoint(position_estimated.lat, position_estimated.lon, lat_next, lon_next);
if (counter % 5 == 0)
printf("bearing: %.4f\n", bearing);
// if (counter % 5 == 0)
// printf("bearing: %.4f\n", bearing);
/* Calculate speed in direction of bearing (needed for controller) */
float speed_norm = sqrtf(position_estimated.vx * position_estimated.vx + position_estimated.vy * position_estimated.vy);
// if(counter % 5 == 0)
// printf("speed_norm: %.4f\n", speed_norm);
float speed_to_waypoint = 0; //(position_estimated.vx * cosf(bearing) + position_estimated.vy * sinf(bearing))/speed_norm; //FIXME, TODO: re-enable this once we have a full estimate of the speed, then we can do a PID for the distance controller
// /* Calculate speed in direction of bearing (needed for controller) */
// float speed_norm = sqrtf(position_estimated.vx * position_estimated.vx + position_estimated.vy * position_estimated.vy);
// // if(counter % 5 == 0)
// // printf("speed_norm: %.4f\n", speed_norm);
// float speed_to_waypoint = 0; //(position_estimated.vx * cosf(bearing) + position_estimated.vy * sinf(bearing))/speed_norm; //FIXME, TODO: re-enable this once we have a full estimate of the speed, then we can do a PID for the distance controller
/* Control Thrust in bearing direction */
float horizontal_thrust = -pid_calculate(&distance_controller, 0, distance_to_waypoint, speed_to_waypoint,
CONTROL_PID_POSITION_INTERVAL); //TODO: maybe this "-" sign is an error somewhere else
// /* Control Thrust in bearing direction */
// float horizontal_thrust = -pid_calculate(&distance_controller, 0, distance_to_waypoint, speed_to_waypoint,
// CONTROL_PID_POSITION_INTERVAL); //TODO: maybe this "-" sign is an error somewhere else
// if(counter % 5 == 0)
// printf("horizontal thrust: %.4f\n", horizontal_thrust);
// // if(counter % 5 == 0)
// // printf("horizontal thrust: %.4f\n", horizontal_thrust);
/* Get total thrust (from remote for now) */
if (0 == global_data_trylock(&global_data_rc_channels->access_conf)) {
thrust_total = (float)global_data_rc_channels->chan[THROTTLE].scale; //TODO: how should we use the RC_CHANNELS_FUNCTION enum?
global_data_unlock(&global_data_rc_channels->access_conf);
}
// /* Get total thrust (from remote for now) */
// if (0 == global_data_trylock(&global_data_rc_channels->access_conf)) {
// thrust_total = (float)global_data_rc_channels->chan[THROTTLE].scale; //TODO: how should we use the RC_CHANNELS_FUNCTION enum?
// global_data_unlock(&global_data_rc_channels->access_conf);
// }
const float max_gas = 500.0f;
thrust_total *= max_gas / 20000.0f; //TODO: check this
thrust_total += max_gas / 2.0f;
// const float max_gas = 500.0f;
// thrust_total *= max_gas / 20000.0f; //TODO: check this
// thrust_total += max_gas / 2.0f;
if (horizontal_thrust > thrust_total) {
horizontal_thrust = thrust_total;
// if (horizontal_thrust > thrust_total) {
// horizontal_thrust = thrust_total;
} else if (horizontal_thrust < -thrust_total) {
horizontal_thrust = -thrust_total;
}
// } else if (horizontal_thrust < -thrust_total) {
// horizontal_thrust = -thrust_total;
// }
//TODO: maybe we want to add a speed controller later...
// //TODO: maybe we want to add a speed controller later...
/* Calclulate thrust in east and north direction */
float thrust_north = cosf(bearing) * horizontal_thrust;
float thrust_east = sinf(bearing) * horizontal_thrust;
// /* Calclulate thrust in east and north direction */
// float thrust_north = cosf(bearing) * horizontal_thrust;
// float thrust_east = sinf(bearing) * horizontal_thrust;
if (counter % 10 == 0) {
printf("thrust north: %.4f\n", thrust_north);
printf("thrust east: %.4f\n", thrust_east);
fflush(stdout);
}
// if (counter % 10 == 0) {
// printf("thrust north: %.4f\n", thrust_north);
// printf("thrust east: %.4f\n", thrust_east);
// fflush(stdout);
// }
/* Get current attitude */
if (0 == global_data_trylock(&global_data_attitude->access_conf)) {
pitch_current = global_data_attitude->pitch;
global_data_unlock(&global_data_attitude->access_conf);
}
// /* Get current attitude */
// if (0 == global_data_trylock(&global_data_attitude->access_conf)) {
// pitch_current = global_data_attitude->pitch;
// global_data_unlock(&global_data_attitude->access_conf);
// }
/* Get desired pitch & roll */
float pitch_desired = 0.0f;
float roll_desired = 0.0f;
// /* Get desired pitch & roll */
// float pitch_desired = 0.0f;
// float roll_desired = 0.0f;
if (thrust_total != 0) {
float pitch_fraction = -thrust_north / thrust_total;
float roll_fraction = thrust_east / (cosf(pitch_current) * thrust_total);
// if (thrust_total != 0) {
// float pitch_fraction = -thrust_north / thrust_total;
// float roll_fraction = thrust_east / (cosf(pitch_current) * thrust_total);
if (roll_fraction < -1) {
roll_fraction = -1;
// if (roll_fraction < -1) {
// roll_fraction = -1;
} else if (roll_fraction > 1) {
roll_fraction = 1;
}
// } else if (roll_fraction > 1) {
// roll_fraction = 1;
// }
// if(counter % 5 == 0)
// {
// printf("pitch_fraction: %.4f, roll_fraction: %.4f\n",pitch_fraction, roll_fraction);
// fflush(stdout);
// }
// // if(counter % 5 == 0)
// // {
// // printf("pitch_fraction: %.4f, roll_fraction: %.4f\n",pitch_fraction, roll_fraction);
// // fflush(stdout);
// // }
pitch_desired = asinf(pitch_fraction);
roll_desired = asinf(roll_fraction);
}
// pitch_desired = asinf(pitch_fraction);
// roll_desired = asinf(roll_fraction);
// }
att_sp.roll = roll_desired;
att_sp.pitch = pitch_desired;
// att_sp.roll = roll_desired;
// att_sp.pitch = pitch_desired;
counter++;
}
// counter++;
// }
apps/multirotor_pos_control/position_control.h
+6 -6
View File
@@ -40,11 +40,11 @@
* Definition of the position control for a multirotor VTOL
*/
#ifndef POSITION_CONTROL_H_
#define POSITION_CONTROL_H_
// #ifndef POSITION_CONTROL_H_
// #define POSITION_CONTROL_H_
void control_multirotor_position(const struct vehicle_state_s *vstatus, const struct vehicle_manual_control_s *manual,
const struct vehicle_attitude_s *att, const struct vehicle_local_position_s *local_pos,
const struct vehicle_local_position_setpoint_s *local_pos_sp, struct vehicle_attitude_setpoint_s *att_sp);
// void control_multirotor_position(const struct vehicle_state_s *vstatus, const struct vehicle_manual_control_s *manual,
// const struct vehicle_attitude_s *att, const struct vehicle_local_position_s *local_pos,
// const struct vehicle_local_position_setpoint_s *local_pos_sp, struct vehicle_attitude_setpoint_s *att_sp);
#endif /* POSITION_CONTROL_H_ */
// #endif /* POSITION_CONTROL_H_ */
apps/nshlib/Kconfig
+43 -15
View File
@@ -291,19 +291,18 @@ config NSH_CONSOLE
console front-end is selected (/dev/console).
Normally, the serial console device is a UART and RS-232
interface. However, if CONFIG_USBDEV is defined, then a USB
interface. However, if USBDEV is defined, then a USB
serial device may, instead, be used if the one of
the following are defined:
CONFIG_PL2303 and CONFIG_PL2303_CONSOLE - Sets up the
Prolifics PL2303 emulation as a console device at /dev/console.
PL2303 and PL2303_CONSOLE - Set up the Prolifics PL2303
emulation as a console device at /dev/console.
CONFIG_CDCACM and CONFIG_CDCACM_CONSOLE - Sets up the
CDC/ACM serial device as a console device at dev/console.
CDCACM and CDCACM_CONSOLE - Set up the CDC/ACM serial
device as a console device at dev/console.
CONFIG_NSH_USBCONSOLE and CONFIG_NSH_USBCONDEV - Sets up the
some other USB serial device as the NSH console (not necessarily
dev/console).
NSH_USBCONSOLE and NSH_USBCONDEV - Sets up some other USB
serial device as the NSH console (not necessarily dev/console).
config NSH_USBCONSOLE
bool "Use a USB console"
@@ -311,20 +310,20 @@ config NSH_USBCONSOLE
depends on NSH_CONSOLE && USBDEV
---help---
If defined, then the an arbitrary USB device may be used
to as the NSH console. In this case, CONFIG_NSH_USBCONDEV
must be defined to indicate which USB device to use as
the console.
to as the NSH console. In this case, NSH_USBCONDEV must
be defined to indicate which USB device to use as the
console.
config NSH_USBCONDEV
string "USB console device"
default "/dev/ttyACM0"
depends on NSH_USBCONSOLE
---help---
If CONFIG_NSH_USBCONSOLE is set to 'y', then CONFIG_NSH_USBCONDEV
must also be set to select the USB device used to support
the NSH console. This should be set to the quoted name of a
If NSH_USBCONSOLE is set to 'y', then NSH_USBCONDEV must
also be set to select the USB device used to support the
NSH console. This should be set to the quoted name of a
readable/write-able USB driver such as:
CONFIG_NSH_USBCONDEV="/dev/ttyACM0".
NSH_USBCONDEV="/dev/ttyACM0".
config UBSDEV_MINOR
int "USB console device minor number"
@@ -448,6 +447,35 @@ config NSH_IOBUFFER_SIZE
---help---
Determines the size of the I/O buffer to use for sending/
receiving TELNET commands/reponses. Default: 512
config NSH_TELNET_LOGIN
bool "Telnet Login"
default n
---help---
If defined, then the Telnet user will be required to provide a
username and password to start the NSH shell.
if NSH_TELNET_LOGIN
config NSH_TELNET_USERNAME
string "Login Username"
default "admin"
---help---
Login user name. Default: "admin"
config NSH_TELNET_PASSWORD
string "Login Password"
default "nuttx"
---help---
Login password: Default: "nuttx"
config NSH_TELNET_FAILCOUNT
int "Login Retry Count"
default 3
---help---
Number of login retry attempts.
endif
endif
config NSH_DHCPC
apps/nshlib/nsh.h
+34 -1
View File
@@ -164,10 +164,19 @@
* Default: SCHED_PRIORITY_DEFAULT
* CONFIG_NSH_TELNETD_DAEMONSTACKSIZE - Stack size allocated for the
* Telnet daemon. Default: 2048
* CONFIG_NSH_TELNETD_CLIENTPRIO- Priority of the Telnet client.
* CONFIG_NSH_TELNETD_CLIENTPRIO - Priority of the Telnet client.
* Default: SCHED_PRIORITY_DEFAULT
* CONFIG_NSH_TELNETD_CLIENTSTACKSIZE - Stack size allocated for the
* Telnet client. Default: 2048
* CONFIG_NSH_TELNET_LOGIN - Support a simple Telnet login.
*
* If CONFIG_NSH_TELNET_LOGIN is defined, then these additional
* options may be specified:
*
* CONFIG_NSH_TELNET_USERNAME - Login user name. Default: "admin"
* CONFIG_NSH_TELNET_PASSWORD - Login password: Default: "nuttx"
* CONFIG_NSH_TELNET_FAILCOUNT - Number of login retry attempts.
* Default 3.
*/
#ifndef CONFIG_NSH_TELNETD_PORT
@@ -190,6 +199,22 @@
# define CONFIG_NSH_TELNETD_CLIENTSTACKSIZE 2048
#endif
#ifdef CONFIG_NSH_TELNET_LOGIN
# ifndef CONFIG_NSH_TELNET_USERNAME
# define CONFIG_NSH_TELNET_USERNAME "admin"
# endif
# ifndef CONFIG_NSH_TELNET_PASSWORD
# define CONFIG_NSH_TELNET_PASSWORD "nuttx"
# endif
# ifndef CONFIG_NSH_TELNET_FAILCOUNT
# define CONFIG_NSH_TELNET_FAILCOUNT 3
# endif
#endif /* CONFIG_NSH_TELNET_LOGIN */
/* Verify support for ROMFS /etc directory support options */
#ifdef CONFIG_NSH_ROMFSETC
@@ -364,6 +389,14 @@ typedef int (*cmd_t)(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv);
****************************************************************************/
extern const char g_nshgreeting[];
#if defined(CONFIG_NSH_TELNET_LOGIN) && defined(CONFIG_NSH_TELNET)
extern const char g_telnetgreeting[];
extern const char g_userprompt[];
extern const char g_passwordprompt[];
extern const char g_loginsuccess[];
extern const char g_badcredentials[];
extern const char g_loginfailure[];
#endif
extern const char g_nshprompt[];
extern const char g_nshsyntax[];
extern const char g_fmtargrequired[];
apps/nshlib/nsh_console.c
+10 -5
View File
@@ -75,12 +75,17 @@ struct serialsave_s
static FAR struct nsh_vtbl_s *nsh_consoleclone(FAR struct nsh_vtbl_s *vtbl);
#endif
static void nsh_consolerelease(FAR struct nsh_vtbl_s *vtbl);
static ssize_t nsh_consolewrite(FAR struct nsh_vtbl_s *vtbl, FAR const void *buffer, size_t nbytes);
static int nsh_consoleoutput(FAR struct nsh_vtbl_s *vtbl, const char *fmt, ...);
static ssize_t nsh_consolewrite(FAR struct nsh_vtbl_s *vtbl,
FAR const void *buffer, size_t nbytes);
static int nsh_consoleoutput(FAR struct nsh_vtbl_s *vtbl,
FAR const char *fmt, ...);
static FAR char *nsh_consolelinebuffer(FAR struct nsh_vtbl_s *vtbl);
static void nsh_consoleredirect(FAR struct nsh_vtbl_s *vtbl, int fd, FAR uint8_t *save);
static void nsh_consoleundirect(FAR struct nsh_vtbl_s *vtbl, FAR uint8_t *save);
static void nsh_consoleexit(FAR struct nsh_vtbl_s *vtbl, int exitstatus);
static void nsh_consoleredirect(FAR struct nsh_vtbl_s *vtbl, int fd,
FAR uint8_t *save);
static void nsh_consoleundirect(FAR struct nsh_vtbl_s *vtbl,
FAR uint8_t *save);
static void nsh_consoleexit(FAR struct nsh_vtbl_s *vtbl, int exitstatus)
noreturn_function;
/****************************************************************************
* Private Data
apps/nshlib/nsh_console.h
+1 -1
View File
@@ -109,7 +109,7 @@ struct nsh_vtbl_s
FAR char *(*linebuffer)(FAR struct nsh_vtbl_s *vtbl);
void (*redirect)(FAR struct nsh_vtbl_s *vtbl, int fd, FAR uint8_t *save);
void (*undirect)(FAR struct nsh_vtbl_s *vtbl, FAR uint8_t *save);
void (*exit)(FAR struct nsh_vtbl_s *vtbl, int exitstatus);
void (*exit)(FAR struct nsh_vtbl_s *vtbl, int exitstatus) noreturn_function;
/* Parser state data */
apps/nshlib/nsh_parse.c
+11
View File
@@ -395,6 +395,17 @@ const char g_nshgreeting[] = "\nNuttShell (NSH) NuttX-" CONFIG_VERSION_STR
const char g_nshgreeting[] = "\nNuttShell (NSH)\n";
#endif
/* Telnet login prompts */
#if defined(CONFIG_NSH_TELNET_LOGIN) && defined(CONFIG_NSH_TELNET)
const char g_telnetgreeting[] = "\nWelcome to NuttShell(NSH) Telnet Server...\n";
const char g_userprompt[] = "login: ";
const char g_passwordprompt[] = "password: ";
const char g_loginsuccess[] = "\nUser Logged-in!\n";
const char g_badcredentials[] = "\nInvalid username or password\n";
const char g_loginfailure[] = "Login failed!\n";
#endif
/* The NSH prompt */
const char g_nshprompt[] = "nsh> ";
apps/nshlib/nsh_telnetd.c
+109 -1
View File
@@ -43,6 +43,7 @@
#include <unistd.h>
#include <assert.h>
#include <debug.h>
#include <string.h>
#include <apps/netutils/telnetd.h>
@@ -55,6 +56,18 @@
* Pre-processor Definitions
****************************************************************************/
#ifdef CONFIG_NSH_TELNET_LOGIN
# define TELNET_IAC 255
# define TELNET_WILL 251
# define TELNET_WONT 252
# define TELNET_DO 253
# define TELNET_DONT 254
# define TELNET_USE_ECHO 1
# define TELNET_NOTUSE_ECHO 0
#endif /* CONFIG_NSH_TELNET_LOGIN */
/****************************************************************************
* Private Types
****************************************************************************/
@@ -75,6 +88,91 @@
* Private Functions
****************************************************************************/
/****************************************************************************
* Name: nsh_telnetecho
****************************************************************************/
#ifdef CONFIG_NSH_TELNET_LOGIN
void nsh_telnetecho(struct console_stdio_s *pstate, uint8_t is_use)
{
uint8_t optbuf[4];
optbuf[0] = TELNET_IAC;
optbuf[1] = (is_use == TELNET_USE_ECHO) ? TELNET_WILL : TELNET_DO;
optbuf[2] = 1;
optbuf[3] = 0;
fputs((char *)optbuf, pstate->cn_outstream);
fflush(pstate->cn_outstream);
}
#endif
/****************************************************************************
* Name: nsh_telnetlogin
****************************************************************************/
#ifdef CONFIG_NSH_TELNET_LOGIN
int nsh_telnetlogin(struct console_stdio_s *pstate)
{
char username[16];
char password[16];
uint8_t i;
/* Present the NSH Telnet greeting */
fputs(g_telnetgreeting, pstate->cn_outstream);
fflush(pstate->cn_outstream);
/* Loop for the configured number of retries */
for(i = 0; i < CONFIG_NSH_TELNET_FAILCOUNT; i++)
{
/* Ask for the login username */
fputs(g_userprompt, pstate->cn_outstream);
fflush(pstate->cn_outstream);
if (fgets(pstate->cn_line, CONFIG_NSH_LINELEN, INSTREAM(pstate)) != NULL)
{
strcpy(username, pstate->cn_line);
username[strlen(pstate->cn_line) - 1] = 0;
}
/* Ask for the login password */
fputs(g_passwordprompt, pstate->cn_outstream);
fflush(pstate->cn_outstream);
nsh_telnetecho(pstate, TELNET_NOTUSE_ECHO);
if (fgets(pstate->cn_line, CONFIG_NSH_LINELEN, INSTREAM(pstate)) != NULL)
{
/* Verify the username and password */
strcpy(password,pstate->cn_line);
password[strlen(pstate->cn_line) - 1] = 0;
if (strcmp(password, CONFIG_NSH_TELNET_PASSWORD) == 0 &&
strcmp(username, CONFIG_NSH_TELNET_USERNAME) == 0)
{
fputs(g_loginsuccess, pstate->cn_outstream);
fflush(pstate->cn_outstream);
nsh_telnetecho(pstate, TELNET_USE_ECHO);
return OK;
}
else
{
fputs(g_badcredentials, pstate->cn_outstream);
fflush(pstate->cn_outstream);
}
}
nsh_telnetecho(pstate, TELNET_USE_ECHO);
}
/* Too many failed login attempts */
fputs(g_loginfailure, pstate->cn_outstream);
fflush(pstate->cn_outstream);
return -1;
}
#endif /* CONFIG_NSH_TELNET_LOGIN */
/****************************************************************************
* Public Functions
****************************************************************************/
@@ -90,7 +188,17 @@ int nsh_telnetmain(int argc, char *argv[])
dbg("Session [%d] Started\n", getpid());
/* Present a greeting */
/* Login User and Password Check */
#ifdef CONFIG_NSH_TELNET_LOGIN
if (nsh_telnetlogin(pstate) != OK)
{
nsh_exit(&pstate->cn_vtbl, 1);
return -1; /* nsh_exit does not return */
}
#endif /* CONFIG_NSH_TELNET_LOGIN */
/* Present the NSH greeting */
fputs(g_nshgreeting, pstate->cn_outstream);
fflush(pstate->cn_outstream);
apps/position_estimator/Makefile
-2
View File
@@ -47,6 +47,4 @@ CSRCS = position_estimator_main.c \
codegen/rtGetInf.c \
codegen/rtGetNaN.c
INCLUDES = $(TOPDIR)/../mavlink/include/mavlink
include $(APPDIR)/mk/app.mk
apps/position_estimator/position_estimator_main.c
+85 -85
View File
@@ -35,8 +35,9 @@
*
****************************************************************************/
/*
* @file Model-identification based position estimator for multirotors
/**
* @file position_estimator_main.c
* Model-identification based position estimator for multirotors
*/
#include <nuttx/config.h>
@@ -45,7 +46,6 @@
#include <stdio.h>
#include <stdbool.h>
#include <fcntl.h>
#include <v1.0/common/mavlink.h>
#include <float.h>
#include <nuttx/sched.h>
#include <sys/prctl.h>
@@ -58,6 +58,7 @@
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_local_position.h>
#include <poll.h>
#include "codegen/position_estimator.h"
@@ -251,14 +252,16 @@ int position_estimator_main(int argc, char *argv[])
bool new_initialization = true;
static double lat_current = 0;//[°]] --> 47.0
static double lon_current = 0; //[°]] -->8.5
static double lat_current = 0.0d;//[°]] --> 47.0
static double lon_current = 0.0d; //[°]] -->8.5
float alt_current = 0.0f;
//TODO: handle flight without gps but with estimator
/* subscribe to vehicle status, attitude, gps */
struct vehicle_gps_position_s gps;
gps.fix_type = 0;
struct vehicle_status_s vstatus;
struct vehicle_attitude_s att;
@@ -269,7 +272,7 @@ int position_estimator_main(int argc, char *argv[])
int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
/* wait until gps signal turns valid, only then can we initialize the projection */
while (!gps_valid) {
while (gps.fix_type < 3) {
struct pollfd fds[1] = { {.fd = vehicle_gps_sub, .events = POLLIN} };
/* wait for GPS updates, BUT READ VEHICLE STATUS (!)
@@ -281,8 +284,8 @@ int position_estimator_main(int argc, char *argv[])
/* Wait for the GPS update to propagate (we have some time) */
usleep(5000);
/* Read wether the vehicle status changed */
orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus);
gps_valid = vstatus.gps_valid;
orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_sub, &gps);
gps_valid = (gps.fix_type > 2);
}
}
@@ -290,14 +293,18 @@ int position_estimator_main(int argc, char *argv[])
orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_sub, &gps);
lat_current = ((double)(gps.lat)) * 1e-7;
lon_current = ((double)(gps.lon)) * 1e-7;
alt_current = gps.alt * 1e-3;
/* initialize coordinates */
map_projection_init(lat_current, lon_current);
/* publish global position messages only after first GPS message */
struct vehicle_global_position_s global_pos = {
.lat = lat_current * 1e7,
.lon = lon_current * 1e7,
.alt = gps.alt
struct vehicle_local_position_s local_pos = {
.x = 0,
.y = 0,
.z = 0
};
orb_advert_t global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos);
orb_advert_t local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos);
printf("[multirotor position estimator] initialized projection with: lat: %.10f, lon:%.10f\n", lat_current, lon_current);
@@ -320,91 +327,84 @@ int position_estimator_main(int argc, char *argv[])
u[1] = att.pitch;
/* initialize map projection with the last estimate (not at full rate) */
if (counter % PROJECTION_INITIALIZE_COUNTER_LIMIT == 0) {
map_projection_init(lat_current, lon_current);
new_initialization = true;
} else {
new_initialization = false;
}
/*check if new gps values are available */
gps_valid = vstatus.gps_valid;
if (gps_valid) { //we are safe to use the gps signal (it has good quality)
predict_only = 0;
if (gps.fix_type > 2) {
/* Project gps lat lon (Geographic coordinate system) to plane*/
map_projection_project((double)(gps.lat) * 1e-7, (double)(gps.lon) * 1e-7, &(z[0]), &(z[1]));
map_projection_project(((double)(gps.lat)) * 1e-7, ((double)(gps.lon)) * 1e-7, &(z[0]), &(z[1]));
/* copy altitude */
z[2] = (gps.alt) * 1e-3;
gps_covariance[0] = gps.eph; //TODO: needs scaling
gps_covariance[1] = gps.eph;
gps_covariance[2] = gps.epv;
} else {
/* we can not use the gps signal (it is of low quality) */
predict_only = 1;
}
// predict_only = 0; //TODO: only for testing, removeme, XXX
// z[0] = sinf(((float)counter)/180.0f*3.14159265f); //TODO: only for testing, removeme, XXX
// usleep(100000); //TODO: only for testing, removeme, XXX
local_pos.x = z[0];
local_pos.y = z[1];
/* negative offset from initialization altitude */
local_pos.z = alt_current - (gps.alt) * 1e-3;
/*Get new estimation (this is calculated in the plane) */
//TODO: if new_initialization == true: use 0,0,0, else use xapo
if (true == new_initialization) { //TODO,XXX: uncomment!
xapo[0] = 0; //we have a new plane initialization. the current estimate is in the center of the plane
xapo[2] = 0;
xapo[4] = 0;
position_estimator(u, z, xapo, Papo, gps_covariance, predict_only, xapo1, Papo1);
} else {
position_estimator(u, z, xapo, Papo, gps_covariance, predict_only, xapo1, Papo1);
orb_publish(ORB_ID(vehicle_local_position), local_pos_pub, &local_pos);
}
// gps_covariance[0] = gps.eph; //TODO: needs scaling
// gps_covariance[1] = gps.eph;
// gps_covariance[2] = gps.epv;
/* Copy values from xapo1 to xapo */
int i;
// } else {
// /* we can not use the gps signal (it is of low quality) */
// predict_only = 1;
// }
for (i = 0; i < N_STATES; i++) {
xapo[i] = xapo1[i];
}
// // predict_only = 0; //TODO: only for testing, removeme, XXX
// // z[0] = sinf(((float)counter)/180.0f*3.14159265f); //TODO: only for testing, removeme, XXX
// // usleep(100000); //TODO: only for testing, removeme, XXX
if ((counter % REPROJECTION_COUNTER_LIMIT == 0) || (counter % (PROJECTION_INITIALIZE_COUNTER_LIMIT - 1) == 0)) {
/* Reproject from plane to geographic coordinate system */
// map_projection_reproject(xapo1[0], xapo1[2], map_scale, phi_1, lambda_0, &lat_current, &lon_current) //TODO,XXX: uncomment!
map_projection_reproject(z[0], z[1], &lat_current, &lon_current); //do not use estimator for projection testing, removeme
// //DEBUG
// if(counter%500 == 0)
// {
// printf("phi_1: %.10f\n", phi_1);
// printf("lambda_0: %.10f\n", lambda_0);
// printf("lat_estimated: %.10f\n", lat_current);
// printf("lon_estimated: %.10f\n", lon_current);
// printf("z[0]=%.10f, z[1]=%.10f, z[2]=%f\n", z[0], z[1], z[2]);
// fflush(stdout);
//
// }
// if(!isnan(lat_current) && !isnan(lon_current))// && !isnan(xapo1[4]) && !isnan(xapo1[1]) && !isnan(xapo1[3]) && !isnan(xapo1[5]))
// {
/* send out */
// /*Get new estimation (this is calculated in the plane) */
// //TODO: if new_initialization == true: use 0,0,0, else use xapo
// if (true == new_initialization) { //TODO,XXX: uncomment!
// xapo[0] = 0; //we have a new plane initialization. the current estimate is in the center of the plane
// xapo[2] = 0;
// xapo[4] = 0;
// position_estimator(u, z, xapo, Papo, gps_covariance, predict_only, xapo1, Papo1);
global_pos.lat = lat_current;
global_pos.lon = lon_current;
global_pos.alt = xapo1[4];
global_pos.vx = xapo1[1];
global_pos.vy = xapo1[3];
global_pos.vz = xapo1[5];
// } else {
// position_estimator(u, z, xapo, Papo, gps_covariance, predict_only, xapo1, Papo1);
// }
// /* Copy values from xapo1 to xapo */
// int i;
// for (i = 0; i < N_STATES; i++) {
// xapo[i] = xapo1[i];
// }
// if ((counter % REPROJECTION_COUNTER_LIMIT == 0) || (counter % (PROJECTION_INITIALIZE_COUNTER_LIMIT - 1) == 0)) {
// /* Reproject from plane to geographic coordinate system */
// // map_projection_reproject(xapo1[0], xapo1[2], map_scale, phi_1, lambda_0, &lat_current, &lon_current) //TODO,XXX: uncomment!
// map_projection_reproject(z[0], z[1], &lat_current, &lon_current); //do not use estimator for projection testing, removeme
// // //DEBUG
// // if(counter%500 == 0)
// // {
// // printf("phi_1: %.10f\n", phi_1);
// // printf("lambda_0: %.10f\n", lambda_0);
// // printf("lat_estimated: %.10f\n", lat_current);
// // printf("lon_estimated: %.10f\n", lon_current);
// // printf("z[0]=%.10f, z[1]=%.10f, z[2]=%f\n", z[0], z[1], z[2]);
// // fflush(stdout);
// //
// // }
// // if(!isnan(lat_current) && !isnan(lon_current))// && !isnan(xapo1[4]) && !isnan(xapo1[1]) && !isnan(xapo1[3]) && !isnan(xapo1[5]))
// // {
// /* send out */
// global_pos.lat = lat_current;
// global_pos.lon = lon_current;
// global_pos.alt = xapo1[4];
// global_pos.vx = xapo1[1];
// global_pos.vy = xapo1[3];
// global_pos.vz = xapo1[5];
/* publish current estimate */
orb_publish(ORB_ID(vehicle_global_position), global_pos_pub, &global_pos);
// orb_publish(ORB_ID(vehicle_global_position), global_pos_pub, &global_pos);
// }
// else
// {
@@ -412,7 +412,7 @@ int position_estimator_main(int argc, char *argv[])
// fflush(stdout);
// }
}
// }
counter++;
}
apps/px4/fmu/fmu.cpp
+35 -18
View File
@@ -79,7 +79,7 @@ public:
FMUServo(Mode mode, int update_rate);
~FMUServo();
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
virtual int ioctl(file *filp, int cmd, unsigned long arg);
virtual int init();
@@ -93,6 +93,7 @@ private:
int _t_armed;
orb_advert_t _t_outputs;
unsigned _num_outputs;
bool _primary_pwm_device;
volatile bool _task_should_exit;
bool _armed;
@@ -118,7 +119,7 @@ FMUServo *g_servo;
} // namespace
FMUServo::FMUServo(Mode mode, int update_rate) :
CDev("fmuservo", PWM_OUTPUT_DEVICE_PATH),
CDev("fmuservo", "/dev/px4fmu"),
_mode(mode),
_update_rate(update_rate),
_task(-1),
@@ -126,6 +127,7 @@ FMUServo::FMUServo(Mode mode, int update_rate) :
_t_armed(-1),
_t_outputs(0),
_num_outputs(0),
_primary_pwm_device(false),
_task_should_exit(false),
_armed(false),
_mixers(nullptr)
@@ -135,18 +137,16 @@ FMUServo::FMUServo(Mode mode, int update_rate) :
FMUServo::~FMUServo()
{
if (_task != -1) {
/* task should wake up every 100ms or so at least */
/* tell the task we want it to go away */
_task_should_exit = true;
unsigned i = 0;
unsigned i = 10;
do {
/* wait 20ms */
usleep(20000);
/* wait 50ms - it should wake every 100ms or so worst-case */
usleep(50000);
/* if we have given up, kill it */
if (++i > 10) {
if (--i == 0) {
task_delete(_task);
break;
}
@@ -154,6 +154,10 @@ FMUServo::~FMUServo()
} while (_task != -1);
}
/* clean up the alternate device node */
if (_primary_pwm_device)
unregister_driver(PWM_OUTPUT_DEVICE_PATH);
g_servo = nullptr;
}
@@ -170,9 +174,16 @@ FMUServo::init()
if (ret != OK)
return ret;
/* try to claim the generic PWM output device node as well - it's OK if we fail at this */
ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this);
if (ret == OK) {
log("default PWM output device");
_primary_pwm_device = true;
}
/* start the IO interface task */
_task = task_spawn("fmuservo",
SCHED_RR,
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
1024,
(main_t)&FMUServo::task_main_trampoline,
@@ -216,8 +227,12 @@ FMUServo::task_main()
break;
}
/* subscribe to objects that we are interested in watching */
_t_actuators = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
/*
* Subscribe to the appropriate PWM output topic based on whether we are the
* primary PWM output or not.
*/
_t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
ORB_ID(actuator_controls_1));
/* convert the update rate in hz to milliseconds, rounding down if necessary */
int update_rate_in_ms = int(1000 / _update_rate);
orb_set_interval(_t_actuators, update_rate_in_ms);
@@ -226,11 +241,13 @@ FMUServo::task_main()
orb_set_interval(_t_armed, 200); /* 5Hz update rate */
/* advertise the mixed control outputs */
struct actuator_outputs_s outputs;
actuator_outputs_s outputs;
memset(&outputs, 0, sizeof(outputs));
_t_outputs = orb_advertise(ORB_ID_VEHICLE_CONTROLS, &outputs);
/* advertise the mixed control outputs */
_t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1),
&outputs);
struct pollfd fds[2];
pollfd fds[2];
fds[0].fd = _t_actuators;
fds[0].events = POLLIN;
fds[1].fd = _t_armed;
@@ -282,7 +299,7 @@ FMUServo::task_main()
/* how about an arming update? */
if (fds[1].revents & POLLIN) {
struct actuator_armed_s aa;
actuator_armed_s aa;
/* get new value */
orb_copy(ORB_ID(actuator_armed), _t_armed, &aa);
@@ -320,7 +337,7 @@ FMUServo::control_callback(uintptr_t handle,
}
int
FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg)
FMUServo::ioctl(file *filp, int cmd, unsigned long arg)
{
int ret = OK;
int channel;
@@ -569,7 +586,7 @@ fake(int argc, char *argv[])
exit(1);
}
struct actuator_controls_s ac;
actuator_controls_s ac;
ac.control[0] = strtol(argv[1], 0, 0) / 100.0f;
apps/px4/px4io/driver/px4io.cpp
+306 -259
View File
@@ -37,8 +37,7 @@
*
* 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>
@@ -53,6 +52,7 @@
#include <errno.h>
#include <string.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <fcntl.h>
@@ -61,129 +61,21 @@
#include <drivers/device/device.h>
#include <drivers/drv_rc_input.h>
#include <drivers/drv_pwm_output.h>
#include <systemlib/mixer/mixer.h>
#include <drivers/drv_mixer.h>
#include <systemlib/perf_counter.h>
#include <systemlib/hx_stream.h>
#include <systemlib/err.h>
#include <systemlib/systemlib.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/rc_channels.h>
#include "px4io/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:
orb_advert_t _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
{
@@ -193,53 +85,98 @@ public:
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);
virtual int ioctl(file *filp, int cmd, unsigned long arg);
private:
int _fd;
int _task;
PX4IO_RC *_rc;
static const unsigned _max_actuators = PX4IO_OUTPUT_CHANNELS;
/** command to be sent to IO */
struct px4io_command _next_command;
int _serial_fd; ///< serial interface to PX4IO
hx_stream_t _io_stream; ///< HX protocol stream
/** RC channel input from IO */
servo_position_t _rc_channel[PX4IO_INPUT_CHANNELS];
int _rc_channel_count;
volatile bool _armed;
int _task; ///< worker task
volatile bool _task_should_exit;
bool _send_needed;
int _t_actuators; ///< actuator output topic
actuator_controls_s _controls; ///< actuator outputs
hx_stream_t _io_stream;
int _t_armed; ///< system armed control topic
actuator_armed_s _armed; ///< system armed state
orb_advert_t _t_outputs; ///< mixed outputs topic
actuator_outputs_s _outputs; ///< mixed outputs
MixerGroup *_mixers; ///< loaded mixers
bool _primary_pwm_device; ///< true if we are the default PWM output
volatile bool _switch_armed; ///< PX4IO switch armed state
// XXX how should this work?
bool _send_needed; ///< If true, we need to send a packet to IO
/**
* Trampoline to the worker task
*/
static void task_main_trampoline(int argc, char *argv[]);
/**
* worker task
*/
void task_main();
/**
* Handle receiving bytes from PX4IO
*/
void io_recv();
/**
* HX protocol callback trampoline.
*/
static void rx_callback_trampoline(void *arg, const void *buffer, size_t bytes_received);
/**
* Callback invoked when we receive a whole packet from PX4IO
*/
void rx_callback(const uint8_t *buffer, size_t bytes_received);
/**
* Send an update packet to PX4IO
*/
void io_send();
/**
* Mixer control callback; invoked to fetch a control from a specific
* group/index during mixing.
*/
static int control_callback(uintptr_t handle,
uint8_t control_group,
uint8_t control_index,
float &input);
};
namespace
{
PX4IO *g_dev;
}
PX4IO::PX4IO() :
CDev("px4io", "/dev/px4io"),
_fd(-1),
_serial_fd(-1),
_io_stream(nullptr),
_task(-1),
_rc(new PX4IO_RC),
_rc_channel_count(0),
_armed(false),
_task_should_exit(false),
_send_needed(false),
_io_stream(nullptr)
_t_actuators(-1),
_t_armed(-1),
_t_outputs(-1),
_mixers(nullptr),
_primary_pwm_device(false),
_switch_armed(false),
_send_needed(false)
{
/* 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 */
/* we need this potentially before it could be set in task_main */
g_dev = this;
_debug_enabled = true;
@@ -247,19 +184,18 @@ PX4IO::PX4IO() :
PX4IO::~PX4IO()
{
if (_rc != nullptr)
delete _rc;
if (_task != -1) {
/* task should wake up every 100ms or so at least */
/* tell the task we want it to go away */
_task_should_exit = true;
unsigned i = 0;
/* spin waiting for the thread to stop */
unsigned i = 10;
do {
/* wait 20ms */
usleep(20000);
/* wait 50ms - it should wake every 100ms or so worst-case */
usleep(50000);
/* if we have given up, kill it */
if (++i > 10) {
if (--i == 0) {
task_delete(_task);
break;
}
@@ -267,6 +203,14 @@ PX4IO::~PX4IO()
} while (_task != -1);
}
/* clean up the alternate device node */
if (_primary_pwm_device)
unregister_driver(PWM_OUTPUT_DEVICE_PATH);
/* kill the HX stream */
if (_io_stream != nullptr)
hx_stream_free(_io_stream);
g_dev = nullptr;
}
@@ -277,17 +221,20 @@ PX4IO::init()
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;
/* try to claim the generic PWM output device node as well - it's OK if we fail at this */
ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this);
if (ret == OK) {
log("default PWM output device");
_primary_pwm_device = true;
}
/* start the IO interface task */
_task = task_create("px4io", SCHED_PRIORITY_DEFAULT, 1024, (main_t)&PX4IO::task_main_trampoline, nullptr);
_task = task_create("px4io", SCHED_PRIORITY_DEFAULT, 4096, (main_t)&PX4IO::task_main_trampoline, nullptr);
if (_task < 0) {
debug("task start failed: %d", errno);
return -errno;
@@ -305,36 +252,61 @@ PX4IO::task_main_trampoline(int argc, char *argv[])
void
PX4IO::task_main()
{
ASSERT(_fd == -1);
log("ready");
log("starting");
/* open the serial port */
_fd = ::open("/dev/ttyS2", O_RDWR | O_NONBLOCK);
if (_fd < 0) {
_serial_fd = ::open("/dev/ttyS2", O_RDWR);
if (_serial_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);
_io_stream = hx_stream_init(_serial_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_tx_frames = perf_alloc(PC_COUNT, "PX4IO frames transmitted");
perf_counter_t pc_rx_frames = 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);
hx_stream_set_counters(_io_stream, pc_tx_frames, pc_rx_frames, pc_rx_errors);
/* poll descriptor(s) */
struct pollfd fds[1];
fds[0].fd = _fd;
/* XXX send a who-are-you request */
/* XXX verify firmware/protocol version */
/*
* Subscribe to the appropriate PWM output topic based on whether we are the
* primary PWM output or not.
*/
_t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
ORB_ID(actuator_controls_1));
/* convert the update rate in hz to milliseconds, rounding down if necessary */
//int update_rate_in_ms = int(1000 / _update_rate);
orb_set_interval(_t_actuators, 20); /* XXX 50Hz hardcoded for now */
_t_armed = orb_subscribe(ORB_ID(actuator_armed));
orb_set_interval(_t_armed, 200); /* 5Hz update rate */
/* advertise the mixed control outputs */
_t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1),
&_outputs);
/* poll descriptor */
pollfd fds[3];
fds[0].fd = _serial_fd;
fds[0].events = POLLIN;
fds[1].fd = _t_actuators;
fds[1].events = POLLIN;
fds[2].fd = _t_armed;
fds[2].events = POLLIN;
log("ready");
/* 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);
int ret = ::poll(&fds[0], sizeof(fds) / sizeof(fds[0]), 1000);
/* this would be bad... */
if (ret < 0) {
@@ -347,9 +319,36 @@ PX4IO::task_main()
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();
if (ret > 0) {
/* if we have new data from IO, go handle it */
if (fds[0].revents & POLLIN)
io_recv();
/* if we have new data from the ORB, go handle it */
if (fds[1].revents & POLLIN) {
/* get controls */
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls);
/* mix */
if (_mixers != nullptr) {
/* XXX is this the right count? */
_mixers->mix(&_outputs.output[0], _max_actuators);
/* convert to PWM values */
for (unsigned i = 0; i < _max_actuators; i++)
_outputs.output[i] = 1500 + (600 * _outputs.output[i]);
/* and flag for update */
_send_needed = true;
}
}
if (fds[2].revents & POLLIN) {
orb_copy(ORB_ID(actuator_armed), _t_armed, &_controls);
_send_needed = true;
}
}
/* send an update to IO if required */
if (_send_needed) {
@@ -357,23 +356,40 @@ PX4IO::task_main()
io_send();
}
}
if (_io_stream != nullptr)
hx_stream_free(_io_stream);
::close(_fd);
/* tell the dtor that we are exiting */
_task = -1;
_exit(0);
}
int
PX4IO::control_callback(uintptr_t handle,
uint8_t control_group,
uint8_t control_index,
float &input)
{
const actuator_controls_s *controls = (actuator_controls_s *)handle;
input = controls->control[control_index];
return 0;
}
void
PX4IO::io_recv()
{
uint8_t c;
uint8_t buf[32];
int count;
/* handle bytes from IO */
while (::read(_fd, &c, 1) == 1)
hx_stream_rx(_io_stream, c);
/*
* We are here because poll says there is some data, so this
* won't block even on a blocking device. If more bytes are
* available, we'll go back to poll() again...
*/
count = ::read(_serial_fd, buf, sizeof(buf));
/* pass received bytes to the packet decoder */
for (int i = 0; i < count; i++)
hx_stream_rx(_io_stream, buf[i]);
}
void
@@ -385,98 +401,139 @@ PX4IO::rx_callback_trampoline(void *arg, const void *buffer, size_t bytes_receiv
void
PX4IO::rx_callback(const uint8_t *buffer, size_t bytes_received)
{
const struct px4io_report *rep = (const struct px4io_report *)buffer;
const px4io_report *rep = (const px4io_report *)buffer;
/* sanity-check the received frame size */
if (bytes_received != sizeof(struct px4io_report))
if (bytes_received != sizeof(px4io_report))
return;
lock();
/* XXX handle R/C inputs here ... needs code sharing/library */
/* 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;
/* remember the latched arming switch state */
_switch_armed = rep->armed;
unlock();
}
void
PX4IO::io_send()
{
lock();
px4io_command cmd;
/* send packet to IO while we're guaranteed it won't change */
hx_stream_send(_io_stream, &_next_command, sizeof(_next_command));
cmd.f2i_magic = F2I_MAGIC;
unlock();
}
/* set outputs */
for (unsigned i = 0; i < _max_actuators; i++)
cmd.servo_command[i] = _outputs.output[i];
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;
/* publish as we send */
orb_publish(ORB_ID_VEHICLE_CONTROLS, _t_outputs, &_outputs);
if (channels > PX4IO_OUTPUT_CHANNELS)
return -EIO;
// XXX relays
lock();
for (i = 0; i < channels; i++)
_next_command.servo_command[i] = pdata[i];
unlock();
cmd.arm_ok = _armed.armed;
return i * sizeof(servo_position_t);
hx_stream_send(_io_stream, &cmd, sizeof(cmd));
}
int
PX4IO::ioctl(struct file *filep, int cmd, unsigned long arg)
PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
{
int ret = -ENOTTY;
int ret = OK;
lock();
/* regular ioctl? */
switch (cmd) {
case PWM_SERVO_ARM:
_next_command.arm_ok = true;
ret = 0;
/* fake an armed transition */
_armed.armed = true;
_send_needed = true;
break;
case PWM_SERVO_DISARM:
_next_command.arm_ok = false;
ret = 0;
/* fake a disarmed transition */
_armed.armed = true;
_send_needed = true;
break;
case PWM_SERVO_SET(0) ... PWM_SERVO_SET(_max_actuators - 1):
/* fake an update to the selected servo channel */
if ((arg >= 900) && (arg <= 2100)) {
_outputs.output[cmd - PWM_SERVO_SET(0)] = arg;
_send_needed = true;
} else {
ret = -EINVAL;
}
break;
case PWM_SERVO_GET(0) ... PWM_SERVO_GET(_max_actuators - 1):
/* copy the current output value from the channel */
*(servo_position_t *)arg = _outputs.output[cmd - PWM_SERVO_GET(0)];
break;
case MIXERIOCGETOUTPUTCOUNT:
*(unsigned *)arg = _max_actuators;
break;
case MIXERIOCRESET:
if (_mixers != nullptr) {
delete _mixers;
_mixers = nullptr;
}
break;
case MIXERIOCADDSIMPLE: {
mixer_simple_s *mixinfo = (mixer_simple_s *)arg;
/* build the new mixer from the supplied argument */
SimpleMixer *mixer = new SimpleMixer(control_callback,
(uintptr_t)&_controls, mixinfo);
/* validate the new mixer */
if (mixer->check()) {
delete mixer;
ret = -EINVAL;
} else {
/* if we don't have a group yet, allocate one */
if (_mixers == nullptr)
_mixers = new MixerGroup(control_callback,
(uintptr_t)&_controls);
/* add the new mixer to the group */
_mixers->add_mixer(mixer);
}
}
break;
case MIXERIOCADDMULTIROTOR:
/* XXX not yet supported */
ret = -ENOTTY;
break;
case MIXERIOCLOADFILE: {
MixerGroup *newmixers;
const char *path = (const char *)arg;
/* allocate a new mixer group and load it from the file */
newmixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
if (newmixers->load_from_file(path) != 0) {
delete newmixers;
ret = -EINVAL;
}
/* swap the new mixers in for the old */
if (_mixers != nullptr) {
delete _mixers;
}
_mixers = newmixers;
}
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;
}
@@ -492,28 +549,25 @@ px4io_main(int argc, char *argv[])
{
if (!strcmp(argv[1], "start")) {
if (g_dev != nullptr) {
fprintf(stderr, "PX4IO: already loaded\n");
return -EBUSY;
}
if (g_dev != nullptr)
errx(1, "already loaded");
/* 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 (g_dev == nullptr)
errx(1, "driver alloc failed");
if (OK != g_dev->init()) {
fprintf(stderr, "PX4IO: driver init failed\n");
delete g_dev;
return -EIO;
errx(1, "driver init failed");
}
return OK;
exit(0);
}
/* note, stop not currently implemented */
if (!strcmp(argv[1], "update")) {
PX4IO_Uploader *up;
const char *fn[3];
@@ -536,26 +590,19 @@ px4io_main(int argc, char *argv[])
case OK:
break;
case -ENOENT:
fprintf(stderr, "PX4IO firmware file not found\n");
break;
errx(1, "PX4IO firmware file not found");
case -EEXIST:
case -EIO:
fprintf(stderr, "error updating PX4IO - check that bootloader mode is enabled\n");
break;
errx(1, "error updating PX4IO - check that bootloader mode is enabled");
case -EINVAL:
fprintf(stderr, "verify failed - retry the update\n");
break;
errx(1, "verify failed - retry the update");
case -ETIMEDOUT:
fprintf(stderr, "timed out waiting for bootloader - power-cycle and try again\n");
errx(1, "timed out waiting for bootloader - power-cycle and try again");
default:
fprintf(stderr, "unexpected error %d\n", ret);
break;
errx(1, "unexpected error %d", ret);
}
return ret;
}
printf("need a verb, only support 'start' and 'update'\n");
return ERROR;
errx(1, "need a verb, only support 'start' and 'update'");
}
apps/px4/px4io/driver/uploader.cpp
+1 -1
View File
@@ -318,7 +318,7 @@ PX4IO_Uploader::verify()
ret = recv(c);
if (ret != OK) {
log("%d: got %d waiting for bytes", ret);
log("%d: got %d waiting for bytes", base + i, ret);
return ret;
}
apps/px4/tests/test_sensors.c
-330
View File
@@ -56,10 +56,6 @@
#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 <drivers/drv_accel.h>
/****************************************************************************
@@ -75,10 +71,6 @@
****************************************************************************/
//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[]);
/****************************************************************************
@@ -90,12 +82,7 @@ struct {
const char *path;
int (* test)(int argc, char *argv[]);
} sensors[] = {
{"bma180", "/dev/bma180", bma180},
{"mpu6000", "/dev/accel", mpu6000},
{"l3gd20", "/dev/l3gd20", l3gd20},
{"hmc5883l", "/dev/hmc5883l", hmc5883l},
{"ms5611", "/dev/ms5611", ms5611},
// {"lis331", "/dev/lis331", lis331},
{NULL, NULL, NULL}
};
@@ -107,226 +94,6 @@ struct {
* 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[])
{
// XXX THIS SENSOR IS OBSOLETE
// TEST REMAINS, BUT ALWAYS RETURNS OK
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 OK;
}
// 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 OK;
} 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 OK;
} 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[])
{
@@ -379,103 +146,6 @@ mpu6000(int argc, char *argv[])
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 ((int8_t)ret == -EAGAIN || (int8_t)ret == -EINPROGRESS) {
/* 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[7] = {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
****************************************************************************/
apps/px4io/protocol.h
+2
View File
@@ -39,6 +39,8 @@
* messages and the corresponding complexity involved.
*/
#pragma once
/*
* XXX MUST BE KEPT IN SYNC WITH THE VERSION IN PX4FMU UNTIL
* TREES ARE MERGED.
apps/sdlog/sdlog.c
+38 -3
View File
@@ -60,6 +60,9 @@
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <systemlib/systemlib.h>
@@ -106,7 +109,7 @@ usage(const char *reason)
* Makefile does only apply to this management task.
*
* The actual stack size should be set in the call
* to task_create().
* to task_spawn().
*/
int sdlog_main(int argc, char *argv[])
{
@@ -123,7 +126,7 @@ int sdlog_main(int argc, char *argv[])
thread_should_exit = false;
deamon_task = task_spawn("sdlog",
SCHED_RR,
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT - 30,
4096,
sdlog_thread_main,
@@ -273,6 +276,9 @@ int sdlog_thread_main(int argc, char *argv[]) {
struct actuator_outputs_s act_outputs;
struct actuator_controls_s act_controls;
struct vehicle_command_s cmd;
struct vehicle_local_position_s local_pos;
struct vehicle_global_position_s global_pos;
struct vehicle_gps_position_s gps_pos;
} buf;
memset(&buf, 0, sizeof(buf));
@@ -283,6 +289,9 @@ int sdlog_thread_main(int argc, char *argv[]) {
int spa_sub;
int act_0_sub;
int controls0_sub;
int local_pos_sub;
int global_pos_sub;
int gps_pos_sub;
} subs;
/* --- MANAGEMENT - LOGGING COMMAND --- */
@@ -329,6 +338,27 @@ int sdlog_thread_main(int argc, char *argv[]) {
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- LOCAL POSITION --- */
/* subscribe to ORB for local position */
subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
fds[fdsc_count].fd = subs.local_pos_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- GLOBAL POSITION --- */
/* subscribe to ORB for global position */
subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
fds[fdsc_count].fd = subs.global_pos_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- GPS POSITION --- */
/* subscribe to ORB for global position */
subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
fds[fdsc_count].fd = subs.gps_pos_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* WARNING: If you get the error message below,
* then the number of registered messages (fdsc)
* differs from the number of messages in the above list.
@@ -435,6 +465,9 @@ int sdlog_thread_main(int argc, char *argv[]) {
/* copy actuator data into local buffer */
orb_copy(ORB_ID(actuator_outputs_0), subs.act_0_sub, &buf.act_outputs);
orb_copy(ORB_ID(vehicle_attitude_setpoint), subs.spa_sub, &buf.att_sp);
orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos);
orb_copy(ORB_ID(vehicle_local_position), subs.local_pos_sub, &buf.local_pos);
orb_copy(ORB_ID(vehicle_global_position), subs.global_pos_sub, &buf.global_pos);
#pragma pack(push, 1)
struct {
@@ -450,6 +483,7 @@ int sdlog_thread_main(int argc, char *argv[]) {
float actuators[8];
float vbat;
float adc[3];
float local_pos[3];
} sysvector = {
.timestamp = buf.raw.timestamp,
.gyro = {buf.raw.gyro_rad_s[0], buf.raw.gyro_rad_s[1], buf.raw.gyro_rad_s[2]},
@@ -462,7 +496,8 @@ int sdlog_thread_main(int argc, char *argv[]) {
.actuators = {buf.act_outputs.output[0], buf.act_outputs.output[1], buf.act_outputs.output[2], buf.act_outputs.output[3],
buf.act_outputs.output[4], buf.act_outputs.output[5], buf.act_outputs.output[6], buf.act_outputs.output[7]},
.vbat = buf.raw.battery_voltage_v,
.adc = {buf.raw.adc_voltage_v[0], buf.raw.adc_voltage_v[1], buf.raw.adc_voltage_v[2]}
.adc = {buf.raw.adc_voltage_v[0], buf.raw.adc_voltage_v[1], buf.raw.adc_voltage_v[2]},
.local_pos = {buf.local_pos.x, buf.local_pos.y, buf.local_pos.z}
};
#pragma pack(pop)
apps/sensors/sensor_params.c
+3 -3
View File
@@ -56,9 +56,9 @@ PARAM_DEFINE_FLOAT(SENS_ACC_XOFF, 0.0f);
PARAM_DEFINE_FLOAT(SENS_ACC_YOFF, 0.0f);
PARAM_DEFINE_FLOAT(SENS_ACC_ZOFF, 0.0f);
PARAM_DEFINE_FLOAT(SENS_ACC_XSCALE, 0.0f);
PARAM_DEFINE_FLOAT(SENS_ACC_YSCALE, 0.0f);
PARAM_DEFINE_FLOAT(SENS_ACC_ZSCALE, 0.0f);
PARAM_DEFINE_FLOAT(SENS_ACC_XSCALE, 1.0f);
PARAM_DEFINE_FLOAT(SENS_ACC_YSCALE, 1.0f);
PARAM_DEFINE_FLOAT(SENS_ACC_ZSCALE, 1.0f);
PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f);
PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f);
apps/sensors/sensors.cpp
+27 -105
View File
@@ -101,8 +101,6 @@ extern "C" {
/* PPM Settings */
# define PPM_MIN 1000
# define PPM_MAX 2000
/* Internal resolution is 10000 */
# define PPM_SCALE 10000/((PPM_MAX-PPM_MIN)/2)
# define PPM_MID (PPM_MIN+PPM_MAX)/2
#endif
@@ -136,10 +134,6 @@ public:
private:
static const unsigned _rc_max_chan_count = 8; /**< maximum number of r/c channels we handle */
/* legacy sensor descriptors */
int _fd_bma180; /**< old accel driver */
int _fd_gyro_l3gd20; /**< old gyro driver */
#if CONFIG_HRT_PPM
hrt_abstime _ppm_last_valid; /**< last time we got a valid ppm signal */
@@ -334,8 +328,6 @@ Sensors *g_sensors;
}
Sensors::Sensors() :
_fd_bma180(-1),
_fd_gyro_l3gd20(-1),
_ppm_last_valid(0),
_fd_adc(-1),
@@ -562,19 +554,7 @@ Sensors::accel_init()
fd = open(ACCEL_DEVICE_PATH, 0);
if (fd < 0) {
warn("%s", ACCEL_DEVICE_PATH);
/* fall back to bma180 here (new driver would be better...) */
_fd_bma180 = open("/dev/bma180", O_RDONLY);
if (_fd_bma180 < 0) {
warn("/dev/bma180");
warn("FATAL: no accelerometer found");
}
/* discard first (junk) reading */
int16_t junk_buf[3];
read(_fd_bma180, junk_buf, sizeof(junk_buf));
warnx("using BMA180");
errx(1, "FATAL: no accelerometer found");
} else {
/* set the accel internal sampling rate up to at leat 500Hz */
ioctl(fd, ACCELIOCSSAMPLERATE, 500);
@@ -595,19 +575,7 @@ Sensors::gyro_init()
fd = open(GYRO_DEVICE_PATH, 0);
if (fd < 0) {
warn("%s", GYRO_DEVICE_PATH);
/* fall back to bma180 here (new driver would be better...) */
_fd_gyro_l3gd20 = open("/dev/l3gd20", O_RDONLY);
if (_fd_gyro_l3gd20 < 0) {
warn("/dev/l3gd20");
warn("FATAL: no gyro found");
}
/* discard first (junk) reading */
int16_t junk_buf[3];
read(_fd_gyro_l3gd20, junk_buf, sizeof(junk_buf));
warn("using L3GD20");
errx(1, "FATAL: no gyro found");
} else {
/* set the gyro internal sampling rate up to at leat 500Hz */
ioctl(fd, GYROIOCSSAMPLERATE, 500);
@@ -648,7 +616,7 @@ Sensors::baro_init()
fd = open(BARO_DEVICE_PATH, 0);
if (fd < 0) {
warn("%s", BARO_DEVICE_PATH);
errx(1, "FATAL: no barometer found");
warnx("No barometer found, ignoring");
}
/* set the driver to poll at 150Hz */
@@ -671,67 +639,36 @@ Sensors::adc_init()
void
Sensors::accel_poll(struct sensor_combined_s &raw)
{
struct accel_report accel_report;
bool accel_updated;
orb_check(_accel_sub, &accel_updated);
if (_fd_bma180 >= 0) {
/* do ORB emulation for BMA180 */
int16_t buf[3];
if (accel_updated) {
struct accel_report accel_report;
read(_fd_bma180, buf, sizeof(buf));
orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report);
accel_report.timestamp = hrt_absolute_time();
raw.accelerometer_m_s2[0] = accel_report.x;
raw.accelerometer_m_s2[1] = accel_report.y;
raw.accelerometer_m_s2[2] = accel_report.z;
accel_report.x_raw = (buf[1] == -32768) ? 32767 : -buf[1];
accel_report.y_raw = buf[0];
accel_report.z_raw = buf[2];
raw.accelerometer_raw[0] = accel_report.x_raw;
raw.accelerometer_raw[1] = accel_report.y_raw;
raw.accelerometer_raw[2] = accel_report.z_raw;
const float range_g = 4.0f;
/* scale from 14 bit to m/s2 */
accel_report.x = (((accel_report.x_raw - _parameters.accel_offset[0]) * range_g) / 8192.0f) / 9.81f;
accel_report.y = (((accel_report.y_raw - _parameters.accel_offset[0]) * range_g) / 8192.0f) / 9.81f;
accel_report.z = (((accel_report.z_raw - _parameters.accel_offset[0]) * range_g) / 8192.0f) / 9.81f;
raw.accelerometer_counter++;
} else {
bool accel_updated;
orb_check(_accel_sub, &accel_updated);
if (accel_updated) {
orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report);
raw.accelerometer_counter++;
}
}
raw.accelerometer_m_s2[0] = accel_report.x;
raw.accelerometer_m_s2[1] = accel_report.y;
raw.accelerometer_m_s2[2] = accel_report.z;
raw.accelerometer_raw[0] = accel_report.x_raw;
raw.accelerometer_raw[1] = accel_report.y_raw;
raw.accelerometer_raw[2] = accel_report.z_raw;
}
void
Sensors::gyro_poll(struct sensor_combined_s &raw)
{
struct gyro_report gyro_report;
bool gyro_updated;
orb_check(_gyro_sub, &gyro_updated);
if (_fd_gyro_l3gd20 >= 0) {
/* do ORB emulation for L3GD20 */
int16_t buf[3];
if (gyro_updated) {
struct gyro_report gyro_report;
read(_fd_gyro_l3gd20, buf, sizeof(buf));
gyro_report.timestamp = hrt_absolute_time();
gyro_report.x_raw = buf[1];
gyro_report.y_raw = ((buf[0] == -32768) ? 32767 : -buf[0]);
gyro_report.z_raw = buf[2];
/* scaling calculated as: raw * (1/(32768*(500/180*PI))) */
gyro_report.x = (gyro_report.x_raw - _parameters.gyro_offset[0]) * 0.000266316109f;
gyro_report.y = (gyro_report.y_raw - _parameters.gyro_offset[1]) * 0.000266316109f;
gyro_report.z = (gyro_report.z_raw - _parameters.gyro_offset[2]) * 0.000266316109f;
orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report);
raw.gyro_rad_s[0] = gyro_report.x;
raw.gyro_rad_s[1] = gyro_report.y;
@@ -742,25 +679,6 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
raw.gyro_raw[2] = gyro_report.z_raw;
raw.gyro_counter++;
} else {
bool gyro_updated;
orb_check(_gyro_sub, &gyro_updated);
if (gyro_updated) {
orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report);
raw.gyro_rad_s[0] = gyro_report.x;
raw.gyro_rad_s[1] = gyro_report.y;
raw.gyro_rad_s[2] = gyro_report.z;
raw.gyro_raw[0] = gyro_report.x_raw;
raw.gyro_raw[1] = gyro_report.y_raw;
raw.gyro_raw[2] = gyro_report.z_raw;
raw.gyro_counter++;
}
}
}
@@ -960,8 +878,6 @@ Sensors::ppm_poll()
/* Read out values from HRT */
for (unsigned int i = 0; i < channel_limit; i++) {
_rc.chan[i].raw = ppm_buffer[i];
/* Set the range to +-, then scale up */
_rc.chan[i].scale = (ppm_buffer[i] - _rc.chan[i].mid) * _rc.chan[i].scaling_factor * 10000;
/* scale around the mid point differently for lower and upper range */
if (ppm_buffer[i] > (_parameters.trim[i] + _parameters.dz[i])) {
@@ -976,7 +892,13 @@ Sensors::ppm_poll()
}
/* reverse channel if required */
_rc.chan[i].scaled *= _parameters.rev[i];
if (i == _rc.function[THROTTLE]) {
if ((int)_parameters.rev[i] == -1) {
_rc.chan[i].scaled = 1.0f + -1.0f * _rc.chan[i].scaled;
}
} else {
_rc.chan[i].scaled *= _parameters.rev[i];
}
/* handle any parameter-induced blowups */
if (isnan(_rc.chan[i].scaled) || isinf(_rc.chan[i].scaled))
@@ -1176,7 +1098,7 @@ Sensors::start()
/* start the task */
_sensors_task = task_spawn("sensors_task",
SCHED_RR,
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
6000, /* XXX may be excesssive */
(main_t)&Sensors::task_main_trampoline,
apps/system/Kconfig
+18 -2
View File
@@ -3,7 +3,7 @@
# see misc/tools/kconfig-language.txt.
#
menu "Custom free memory command"
menu "Custom Free Memory Command"
source "$APPSDIR/system/free/Kconfig"
endmenu
@@ -15,6 +15,22 @@ menu "FLASH Program Installation"
source "$APPSDIR/system/install/Kconfig"
endmenu
menu "readline() support"
menu "readline()"
source "$APPSDIR/system/readline/Kconfig"
endmenu
menu "Power Off"
source "$APPSDIR/system/poweroff/Kconfig"
endmenu
menu "RAMTRON"
source "$APPSDIR/system/ramtron/Kconfig"
endmenu
menu "SD Card"
source "$APPSDIR/system/sdcard/Kconfig"
endmenu
menu "Sysinfo"
source "$APPSDIR/system/sysinfo/Kconfig"
endmenu
apps/system/Make.defs
+17
View File
@@ -49,3 +49,20 @@ endif
ifeq ($(CONFIG_SYSTEM_READLINE),y)
CONFIGURED_APPS += system/readline
endif
ifeq ($(CONFIG_SYSTEM_POWEROFF),y)
CONFIGURED_APPS += system/poweroff
endif
ifeq ($(CONFIG_SYSTEM_RAMTRON),y)
CONFIGURED_APPS += system/ramtron
endif
ifeq ($(CONFIG_SYSTEM_SDCARD),y)
CONFIGURED_APPS += system/sdcard
endif
ifeq ($(CONFIG_SYSTEM_SYSINFO),y)
CONFIGURED_APPS += system/sysinfo
endif
apps/system/Makefile
+1 -1
View File
@@ -37,7 +37,7 @@
# Sub-directories containing system task
SUBDIRS = free i2c install readline
SUBDIRS = free i2c install readline poweroff ramtron sdcard sysinfo
# Create the list of installed runtime modules (INSTALLED_DIRS)
apps/system/free/free.c
+45 -37
View File
@@ -33,57 +33,65 @@
*
****************************************************************************/
/****************************************************************************
* Included Files
****************************************************************************/
#include <nuttx/config.h>
#include <nuttx/progmem.h>
#include <stdio.h>
#include <stdlib.h>
/****************************************************************************
* Private Functions
****************************************************************************/
/* \todo Max block size only works on uniform prog mem */
void free_getprogmeminfo(struct mallinfo * mem)
{
uint16_t page = 0, stpage = 0xFFFF;
uint16_t pagesize = 0;
int status;
mem->arena = 0;
mem->fordblks = 0;
mem->uordblks = 0;
mem->mxordblk = 0;
for (status=0, page=0; status >= 0; page++) {
status = up_progmem_ispageerased(page);
pagesize = up_progmem_pagesize(page);
mem->arena += pagesize;
/* Is this beginning of new free space section */
if (status == 0) {
if (stpage == 0xFFFF) stpage = page;
mem->fordblks += pagesize;
}
else if (status != 0) {
mem->uordblks += pagesize;
if (stpage != 0xFFFF && up_progmem_isuniform()) {
stpage = page - stpage;
if (stpage > mem->mxordblk)
mem->mxordblk = stpage;
stpage = 0xFFFF;
/* TODO Max block size only works on uniform prog mem */
static void free_getprogmeminfo(struct mallinfo * mem)
{
uint16_t page = 0, stpage = 0xFFFF;
uint16_t pagesize = 0;
int status;
mem->arena = 0;
mem->fordblks = 0;
mem->uordblks = 0;
mem->mxordblk = 0;
for (status=0, page=0; status >= 0; page++)
{
status = up_progmem_ispageerased(page);
pagesize = up_progmem_pagesize(page);
mem->arena += pagesize;
/* Is this beginning of new free space section */
if (status == 0)
{
if (stpage == 0xFFFF) stpage = page;
mem->fordblks += pagesize;
}
else if (status != 0)
{
mem->uordblks += pagesize;
if (stpage != 0xFFFF && up_progmem_isuniform())
{
stpage = page - stpage;
if (stpage > mem->mxordblk)
{
mem->mxordblk = stpage;
}
stpage = 0xFFFF;
}
}
}
mem->mxordblk *= pagesize;
}
mem->mxordblk *= pagesize;
}
/****************************************************************************
* Public Functions
apps/system/i2c/Makefile
+1 -1
View File
@@ -1,5 +1,5 @@
############################################################################
# apps/system/i2c
# apps/system/i2c/Makefile
#
# Copyright (C) 2011-2012 Gregory Nutt. All rights reserved.
# Author: Gregory Nutt <gnutt@nuttx.org>
apps/system/install/install.c
+342 -284
View File
@@ -33,6 +33,10 @@
*
****************************************************************************/
/****************************************************************************
* Included Files
****************************************************************************/
#include <nuttx/config.h>
#include <nuttx/progmem.h>
#include <sys/stat.h>
@@ -43,9 +47,8 @@
#include <string.h>
#include <errno.h>
/****************************************************************************
* Definitions
* Pre-processor Definitions
****************************************************************************/
#define ACTION_INSTALL 0x01
@@ -55,12 +58,11 @@
#define INSTALL_PROGRAMBLOCKSIZE 1024
/****************************************************************************
* Private data
****************************************************************************/
const char *install_help =
static const char *install_help =
"Installs XIP program into flash and creates a start-up script in the\n"
"destination directory.\n\n"
"Usage:\t%s [options] source-file.xip destination-directory\n\n"
@@ -72,341 +74,397 @@ const char *install_help =
"\t--force\t\t\tReplaces existing installation\n"
"\t--start <page>\t\tInstalls application at or after <page>\n"
"\t--margin <pages>\tLeave some free space after the kernel (default 16)\n";
const char *install_script_text =
static const char *install_script_text =
"# XIP stacksize=%x priority=%x size=%x\n";
const char *install_script_exec =
static const char *install_script_exec =
"exec 0x%x\n";
/****************************************************************************
* Private functions
****************************************************************************/
int install_getstartpage(int startpage, int pagemargin, int desiredsize)
static int install_getstartpage(int startpage, int pagemargin, int desiredsize)
{
uint16_t page = 0, stpage = 0xFFFF;
uint16_t pagesize = 0;
int maxlen = -1;
int maxlen_start = 0xFFFF;
int status;
for (status=0, page=0; status >= 0; page++) {
status = up_progmem_ispageerased(page);
pagesize = up_progmem_pagesize(page);
/* Is this beginning of new free space section */
if (status == 0) {
if (stpage == 0xFFFF) stpage = page;
uint16_t page = 0, stpage = 0xffff;
uint16_t pagesize = 0;
int maxlen = -1;
int maxlen_start = 0xffff;
int status;
for (status=0, page=0; status >= 0; page++)
{
status = up_progmem_ispageerased(page);
pagesize = up_progmem_pagesize(page);
/* Is this beginning of new free space section */
if (status == 0)
{
if (stpage == 0xffff) stpage = page;
}
else if (status != 0) {
else if (status != 0)
{
if (stpage != 0xffff)
{
if ((page - stpage) > maxlen)
{
if (maxlen==-1)
{
/* First time found sth? */
if (stpage != 0xFFFF) {
if ( (page - stpage) > maxlen) {
stpage += pagemargin;
maxlen = 0;
}
if (maxlen==-1) { /* First time found sth? */
stpage += pagemargin;
maxlen = 0;
if(stpage < startpage)
{
stpage = startpage;
}
if(stpage < startpage)
stpage = startpage;
if (page > stpage) {
maxlen = page - stpage;
maxlen_start = stpage;
if (page > stpage)
{
maxlen = page - stpage;
maxlen_start = stpage;
}
if (maxlen*pagesize >= desiredsize) {
/* printf("Found page at %d ... %d\n", stpage, page); */
return maxlen_start*pagesize;
if (maxlen*pagesize >= desiredsize)
{
/* printf("Found page at %d ... %d\n", stpage, page); */
return maxlen_start*pagesize;
}
}
stpage = 0xFFFF;
stpage = 0xffff;
}
}
}
/* Requested space is not available */
return -1;
/* Requested space is not available */
return -1;
}
int install_programflash(int startaddr, const char *source)
static int install_programflash(int startaddr, const char *source)
{
int status;
int count;
int totalsize = 0;
char *buf;
FILE *fp;
if ( (buf = malloc(INSTALL_PROGRAMBLOCKSIZE)) == NULL )
return -errno;
if ( (fp=fopen(source, "r")) ) {
do {
count = fread(buf, 1, INSTALL_PROGRAMBLOCKSIZE, fp);
if ( (status = up_progmem_write(startaddr, buf, count)) < 0) {
totalsize = status;
break;
}
startaddr += count;
totalsize += count;
}
while(count);
int status;
int count;
int totalsize = 0;
char *buf;
FILE *fp;
if ((buf = malloc(INSTALL_PROGRAMBLOCKSIZE)) == NULL)
{
return -ENOMEM;
}
else totalsize = -errno;
fclose(fp);
free(buf);
return totalsize;
if ((fp = fopen(source, "r")))
{
do
{
count = fread(buf, 1, INSTALL_PROGRAMBLOCKSIZE, fp);
if ((status = up_progmem_write(startaddr, buf, count)) < 0)
{
totalsize = status;
break;
}
startaddr += count;
totalsize += count;
}
while(count);
}
else
{
totalsize = -errno;
}
fclose(fp);
free(buf);
return totalsize;
}
void install_getscriptname(char *scriptname, const char *progname, const char *destdir)
static void install_getscriptname(char *scriptname, const char *progname, const char *destdir)
{
const char * progonly;
const char * progonly;
/* I.e. as /usr/bin */
strcpy(scriptname, destdir);
/* extract from i.e. /sdcard/demo -> /demo, together with / */
progonly = strrchr(progname, '/');
strcat(scriptname, progonly);
/* I.e. as /usr/bin */
strcpy(scriptname, destdir);
/* extract from i.e. /sdcard/demo -> /demo, together with / */
progonly = strrchr(progname, '/');
strcat(scriptname, progonly);
}
int install_getprogsize(const char *progname)
static int install_getprogsize(const char *progname)
{
struct stat fileinfo;
if ( stat(progname, &fileinfo) < 0 )
return -1;
return fileinfo.st_size;
struct stat fileinfo;
if (stat(progname, &fileinfo) < 0)
{
return -1;
}
return fileinfo.st_size;
}
int install_alreadyexists(const char *scriptname)
static int install_alreadyexists(const char *scriptname)
{
FILE *fp;
if ( (fp=fopen(scriptname, "r"))==NULL )
return 0;
fclose(fp);
FILE *fp;
if ((fp = fopen(scriptname, "r")) == NULL)
{
return 0;
}
fclose(fp);
return 1;
}
int install_createscript(int addr, int stacksize, int progsize,
int priority, const char *scriptname)
static int install_createscript(int addr, int stacksize, int progsize,
int priority, const char *scriptname)
{
FILE *fp;
if ( (fp=fopen(scriptname, "w+"))==NULL )
return -errno;
fprintf(fp, install_script_text, stacksize, priority, progsize);
fprintf(fp, install_script_exec, addr);
fflush(fp);
fclose(fp);
FILE *fp;
return 0;
if ((fp = fopen(scriptname, "w+")) == NULL)
{
return -errno;
}
fprintf(fp, install_script_text, stacksize, priority, progsize);
fprintf(fp, install_script_exec, addr);
fflush(fp);
fclose(fp);
return 0;
}
int install_getlasthexvalue(FILE *fp, char delimiter)
static int install_getlasthexvalue(FILE *fp, char delimiter)
{
char buf[128];
char *p;
if (fgets(buf, 127, fp)) {
if ( (p = strrchr(buf, delimiter)) ) {
return strtol(p+1, NULL, 16);
char buf[128];
char *p;
if (fgets(buf, 127, fp))
{
if ((p = strrchr(buf, delimiter)))
{
return strtol(p+1, NULL, 16);
}
}
return -1;
return -1;
}
int install_remove(const char *scriptname)
static int install_remove(const char *scriptname)
{
FILE *fp;
int progsize, addr, freedsize;
uint16_t page;
int status = 0;
/* Parse script */
if ( (fp=fopen(scriptname, "r")) ) {
progsize = install_getlasthexvalue(fp,'=');
addr = install_getlasthexvalue(fp,' ');
freedsize = progsize;
FILE *fp;
int progsize, addr, freedsize;
uint16_t page;
int status = 0;
/* Parse script */
if ((fp = fopen(scriptname, "r")))
{
progsize = install_getlasthexvalue(fp,'=');
addr = install_getlasthexvalue(fp,' ');
freedsize = progsize;
}
else return -errno;
fclose(fp);
/* Remove pages */
if (progsize <= 0 || addr <= 0)
return -EIO;
do {
if ((page = up_progmem_getpage(addr)) < 0) {
status = -page;
break;
else
{
return -errno;
}
fclose(fp);
/* Remove pages */
if (progsize <= 0 || addr <= 0)
{
return -EIO;
}
do
{
if ((page = up_progmem_getpage(addr)) < 0)
{
status = -page;
break;
}
if ( up_progmem_erasepage(page) < 0) {
status = -page;
break;
if (up_progmem_erasepage(page) < 0)
{
status = -page;
break;
}
addr += up_progmem_pagesize(page);
progsize -= up_progmem_pagesize(page);
} while(progsize > 0);
if (status < 0) return status;
/* Remove script file */
if (unlink(scriptname) < 0) return -errno;
return freedsize;
addr += up_progmem_pagesize(page);
progsize -= up_progmem_pagesize(page);
}
while(progsize > 0);
if (status < 0)
{
return status;
}
/* Remove script file */
if (unlink(scriptname) < 0)
{
return -errno;
}
return freedsize;
}
/****************************************************************************
* Start
* Public Functions
****************************************************************************/
int install_main(int argc, char *argv[])
{
int i;
int progsize;
int scrsta;
int stacksize = 4096;
int priority = SCHED_PRIORITY_DEFAULT;
int pagemargin = 16;
int startpage = 0;
int startaddr = 0;
int action = ACTION_INSTALL;
char scriptname[128];
/* Supported? */
if ( !up_progmem_isuniform() ) {
fprintf(stderr, "Error: install supports uniform organization only.\n");
return -1;
}
/* Parse arguments */
for (i=1; i<argc; i++) {
if (argv[i][0]=='-' && argv[i][1]=='-' && i<=argc) {
if (strcmp(argv[i]+2, "stack")==0) {
stacksize = atoi(argv[++i]);
}
else if (strcmp(argv[i]+2, "priority")==0) {
priority = atoi(argv[++i]);
}
else if (strcmp(argv[i]+2, "start")==0) {
startpage = atoi(argv[++i]);
}
else if (strcmp(argv[i]+2, "margin")==0) {
pagemargin = atoi(argv[++i]);
}
else if (strcmp(argv[i]+2, "remove")==0) {
action = ACTION_REMOVE;
}
else if (strcmp(argv[i]+2, "force")==0) {
action = ACTION_REINSTALL;
}
else fprintf(stderr, "Unknown option: %s\n", argv[i]);
}
else break;
}
/* Do the job */
switch(action & 1) {
case ACTION_REMOVE:
if (i > argc-1) {
action = ACTION_INSUFPARAM;
break; /* are there sufficient parameters */
}
if ( (scrsta=install_remove(argv[i])) < 0) {
fprintf(stderr, "Could not remove program: %s\n", strerror(-scrsta) );
return -1;
}
printf("Removed %s and freed %d bytes\n", argv[i], scrsta);
return 0;
case ACTION_INSTALL:
if (i > argc-2) {
action = ACTION_INSUFPARAM;
break; /* are there sufficient parameters */
}
install_getscriptname(scriptname, argv[i], argv[i+1]);
// script-exists?
if (install_alreadyexists(scriptname)==1) {
if (action != ACTION_REINSTALL) {
fprintf(stderr, "Program with that name already exists.\n");
return -EEXIST;
}
if ( (scrsta=install_remove(scriptname)) < 0) {
fprintf(stderr, "Could not remove program: %s\n", strerror(-scrsta) );
return -1;
}
printf("Replacing %s\n", scriptname);
}
startaddr = install_getstartpage(startpage, pagemargin, install_getprogsize(argv[i]) );
if (startpage < 0) {
fprintf(stderr, "Not enough memory\n");
return -ENOMEM;
}
if ( (progsize = install_programflash(startaddr, argv[i])) <= 0) {
fprintf(stderr, "Error writing program memory: %s\n"
"Note: Flash pages are not released, so you may try again and program will be\n"
" written in other pages.\n", strerror(-progsize) );
return -EIO;
}
if ( (scrsta = install_createscript(startaddr, stacksize, progsize,
priority, scriptname)) < 0) {
fprintf(stderr, "Error writing program script at %s: %s\n",
argv[i+1], strerror(-scrsta) );
return -EIO;
}
printf("Installed application of size %d bytes to program memory [%xh - %xh].\n",
progsize, startaddr, startaddr + progsize);
int i;
int progsize;
int scrsta;
int stacksize = 4096;
int priority = SCHED_PRIORITY_DEFAULT;
int pagemargin = 16;
int startpage = 0;
int startaddr = 0;
int action = ACTION_INSTALL;
char scriptname[128];
return 0;
/* Supported? */
if (!up_progmem_isuniform())
{
fprintf(stderr, "Error: install supports uniform organization only.\n");
return -1;
}
fprintf(stderr, install_help, argv[0], argv[0]);
return -1;
/* Parse arguments */
for (i=1; i<argc; i++)
{
if (argv[i][0]=='-' && argv[i][1]=='-' && i<=argc)
{
if (strcmp(argv[i]+2, "stack")==0)
{
stacksize = atoi(argv[++i]);
}
else if (strcmp(argv[i]+2, "priority")==0)
{
priority = atoi(argv[++i]);
}
else if (strcmp(argv[i]+2, "start")==0)
{
startpage = atoi(argv[++i]);
}
else if (strcmp(argv[i]+2, "margin")==0)
{
pagemargin = atoi(argv[++i]);
}
else if (strcmp(argv[i]+2, "remove")==0)
{
action = ACTION_REMOVE;
}
else if (strcmp(argv[i]+2, "force")==0)
{
action = ACTION_REINSTALL;
}
else fprintf(stderr, "Unknown option: %s\n", argv[i]);
}
else
{
break;
}
}
/* Do the job */
switch(action & 1)
{
case ACTION_REMOVE:
if (i > argc-1)
{
action = ACTION_INSUFPARAM;
break; /* are there sufficient parameters */
}
if ((scrsta=install_remove(argv[i])) < 0)
{
fprintf(stderr, "Could not remove program: %s\n", strerror(-scrsta));
return -1;
}
printf("Removed %s and freed %d bytes\n", argv[i], scrsta);
return 0;
case ACTION_INSTALL:
if (i > argc-2)
{
action = ACTION_INSUFPARAM;
break; /* are there sufficient parameters */
}
install_getscriptname(scriptname, argv[i], argv[i+1]);
/* script-exists? */
if (install_alreadyexists(scriptname) == 1)
{
if (action != ACTION_REINSTALL)
{
fprintf(stderr, "Program with that name already exists.\n");
return -EEXIST;
}
if ((scrsta = install_remove(scriptname)) < 0)
{
fprintf(stderr, "Could not remove program: %s\n", strerror(-scrsta));
return -1;
}
printf("Replacing %s\n", scriptname);
}
startaddr = install_getstartpage(startpage, pagemargin, install_getprogsize(argv[i]));
if (startpage < 0)
{
fprintf(stderr, "Not enough memory\n");
return -ENOMEM;
}
if ((progsize = install_programflash(startaddr, argv[i])) <= 0)
{
fprintf(stderr, "Error writing program memory: %s\n"
"Note: Flash pages are not released, so you may try again and program will be\n"
" written in other pages.\n", strerror(-progsize));
return -EIO;
}
if ((scrsta = install_createscript(startaddr, stacksize, progsize,
priority, scriptname)) < 0)
{
fprintf(stderr, "Error writing program script at %s: %s\n",
argv[i+1], strerror(-scrsta));
return -EIO;
}
printf("Installed application of size %d bytes to program memory [%xh - %xh].\n",
progsize, startaddr, startaddr + progsize);
return 0;
}
fprintf(stderr, install_help, argv[0], argv[0]);
return -1;
}
apps/system/readline/readline.c
+1 -1
View File
@@ -1,5 +1,5 @@
/****************************************************************************
* lib/stdio/lib_fgets.c
* apps/system/readline/readline.c
*
* Copyright (C) 2007-2008, 2011-2012 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <gnutt@nuttx.org>
apps/systemcmds/bl_update/bl_update.c
+36 -2
View File
@@ -51,16 +51,20 @@
#include <arch/board/board.h>
#include "systemlib/systemlib.h"
#include "systemlib/param/param.h"
#include "systemlib/err.h"
__EXPORT int bl_update_main(int argc, char *argv[]);
static void setopt(void);
int
bl_update_main(int argc, char *argv[])
{
if (argc != 2)
errx(1, "missing firmware filename");
errx(1, "missing firmware filename or command");
if (!strcmp(argv[1], "setopt"))
setopt();
int fd = open(argv[1], O_RDONLY);
if (fd < 0)
@@ -172,3 +176,33 @@ flash_end:
free(buf);
exit(0);
}
static void
setopt(void)
{
volatile uint32_t *optcr = (volatile uint32_t *)0x40023c14;
const uint16_t opt_mask = (3 << 2); /* BOR_LEV bitmask */
const uint16_t opt_bits = (0 << 2); /* BOR = 0, setting for 2.7-3.6V operation */
if ((*optcr & opt_mask) == opt_bits)
errx(0, "option bits are already set as required");
/* unlock the control register */
volatile uint32_t *optkeyr = (volatile uint32_t *)0x40023c08;
*optkeyr = 0x08192a3bU;
*optkeyr = 0x4c5d6e7fU;
if (*optcr & 1)
errx(1, "option control register unlock failed");
/* program the new option value */
*optcr = (*optcr & ~opt_mask) | opt_bits | (1 << 1);
usleep(1000);
if ((*optcr & opt_mask) == opt_bits)
errx(0, "option bits set");
errx(1, "option bits setting failed; readback 0x%04x", *optcr);
}
apps/systemcmds/eeprom/eeprom.c
+4
View File
@@ -193,6 +193,8 @@ eeprom_save(const char *name)
if (!name)
err(1, "missing argument for device name, try '/eeprom/parameters'");
warnx("WARNING: 'eeprom save_param' deprecated - use 'param save' instead");
/* delete the file in case it exists */
unlink(name);
@@ -222,6 +224,8 @@ eeprom_load(const char *name)
if (!name)
err(1, "missing argument for device name, try '/eeprom/parameters'");
warnx("WARNING: 'eeprom load_param' deprecated - use 'param load' instead");
int fd = open(name, O_RDONLY);
if (fd < 0)
apps/systemcmds/led/led.c
+1 -1
View File
@@ -148,7 +148,7 @@ int led_main(int argc, char *argv[])
thread_should_exit = false;
led_task = task_spawn("led",
SCHED_RR,
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 15,
4096,
led_thread_main,
apps/systemcmds/param/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.
#
############################################################################
#
# Build the parameters tool.
#
APPNAME = param
PRIORITY = SCHED_PRIORITY_DEFAULT
STACKSIZE = 4096
include $(APPDIR)/mk/app.mk
apps/systemcmds/param/param.c
+185
View File
@@ -0,0 +1,185 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* 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 param.c
*
* Parameter tool.
*/
#include <nuttx/config.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdbool.h>
#include <unistd.h>
#include <fcntl.h>
#include <sys/stat.h>
#include <arch/board/board.h>
#include "systemlib/systemlib.h"
#include "systemlib/param/param.h"
#include "systemlib/err.h"
__EXPORT int param_main(int argc, char *argv[]);
static void do_save(void);
static void do_load(void);
static void do_import(void);
static void do_show(void);
static void do_show_print(void *arg, param_t param);
static const char *param_file_name = "/eeprom/parameters";
int
param_main(int argc, char *argv[])
{
if (argc >= 2) {
if (!strcmp(argv[1], "save"))
do_save();
if (!strcmp(argv[1], "load"))
do_load();
if (!strcmp(argv[1], "import"))
do_import();
if (!strcmp(argv[1], "show"))
do_show();
}
errx(1, "expected a command, try 'load', 'import', 'show' or 'save'\n");
}
static void
do_save(void)
{
/* delete the parameter file in case it exists */
unlink(param_file_name);
/* create the file */
int fd = open(param_file_name, O_WRONLY | O_CREAT | O_EXCL);
if (fd < 0)
err(1, "opening '%s' failed", param_file_name);
int result = param_export(fd, false);
close(fd);
if (result < 0) {
unlink(param_file_name);
errx(1, "error exporting to '%s'", param_file_name);
}
exit(0);
}
static void
do_load(void)
{
int fd = open(param_file_name, O_RDONLY);
if (fd < 0)
err(1, "open '%s'", param_file_name);
int result = param_load(fd);
close(fd);
if (result < 0)
errx(1, "error importing from '%s'", param_file_name);
exit(0);
}
static void
do_import(void)
{
int fd = open(param_file_name, O_RDONLY);
if (fd < 0)
err(1, "open '%s'", param_file_name);
int result = param_import(fd);
close(fd);
if (result < 0)
errx(1, "error importing from '%s'", param_file_name);
exit(0);
}
static void
do_show(void)
{
printf(" + = saved, * = unsaved\n");
param_foreach(do_show_print, NULL, false);
exit(0);
}
static void
do_show_print(void *arg, param_t param)
{
int32_t i;
float f;
printf("%c %s: ",
param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'),
param_name(param));
/*
* This case can be expanded to handle printing common structure types.
*/
switch (param_type(param)) {
case PARAM_TYPE_INT32:
if (!param_get(param, &i)) {
printf("%d\n", i);
return;
}
break;
case PARAM_TYPE_FLOAT:
if (!param_get(param, &f)) {
printf("%4.4f\n", (double)f);
return;
}
break;
case PARAM_TYPE_STRUCT ... PARAM_TYPE_STRUCT_MAX:
printf("<struct type %d size %u>\n", 0 + param_type(param), param_size(param));
return;
default:
printf("<unknown type %d>\n", 0 + param_type(param));
return;
}
printf("<error fetching parameter %d>\n", param);
}
apps/systemcmds/top/top.c
+22 -3
View File
@@ -135,9 +135,9 @@ int top_main(int argc, char *argv[])
memset(header_spaces, ' ', CONFIG_TASK_NAME_SIZE);
header_spaces[CONFIG_TASK_NAME_SIZE] = '\0';
#if CONFIG_RR_INTERVAL > 0
printf("\033[KPID\tCOMMAND%s CPU TOTAL \t%%CPU CURR \tMIN STACK USE\tCURR (BASE) PRIO\tRR SLICE\n", header_spaces);
printf("\033[KPID\tCOMMAND%s CPU TOTAL \t%%CPU CURR \tSTACK USE\tCURR (BASE) PRIO\tRR SLICE\n", header_spaces);
#else
printf("\033[KPID\tCOMMAND%s CPU TOTAL \t%%CPU CURR \tMIN STACK USE\tCURR (BASE) PRIO\n", header_spaces);
printf("\033[KPID\tCOMMAND%s CPU TOTAL \t%%CPU CURR \tSTACK USE\tCURR (BASE) PRIO\n", header_spaces);
#endif
} else {
@@ -190,7 +190,26 @@ int top_main(int argc, char *argv[])
runtime_spaces = "";
}
printf("\033[K % 2d\t%s%s % 8lld ms%s \t % 2d.%03d \t % 6d B", (int)system_load.tasks[i].tcb->pid, system_load.tasks[i].tcb->name, spaces, (system_load.tasks[i].total_runtime / 1000), runtime_spaces, (int)(curr_loads[i] * 100), (int)(curr_loads[i] * 100000.0f - (int)(curr_loads[i] * 1000.0f) * 100), (uint32_t)system_load.tasks[i].tcb->adj_stack_ptr - (uint32_t)system_load.tasks[i].tcb->xcp.regs[REG_R13]);
unsigned stack_size = (uintptr_t)system_load.tasks[i].tcb->adj_stack_ptr -
(uintptr_t)system_load.tasks[i].tcb->stack_alloc_ptr;
unsigned stack_free = 0;
uint8_t *stack_sweeper = (uint8_t *)system_load.tasks[i].tcb->stack_alloc_ptr;
while (stack_free < stack_size) {
if (*stack_sweeper++ != 0xff)
break;
stack_free++;
}
printf("\033[K % 2d\t%s%s % 8lld ms%s \t % 2d.%03d \t % 4u / % 4u",
(int)system_load.tasks[i].tcb->pid,
system_load.tasks[i].tcb->name,
spaces,
(system_load.tasks[i].total_runtime / 1000),
runtime_spaces,
(int)(curr_loads[i] * 100),
(int)(curr_loads[i] * 100000.0f - (int)(curr_loads[i] * 1000.0f) * 100),
stack_size - stack_free,
stack_size);
/* Print scheduling info with RR time slice */
#if CONFIG_RR_INTERVAL > 0
printf("\t%d\t(%d)\t\t%d\n", (int)system_load.tasks[i].tcb->sched_priority, (int)system_load.tasks[i].tcb->base_priority, (int)system_load.tasks[i].tcb->timeslice);
apps/systemlib/bson/tinybson.c
+3 -1
View File
@@ -106,7 +106,9 @@ bson_decoder_next(bson_decoder_t decoder)
/* if the nesting level is now zero, the top-level document is done */
if (decoder->nesting == 0) {
CODER_KILL(decoder, "nesting is zero, document is done");
/* like kill but not an error */
debug("nesting is zero, document is done");
decoder->fd = -1;
/* return end-of-file to the caller */
return 0;
apps/systemlib/param/param.c
+47 -11
View File
@@ -242,6 +242,25 @@ param_name(param_t param)
return NULL;
}
bool
param_value_is_default(param_t param)
{
return param_find_changed(param) ? false : true;
}
bool
param_value_unsaved(param_t param)
{
static struct param_wbuf_s *s;
s = param_find_changed(param);
if (s && s->unsaved)
return true;
return false;
}
enum param_type_e
param_type(param_t param)
{
@@ -330,8 +349,8 @@ param_get(param_t param, void *val)
return result;
}
int
param_set(param_t param, const void *val)
static int
param_set_internal(param_t param, const void *val, bool mark_saved)
{
int result = -1;
bool params_changed = false;
@@ -394,7 +413,7 @@ param_set(param_t param, const void *val)
goto out;
}
s->unsaved = true;
s->unsaved = !mark_saved;
params_changed = true;
result = 0;
}
@@ -412,6 +431,12 @@ out:
return result;
}
int
param_set(param_t param, const void *val)
{
return param_set_internal(param, val, false);
}
void
param_reset(param_t param)
{
@@ -535,6 +560,11 @@ out:
return result;
}
struct param_import_state
{
bool mark_saved;
};
static int
param_import_callback(bson_decoder_t decoder, void *private, bson_node_t node)
{
@@ -542,13 +572,13 @@ param_import_callback(bson_decoder_t decoder, void *private, bson_node_t node)
int32_t i;
void *v, *tmp = NULL;
int result = -1;
struct param_import_state *state = (struct param_import_state *)private;
/*
* EOO means the end of the parameter object. (Currently not supporting
* nested BSON objects).
*/
if (node->type == BSON_EOO) {
*(bool *)private = true;
debug("end of parameters");
return 0;
}
@@ -621,7 +651,7 @@ param_import_callback(bson_decoder_t decoder, void *private, bson_node_t node)
goto out;
}
if (param_set(param, v)) {
if (param_set_internal(param, v, state->mark_saved)) {
debug("error setting value for '%s'", node->name);
goto out;
}
@@ -642,19 +672,19 @@ out:
return result;
}
int
param_import(int fd)
static int
param_import_internal(int fd, bool mark_saved)
{
bool done;
struct bson_decoder_s decoder;
int result = -1;
struct param_import_state state;
if (bson_decoder_init(&decoder, fd, param_import_callback, &done)) {
if (bson_decoder_init(&decoder, fd, param_import_callback, &state)) {
debug("decoder init failed");
goto out;
}
done = false;
state.mark_saved = mark_saved;
do {
result = bson_decoder_next(&decoder);
@@ -668,11 +698,17 @@ out:
return result;
}
int
param_import(int fd)
{
return param_import_internal(fd, false);
}
int
param_load(int fd)
{
param_reset_all();
return param_import(fd);
return param_import_internal(fd, true);
}
void
apps/systemlib/param/param.h
+16 -1
View File
@@ -121,6 +121,20 @@ __EXPORT int param_get_index(param_t param);
*/
__EXPORT const char *param_name(param_t param);
/**
* Test whether a parameter's value has changed from the default.
*
* @return If true, the parameter's value has not been changed from the default.
*/
__EXPORT bool param_value_is_default(param_t param);
/**
* Test whether a parameter's value has been changed but not saved.
*
* @return If true, the parameter's value has not been saved.
*/
__EXPORT bool param_value_unsaved(param_t param);
/**
* Obtain the type of a parameter.
*
@@ -160,7 +174,8 @@ __EXPORT int param_set(param_t param, const void *val);
/**
* Reset a parameter to its default value.
*
* This function frees any storage used by struct parameters, but scalar parameters
* This function frees any storage used by struct parameters, and returns the parameter
* to its default value.
*
* @param param A handle returned by param_find or passed by param_foreach.
*/
apps/systemlib/systemlib.c
+1 -2
View File
@@ -138,9 +138,8 @@ int task_spawn(const char *name, int scheduler, int priority, int stack_size, ma
param.sched_priority = priority;
sched_setscheduler(pid, scheduler, &param);
/* XXX do any other private task accounting here */
/* XXX do any other private task accounting here before the task starts */
}
sched_unlock();
return pid;
apps/systemlib/systemlib.h
+7
View File
@@ -50,6 +50,13 @@ __EXPORT int reboot(void);
/** Sends SIGUSR1 to all processes */
__EXPORT void killall(void);
/** Default scheduler type */
#if CONFIG_RR_INTERVAL > 0
# define SCHED_DEFAULT SCHED_RR
#else
# define SCHED_DEFAULT SCHED_FIFO
#endif
/** Starts a task and performs any specific accounting, scheduler setup, etc. */
__EXPORT int task_spawn(const char *name,
int priority,
apps/uORB/objects_common.cpp
+3
View File
@@ -77,6 +77,9 @@ ORB_DEFINE(vehicle_global_position, struct vehicle_global_position_s);
#include "topics/vehicle_local_position.h"
ORB_DEFINE(vehicle_local_position, struct vehicle_local_position_s);
#include "topics/vehicle_vicon_position.h"
ORB_DEFINE(vehicle_vicon_position, struct vehicle_vicon_position_s);
#include "topics/vehicle_rates_setpoint.h"
ORB_DEFINE(vehicle_rates_setpoint, struct vehicle_rates_setpoint_s);
apps/uORB/topics/rc_channels.h
+2 -3
View File
@@ -86,10 +86,9 @@ struct rc_channels_s {
uint64_t timestamp_last_valid; /**< timestamp of last valid RC signal. */
struct {
uint16_t mid; /**< midpoint (0). */
float scaling_factor; /**< scaling factor from raw counts to 0..1 */
float scaling_factor; /**< scaling factor from raw counts to -1..+1 */
uint16_t raw; /**< current raw value */
int16_t scale;
float scaled; /**< Scaled */
float scaled; /**< Scaled to -1..1 (throttle: 0..1) */
uint16_t override;
enum RC_CHANNELS_STATUS status; /**< status of the channel */
} chan[RC_CHANNELS_FUNCTION_MAX];
apps/uORB/topics/vehicle_attitude.h
+1
View File
@@ -74,6 +74,7 @@ struct vehicle_attitude_s {
float rollspeed; /**< Roll angular speed (rad/s, Tait-Bryan, NED) LOGME */
float pitchspeed; /**< Pitch angular speed (rad/s, Tait-Bryan, NED) LOGME */
float yawspeed; /**< Yaw angular speed (rad/s, Tait-Bryan, NED) LOGME */
float rate_offsets[3];/**< Offsets of the body angular rates from zero */
float R[3][3]; /**< Rotation matrix body to world, (Tait-Bryan, NED) */
float q[4]; /**< Quaternion (NED) */
bool R_valid; /**< Rotation matrix valid */
nuttx/configs/px4fmu/include/drv_lis331.h → apps/uORB/topics/vehicle_vicon_position.h
+33 -38
View File
@@ -1,6 +1,7 @@
/****************************************************************************
*
* 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
@@ -31,53 +32,47 @@
*
****************************************************************************/
/*
* Driver for the ST LIS331 MEMS accelerometer
/**
* @file vehicle_vicon_position.h
* Definition of the raw VICON Motion Capture position
*/
#include <sys/ioctl.h>
#ifndef TOPIC_VEHICLE_VICON_POSITION_H_
#define TOPIC_VEHICLE_VICON_POSITION_H_
#define _LIS331BASE 0x6900
#define LIS331C(_x) _IOC(_LIS331BASE, _x)
#include <stdint.h>
#include <stdbool.h>
#include "../uORB.h"
/*
* Sets the sensor internal sampling rate, and if a buffer
* has been configured, the rate at which entries will be
* added to the buffer.
/**
* @addtogroup topics
* @{
*/
#define LIS331_SETRATE LIS331C(1)
#define LIS331_RATE_50Hz (0<<3)
#define LIS331_RATE_100Hz (1<<3)
#define LIS331_RATE_400Hz (2<<3)
#define LIS331_RATE_1000Hz (3<<3)
/*
* Sets the sensor internal range.
/**
* Fused local position in NED.
*/
#define LIS331_SETRANGE LIS331C(2)
struct vehicle_vicon_position_s
{
uint64_t timestamp; /**< time of this estimate, in microseconds since system start */
bool valid; /**< true if position satisfies validity criteria of estimator */
#define LIS331_RANGE_2G (0<<4)
#define LIS331_RANGE_4G (1<<4)
#define LIS331_RANGE_8G (3<<4)
float x; /**< X positin in meters in NED earth-fixed frame */
float y; /**< X positin in meters in NED earth-fixed frame */
float z; /**< Z positin in meters in NED earth-fixed frame (negative altitude) */
float vx;
float vy;
float vz;
/*
* Sets the address of a shared lis331_buffer
* structure that is maintained by the driver.
*
* If zero is passed as the address, disables
* the buffer updating.
*/
#define LIS331_SETBUFFER LIS331C(3)
// TODO Add covariances here
struct lis331_buffer {
uint32_t size; /* number of entries in the samples[] array */
uint32_t next; /* the next entry that will be populated */
struct {
uint16_t x;
uint16_t y;
uint16_t z;
} samples[];
};
extern int lis331_attach(struct spi_dev_s *spi, int spi_id);
/**
* @}
*/
/* register this as object request broker structure */
ORB_DECLARE(vehicle_vicon_position);
#endif
nuttx/ChangeLog
+65 -14
View File
@@ -3177,7 +3177,7 @@
* arch/arm/src/stm32/stm32_otgfshost.c: This driver now appears to be
functional (although more testing is necesary).
6.22 2012-xx-xx Gregory Nutt <gnutt@nuttx.org>
6.22 2012-09-29 Gregory Nutt <gnutt@nuttx.org>
* include/semaphore.h, sched/sem_holders.c, and lib/semaphore/sem_init.c:
Fix some strange (and probably wrong) list handling when
@@ -3193,7 +3193,7 @@
speed/duplex. This does not work for certain PHYs. Still some unresolved
issues (also from Kate).
* tools/Config.mk, Makefile, configs/*/Make.defs: Add a new Makefile
fragement to de-quoate certain strings from the Kconfig logic that
fragment to de-quote certain strings from the Kconfig logic that
need to be used at path segments (Richard Cochran).
* arch/arm/src/stm32/stm32_usbotghost.c: The STM32 USB host driver only
works with debug turned on. The problem appears to be that with debug
@@ -3201,7 +3201,7 @@
reveals a variety of errors. This check in improves NAK robustness
for control transfers but does not resolve all of the issues.
* configs/stm3220g-eval/*/defconfig: Calibrated delay loop. It had
never been calibrated was was way off.
never been calibrated was way off.
* sched/sem_holder.c: Add logic to handler some priority inheritance
cases when sem_post() is called from an interrupt handler. The
logic is clearly wrong, but it is not known if this is the
@@ -3212,13 +3212,13 @@
CONFIG_LIBC_STRERROR_SHORT that can be used to output shortened
strings by strerror().
* arch/arm/src/stm32/stm32_usbotghost.c: Finally... the USB OTG FS
appears to handle NAKing correctly is complete.
appears to handle NAKing correctly.
* configs/stm32f4discovery/*: Added and verifed support for USB OTG FS
host on the STM32F4Discovery board.
* configs/*/defconfig: Remove configuration documentation from config
files. It is redundant, error-prone, and difficult to maintain.
Configuration documentation is available in configs/README.txt for
common configurations and in configs/*/README.txt for board and MCU_
common configurations and in configs/*/README.txt for board and MCU-
specific configurations.
* configs/stm3240g-eval: Add USB host support.
* sched/os_bring.c, configs/*/defconfig, tools/mkconfig.c, and others: Added
@@ -3226,7 +3226,7 @@
the default entry from user_start to some other symbol. Contributed by
Kate. NOTE: This change does introduce a minor backward incompatibility.
For example, if your application uses NSH as its start-up program, then your
code will not fail because it will be unable to find "user_start". The fix
build will now fail because it will be unable to find "user_start". The fix
for this link failure is to add the following to your configuration file:
CONFIG_USER_ENTRYPOINT="nsh_main".
* libs/stdio/lib_libfread.c and lib_*flush*.c: Correct a couple of
@@ -3269,7 +3269,7 @@
CONFIG_HEAP2_SIZE (decimal) instead of CONFIG_HEAP2_END (hex).
* tools/configure.sh: Don't append the apps directory path setting
if the correct setting is already in defined in the defconfig file.
* fs/fat/fs_utils.c: Improper constructed bool expression. This
* fs/fat/fs_utils.c: Improperly constructed bool expression. This
would cause many unnecessary writes to FLASH (Thanks Ronen Vainish).
* Kconfig: Verify configuration settings for the LPC43xx. This includes
some corrections to configuration variable names and defconfig settings.
@@ -3319,13 +3319,13 @@
in all places.
* drivers/enc28j60.c, include/nuttx/net/enc28j60.h, and
olimex-strp711/src/up_enc28j60.c: No longer passes IRQ number
as a parameters. Instead now passes a call table to manage
as a parameter. Instead now passes a call table to manage
ENC28J60 GPIO interrupts. That is because GPIO interrupts are
handled in different ways by different MCUs and some do not
support IRQ numbers for GPIO interrupts.
* mm/mm_gran* and include/nuttx/gran.h: Add a simple granule-
based allocator. The intent of this allocator is to support
simple allocation of DMA I/O buffers. The initiali check-in
simple allocation of DMA I/O buffers. The initial check-in
is code complete but untested (not event built into the
mm/Makefile yet.
* confgs/fire-stm32v2: The board port is basically functional.
@@ -3352,7 +3352,7 @@
* arch/arm/include/armv7-m/irq.h: Fix a critical bug in irqsave().
It looks like sometimes the compile will re-order some instructions
inapproapriately. This end result is that interrupts will get
stuff off.
stuck off.
* drivers/mtd/w25.c: Beginning of a driver for the Windbond SPI
FLASH family (W25x16, W25x32, and W25x64). The initial check-in
is basically just the SST25 driver with some name changes.
@@ -3372,7 +3372,7 @@
I2C reset logic to recover from locked devices on the bus.
* configs/*/*/Make.defs, tools/Config.mk, Makefile: Refactor all
common make definitions from the various Make.defs files into
the common tools/Make.mk. Add support for a verbosity options:
the common tools/Config.mk. Add support for a verbosity options:
Specify V=1 on the make command line in order to see the exact
commands used in the build (Contributed by Richard Cochran).
* drivers/net/enc28j60.c: The ENC28J60 Ethernet driver is
@@ -3400,17 +3400,17 @@
* configs/shenzhou/src/up_lcd.c: Oops. Shenzhou LCD does not
have an SSD1289 controller. Its an ILI93xx. Ported the
STM3240G-EVAL ILI93xx driver to work on the Shenzhou board.
* configs/shenzhou/nxwm: Added an NxWM configuratino for the
* configs/shenzhou/nxwm: Added an NxWM configuration for the
Shenzhou board. This is untested on initial check-in. It will
be used to verify the Shenzhou LCD driver (and eventually the
touchscreen driver).
* configs/shenzhou/src/up_touchscreen.c: Add ADS7843E touchscreen
support for the Shenzhou board. The initial check-in is untested
and basically a clone of the the touchscreen support fro the SAM-3U.
and basically a clone of the the touchscreen support for the SAM-3U.
* tools/cfgparser.c: There are some NxWidget configuration
settings that must be de-quoted.
* arch/arm/src/stm32/Kconfig: There is no SPI4. Some platforms
SPI3 and some do not (still not clear).
support SPI3 and some do not (still not clear).
* nuttx/configs/shenzhou: Various fixes to build new NxWM
configuration.
* configs/shenzhou: Oops. The Shenzhou LCD is and SSD1289,
@@ -3419,4 +3419,55 @@
on the Shenzhou board.
* graphics/nxmu: Correct some bad parameter checking that caused
failures when DEBUG was enabled.
* arch/arm/src/armv7-m/nvic.h: Add bit definitions for the AIRCR
register.
* drivers/input/ads7843.c: Need semaphore protection in logic
that samples the position.
* drivers/lcd/ssd1289.c: On some platforms we are unable to
read the device ID -- reason unknown; workaround in place.
* drivers/input/ads7843.c: Add thresholding options and an
option to swap X and Y positions. Fix some logic errors in
the SPI locking/selecting logic.
* arch/arm/src/armv7-m/up_systemreset.c: Add logic to reset
the Cortex-Mx using the AIRCR register. Contributed by Darcy
Gong.
* arch/arm/src/stm32/up_eth.c: Add logic specifically for the
DM9161 PHY. If the DM9161 failed to initialize, then use the
up_sysemreset() logic to reset the MCU. Contributed by Darcy
Gong.
* arch/arm/src/stm32/stm32_gpio.c: Add missing logic to set bit
for SPI3 remap. This fixes the XPT2046 touchscreen driver using
drivers/input/ads7843.c
* configs/shenzhou/src/up_ssd1289.c: Fix naming error in
conditional compilation.
* configs/shenzhou/nxwm/defconfig: Disable reading from the LCD.
This does not work. The hardware and the driver support the
capability, but there is some bug that causes memory corruption.
The work around for now: Just disable reading from the LCD.
* drivers/lcd/ssd1289.c: Add some logic to reduce the amount of
output when CONFIG_DEBUG_LCD is enabled.
* configs/shenzhou/nxwm/defconfig: Bug found and fixed... The
original configuration had too much stuff turned on. Reducing
stack sizes, some features, and buffer sizes made the
configuration reliable (Reading from the LCD is still disabled).
* net/uip/uip_icmpping.c: Fix problem that prevented ping from
going outside of local network. Submitted by Darcy Gong
6.23 2012-09-29 Gregory Nutt <gnutt@nuttx.org>
* arch/arm/src/stm32/stm32_rng.c, chip/stm32_rng.h, and other files:
Implementation of /dev/random using the STM32 Random Number
Generator (RNG).
* board.h file for shenzhou, fire-stm32v2, and olimex-stm32-p107:
Add frequencies for HSE, HSI, LSE, and LSI. These are needed
by the STM32 watchdog driver.
* CONFIG_EXAMPLES_*: To make things consistent, changed all occurrences
of CONFIG_EXAMPLE_* to CONFIG_EXAMPLES_*.
* drivers/mtd/w25.c and configs/*/src/up_w25.c: Several fixes for the
W25 SPI FLASH.
* configs/*/Make.defs: All buildroot tools now use the extension
xxx-nuttx-elf- vs. xxx-elf-
* configs/shenzhou/*/Make.defs: Now uses the new buildroot 4.6.3
EABI toolchain.
* lib/stdio/lib_libdtoa.c: Another dtoa() fix from Mike Smith.
nuttx/README.txt
+2 -3
View File
@@ -853,9 +853,8 @@ apps
|- system/
| |- i2c/README.txt
| |- free/README.txt
| `- install
| `- README.txt
|- vsn/
| |- install
| | `- README.txt
| |- poweroff
| | `- README.txt
| |- ramtron
nuttx/ReleaseNotes
+108
View File
@@ -1606,6 +1606,8 @@ The 61st release of NuttX, NuttX-5.14, was made on November 27,
2010. This release includes multiple, important bugfixes as well
as a new driver for the NXP LPC1766.
This release corresponds with SVN release number: r3137
Important bugfixes include:
* Cortex-M3 Hard Fault. Fixed a hard fault problem that can occur
@@ -2052,6 +2054,8 @@ interest expressed by members of the forum and because of the
availability of newer, larger capacity AVR parts (that I don't have
yet).
This release corresponds with SVN release number: r3730
This release includes support for the following AVR boards. As
with any initial support for new architectures, there are some
incomplete areas and a few caveats that need to be stated. Here
@@ -3063,3 +3067,107 @@ Bugfixes (see the change log for details) :
for C++
As well as other, less critical bugs (see the ChangeLog for details)
NuttX-6.22
^^^^^^^^^^
The 89th release of NuttX, Version 6.22, was made on September 29, 2012,
and is available for download from the SourceForge website. Note
that release consists of two tarballs: nuttx-6.22.tar.gz and
apps-6.22.tar.gz. Both may be needed (see the top-level nuttx/README.txt
file for build information).
This release corresponds with SVN release number: r5206
Note that all SVN information has been stripped from the tarballs. If you
need the SVN configuration, you should check out directly from SVN. Revision
r5206 should equivalent to release 6.22 of NuttX 6.22:
svn checkout -r5206 svn://svn.code.sf.net/p/nuttx/code/trunk nuttx-code
Or
svn checkout -r5206 http://svn.code.sf.net/p/nuttx/code/trunk nuttx-code
Additional new features and extended functionality:
* RTOS: Application entry point is no longer user_start, but can be
configured using CONFIG_USER_ENTRYPOINT. NuttX now supports two work
queues: A lower priority work queue (for extended processing) and a
higher priority work queue (for quick, high priority operations).
* Memory Management: Added a new granule-based allocated that can be
used to manage, aligned and quantized DMA memory.
* File System: Add hooks to allocate I/O memory with and external
allocated (need if required by DMA).
* Networking: ENC28J60 driver is (finally) verified.
* Drivers: Add hooks USB device drivers to allocate I/O memory with and
external allocated (need if required by DMA). Driver for the Windbond
SPI FLASH family (W25x16, W25x32, W25x64, and others). ADS7843E driver
extended for TSC2046 and XPT2046 and verified.
* ARMv7-M: Added logic to reset the MCU using the NVIC.
* STM32: Add support for STM32F103VET6.
* STM32 Drivers: Add logic to re-initialize UARTs a second time to
enable DMA (Mike Smith). I2C driver error recovery (Mike Smith).
* STM32 boards: Support for USB host added add to several configurations
(or at least explained in README files). Support for the Shenzhou
STM32F107 board (see www.armjishu.com). Support for M3 Wildfire
STM32F103 board (v2 and v3).
* Build System: Kconfig string de-quoting logic. Remove comments from
defconfig files (Kate). Add tool to create NuttX-style symbol tables.
Numerous changes to configuration logic as needed for the new mconf-based
configuration (much of this from Richard Cochran). Refactor common
Make.defs logic into tools/Config.mk (Richard Cochran).
* Library: Configurable terse output from strerror(). Added perror() (Kate).
Add %n format to sscanf() (Kate).
* Applications: Numerous changes and extensions to the old uIP web server
(from Kate and Max Holtzberg, see the ChangeLog for specific extensions).
UDP network discovery utility (Max Holtzberg). Embeddable Lightweight
XML-RPC Server (http://www.drdobbs.com/web-development/an-embeddable-lightweight-xml-rpc-server/184405364, Max Holtzberg).
Bugfixes (see the change log for details). Some of these are very important
(marked *critical*):
* RTOS: Fixes to priority inheritance logic (*critical*). waitpid()
critical section. Assertion in work_cancel() (Mike Smith). mmap() (Kate).
* FAT File System: Improper Boolean expression caused un-necessary writes
and performance issues (*critical*, Ronen Vainish).
* Networking: Remove an un-necessary delay from recvfrom(). This greatly
improves network performance (*critical*, Max Holtzberg).
* Graphics: NX parameter checking errors.
* Drivers: Fix double release of memory in SDIO-based, MMC/SD driver
(Ronen Vainish).
* LPC17xx: Ethernet driver fixes needed for certain PHYs (Kate).
* AVR: Fix build error (Richard Cochran).
* STM32: USB OTG FS host driver NAKing an retries. Power management
compilation errors (Diego Sanchez). Missing SPI3 remap logic.
* STM32 Drivers: Fix for Ethernet errata for STM32F107 (*critical*).
Ethernet buffer alignment check. Add "kludge" to Ethernet driver to
handle DM9161 PHY which (at least on the Shenzhou board), sometimes
does not come up correctly.
* Applications: THTTPD (Kate). NSH ping when IP address is on a different
network (Darcy Gong).
* Library: fread(), fflush(), fdopen(): Fix error handling logic (Ronen
Vainish). Fix some field-width handling issues in sscanf()
As well as other, less critical bugs (see the ChangeLog for details)
nuttx/TODO
+46 -4
View File
@@ -12,7 +12,7 @@ nuttx/
(2) Signals (sched/, arch/)
(2) pthreads (sched/)
(2) C++ Support
(5) Binary loaders (binfmt/)
(6) Binary loaders (binfmt/)
(17) Network (net/, drivers/net)
(3) USB (drivers/usbdev, drivers/usbhost)
(11) Libraries (lib/)
@@ -376,15 +376,15 @@ o Binary loaders (binfmt/)
Description: Windows build issue. Some of the configurations that use NXFLAT have
the linker script specified like this:
NXFLATLDFLAGS2 = $(NXFLATLDFLAGS1) -T$(TOPDIR)/binfmt/libnxflat/gnu-nxflat.ld -no-check-sections
NXFLATLDFLAGS2 = $(NXFLATLDFLAGS1) -T$(TOPDIR)/binfmt/libnxflat/gnu-nxflat-gotoff.ld -no-check-sections
That will not work for windows-based tools because they require Windows
style paths. The solution is to do something like this:
if ($(WINTOOL)y)
NXFLATLDSCRIPT=${cygpath -w $(TOPDIR)/binfmt/libnxflat/gnu-nxflat.ld}
NXFLATLDSCRIPT=${cygpath -w $(TOPDIR)/binfmt/libnxflat/gnu-nxflat-gotoff.ld}
else
NXFLATLDSCRIPT=$(TOPDIR)/binfmt/libnxflat/gnu-nxflat.ld
NXFLATLDSCRIPT=$(TOPDIR)/binfmt/libnxflat/gnu-nxflat-gotoff.ld
endif
Then use
@@ -395,6 +395,48 @@ o Binary loaders (binfmt/)
Priority: There are too many references like the above. They will have
to get fixed as needed for Windows native tool builds.
Title: TOOLCHAIN COMPATIBILITY PROBLEM
Descripton: The older 4.3.3 compiler generates GOTOFF relocations to the constant
strings, like:
.L3:
.word .LC0(GOTOFF)
.word .LC1(GOTOFF)
.word .LC2(GOTOFF)
.word .LC3(GOTOFF)
.word .LC4(GOTOFF)
Where .LC0, LC1, LC2, LC3, and .LC4 are the labels correponding to strings in
the .rodata.str1.1 section. One consequence of this is that .rodata must reside
in D-Space since it will addressed relative to the GOT (see the section entitled
"Read-Only Data in RAM" at
http://nuttx.org/Documentation/NuttXNxFlat.html#limitations).
The newer 4.6.3compiler generated PC relative relocations to the strings:
.L2:
.word .LC0-(.LPIC0+4)
.word .LC1-(.LPIC1+4)
.word .LC2-(.LPIC2+4)
.word .LC3-(.LPIC4+4)
.word .LC4-(.LPIC5+4)
This is good and bad. This is good because it means that .rodata.str1.1 can not
reside in FLASH with .text and can be accessed using PC-relative addressing.
That can be accomplished by simply moving the .rodata from the .data section to
the .text section in the linker script. (The NXFLAT linker script is located at
nuttx/binfmt/libnxflat/gnu-nxflat.ld).
This is bad because a lot of stuff may get broken an a lot of test will need to
be done. One question that I have is does this apply to all kinds of .rodata?
Or just to .rodata.str1.1?
Status: Open. Many of the required changes are in place but, unfortunately, not enough
go be fully functional. I think all of the I-Space-to-I-Space fixes are in place.
However, the generated code also includes PC-relative references to .bss which
just cannot be done.
Priority: Medium. The workaround for now is to use the older, 4.3.3 OABI compiler.
o Network (net/, drivers/net)
^^^^^^^^^^^^^^^^^^^^^^^^^^^
nuttx/arch/arm/src/armv7-m/nvic.h
+16 -2
View File
@@ -175,7 +175,7 @@
#define NVIC_CPUID_BASE_OFFSET 0x0d00 /* CPUID base register */
#define NVIC_INTCTRL_OFFSET 0x0d04 /* Interrupt control state register */
#define NVIC_VECTAB_OFFSET 0x0d08 /* Vector table offset register */
#define NVIC_AIRC_OFFSET 0x0d0c /* Application interrupt/reset contol registr */
#define NVIC_AIRCR_OFFSET 0x0d0c /* Application interrupt/reset contol registr */
#define NVIC_SYSCON_OFFSET 0x0d10 /* System control register */
#define NVIC_CFGCON_OFFSET 0x0d14 /* Configuration control register */
#define NVIC_SYSH_PRIORITY_OFFSET(n) (0x0d14 + 4*((n) >> 2))
@@ -348,7 +348,7 @@
#define NVIC_CPUID_BASE (ARMV7M_NVIC_BASE + NVIC_CPUID_BASE_OFFSET)
#define NVIC_INTCTRL (ARMV7M_NVIC_BASE + NVIC_INTCTRL_OFFSET)
#define NVIC_VECTAB (ARMV7M_NVIC_BASE + NVIC_VECTAB_OFFSET)
#define NVIC_AIRC (ARMV7M_NVIC_BASE + NVIC_AIRC_OFFSET)
#define NVIC_AIRCR (ARMV7M_NVIC_BASE + NVIC_AIRCR_OFFSET)
#define NVIC_SYSCON (ARMV7M_NVIC_BASE + NVIC_SYSCON_OFFSET)
#define NVIC_CFGCON (ARMV7M_NVIC_BASE + NVIC_CFGCON_OFFSET)
#define NVIC_SYSH_PRIORITY(n) (ARMV7M_NVIC_BASE + NVIC_SYSH_PRIORITY_OFFSET(n))
@@ -500,6 +500,20 @@
#define NVIC_SYSHCON_BUSFAULTENA (1 << 17) /* Bit 17: BusFault enabled */
#define NVIC_SYSHCON_USGFAULTENA (1 << 18) /* Bit 18: UsageFault enabled */
/* Application Interrupt and Reset Control Register (AIRCR) */
#define NVIC_AIRCR_VECTRESET (1 << 0) /* Bit 0: VECTRESET */
#define NVIC_AIRCR_VECTCLRACTIVE (1 << 1) /* Bit 1: Reserved for debug use */
#define NVIC_AIRCR_SYSRESETREQ (1 << 2) /* Bit 2: System reset */
/* Bits 2-7: Reserved */
#define NVIC_AIRCR_PRIGROUP_SHIFT (8) /* Bits 8-14: PRIGROUP */
#define NVIC_AIRCR_PRIGROUP_MASK (7 << NVIC_AIRCR_PRIGROUP_SHIFT)
#define NVIC_AIRCR_ENDIANNESS (1 << 15) /* Bit 15: 1=Big endian */
#define NVIC_AIRCR_VECTKEY_SHIFT (16) /* Bits 16-31: VECTKEY */
#define NVIC_AIRCR_VECTKEY_MASK (0xffff << NVIC_AIRCR_VECTKEY_SHIFT)
#define NVIC_AIRCR_VECTKEYSTAT_SHIFT (16) /* Bits 16-31: VECTKEYSTAT */
#define NVIC_AIRCR_VECTKEYSTAT_MASK (0xffff << NVIC_AIRCR_VECTKEYSTAT_SHIFT)
/* Debug Exception and Monitor Control Register (DEMCR) */
#define NVIC_DEMCR_VCCORERESET (1 << 0) /* Bit 0: Reset Vector Catch */
nuttx/arch/arm/src/armv7-m/up_systemreset.c
+79
View File
@@ -0,0 +1,79 @@
/****************************************************************************
* arch/arm/src/armv7-m/up_systemreset.c
*
* Copyright (C) 2012 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <gnutt@nuttx.org>
* Darcy Gong
*
* 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 <stdint.h>
#include "up_arch.h"
#include "nvic.h"
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
/****************************************************************************
* Public Types
****************************************************************************/
/****************************************************************************
* Public functions
****************************************************************************/
void up_systemreset(void)
{
uint32_t regval;
/* Set up for the system reset, retaining the priority group from the
* the AIRCR register.
*/
regval = getreg32(NVIC_AIRCR) & NVIC_AIRCR_PRIGROUP_MASK;
regval |= ((0x5fa << NVIC_AIRCR_VECTKEY_SHIFT) | NVIC_AIRCR_SYSRESETREQ);
putreg32(regval, NVIC_AIRCR);
/* Ensure completion of memory accesses */
__asm volatile ("dsb");
/* Wait for the reset */
for (;;);
}
nuttx/arch/arm/src/common/up_initialize.c
+6
View File
@@ -171,6 +171,12 @@ void up_initialize(void)
ramlog_consoleinit();
#endif
/* Initialize the Random Number Generator (RNG) */
#ifdef CONFIG_DEV_RANDOM
up_rnginitialize();
#endif
/* Initialize the system logging device */
#ifdef CONFIG_SYSLOG_CHAR
nuttx/arch/arm/src/common/up_internal.h
+10
View File
@@ -241,6 +241,10 @@ extern void up_pminitialize(void);
# define up_pminitialize()
#endif
#if defined(CONFIG_ARCH_CORTEXM3) || defined(CONFIG_ARCH_CORTEXM4)
extern void up_systemreset(void) noreturn_function;
#endif
/* Interrupt handling *******************************************************/
extern void up_irqinitialize(void);
@@ -369,6 +373,12 @@ extern void up_usbuninitialize(void);
# define up_usbuninitialize()
#endif
/* Random Number Generator (RNG) ********************************************/
#ifdef CONFIG_DEV_RANDOM
extern void up_rnginitialize(void);
#endif
/****************************************************************************
* Name: up_check_stack
*
nuttx/arch/arm/src/stm32/Kconfig
+2
View File
@@ -269,6 +269,7 @@ config STM32_ETHMAC
bool "Ethernet MAC"
default n
depends on STM32_CONNECTIVITYLINE || STM32_STM32F20XX || STM32_STM32F40XX
select ARCH_HAVE_PHY
config STM32_FSMC
bool "FSMC"
@@ -319,6 +320,7 @@ config STM32_RNG
bool "RNG"
default n
depends on STM32_STM32F20XX || STM32_STM32F40XX
select ARCH_HAVE_RNG
config STM32_SDIO
bool "SDIO"
nuttx/arch/arm/src/stm32/Make.defs
+7 -3
View File
@@ -45,8 +45,8 @@ CMN_CSRCS = up_assert.c up_blocktask.c up_copystate.c \
up_initialize.c up_initialstate.c up_interruptcontext.c \
up_memfault.c up_modifyreg8.c up_modifyreg16.c up_modifyreg32.c \
up_releasepending.c up_releasestack.c up_reprioritizertr.c \
up_schedulesigaction.c up_sigdeliver.c up_unblocktask.c \
up_usestack.c up_doirq.c up_hardfault.c up_svcall.c \
up_schedulesigaction.c up_sigdeliver.c up_systemreset.c \
up_unblocktask.c up_usestack.c up_doirq.c up_hardfault.c up_svcall.c \
up_stackcheck.c
ifeq ($(CONFIG_ARMV7M_CMNVECTOR),y)
@@ -83,7 +83,7 @@ endif
ifeq ($(CONFIG_USBHOST),y)
ifeq ($(CONFIG_STM32_OTGFS),y)
CMN_CSRCS += stm32_otgfshost.c
CMN_CSRCS += stm32_otgfshost.c
endif
endif
@@ -124,6 +124,10 @@ ifeq ($(CONFIG_DAC),y)
CHIP_CSRCS += stm32_dac.c
endif
ifeq ($(CONFIG_DEV_RANDOM),y)
CHIP_CSRCS += stm32_rng.c
endif
ifeq ($(CONFIG_PWM),y)
CHIP_CSRCS += stm32_pwm.c
endif
nuttx/arch/arm/src/stm32/chip/stm32_rng.h
+77
View File
@@ -0,0 +1,77 @@
/************************************************************************************
* arch/arm/src/stm32/chip/stm32_rng.h
*
* Copyright (C) 2012 Max Holtzberg. All rights reserved.
* Author: Max Holtzberg <mh@uvc.de>
*
* 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.
*
************************************************************************************/
#ifndef __ARCH_ARM_STC_STM32_CHIP_STM32_RNG_H
#define __ARCH_ARM_STC_STM32_CHIP_STM32_RNG_H
/************************************************************************************
* Included Files
************************************************************************************/
#include <nuttx/config.h>
#include "chip.h"
/************************************************************************************
* Pre-processor Definitions
************************************************************************************/
/* Register Offsets *****************************************************************/
#define STM32_RNG_CR_OFFSET 0x0000 /* RNG Control Register */
#define STM32_RNG_SR_OFFSET 0x0004 /* RNG Status Register */
#define STM32_RNG_DR_OFFSET 0x0008 /* RNG Data Register */
/* Register Addresses ***************************************************************/
#define STM32_RNG_CR (STM32_RNG_BASE+STM32_RNG_CR_OFFSET)
#define STM32_RNG_SR (STM32_RNG_BASE+STM32_RNG_SR_OFFSET)
#define STM32_RNG_DR (STM32_RNG_BASE+STM32_RNG_DR_OFFSET)
/* Register Bitfield Definitions ****************************************************/
/* RNG Control Register */
#define RNG_CR_RNGEN (1 << 2) /* Bit 2: RNG enable */
#define RNG_CR_IE (1 << 3) /* Bit 3: Interrupt enable */
/* RNG Status Register */
#define RNG_SR_DRDY (1 << 0) /* Bit 0: Data ready */
#define RNG_SR_CECS (1 << 1) /* Bit 1: Clock error current status */
#define RNG_SR_SECS (1 << 2) /* Bit 2: Seed error current status */
#define RNG_SR_CEIS (1 << 5) /* Bit 5: Clock error interrupt status */
#define RNG_SR_SEIS (1 << 6) /* Bit 6: Seed error interrupt status */
#endif /* __ARCH_ARM_STC_STM32_CHIP_STM32_RNG_H */
nuttx/arch/arm/src/stm32/stm32_eth.c
+81 -1
View File
@@ -664,6 +664,9 @@ static void stm32_rxdescinit(FAR struct stm32_ethmac_s *priv);
static int stm32_phyread(uint16_t phydevaddr, uint16_t phyregaddr, uint16_t *value);
static int stm32_phywrite(uint16_t phydevaddr, uint16_t phyregaddr, uint16_t value);
#ifdef CONFIG_PHY_DM9161
static inline int stm32_dm9161(FAR struct stm32_ethmac_s *priv);
#endif
static int stm32_phyinit(FAR struct stm32_ethmac_s *priv);
/* MAC/DMA Initialization */
@@ -1653,6 +1656,7 @@ static void stm32_receive(FAR struct stm32_ethmac_s *priv)
stm32_freebuffer(priv, dev->d_buf);
dev->d_buf = NULL;
dev->d_len = 0;
}
}
}
@@ -1953,7 +1957,7 @@ static void stm32_polltimer(int argc, uint32_t arg, ...)
/* Check if the next TX descriptor is owned by the Ethernet DMA or CPU. We
* cannot perform the timer poll if we are unable to accept another packet
* for transmission. Hmmm.. might be bug here. Does this mean if there is
* a transmit in progress, we will missing TCP time state updates?
* a transmit in progress, we will miss TCP time state updates?
*
* In a race condition, ETH_TDES0_OWN may be cleared BUT still not available
* because stm32_freeframe() has not yet run. If stm32_freeframe() has run,
@@ -2479,6 +2483,72 @@ static int stm32_phywrite(uint16_t phydevaddr, uint16_t phyregaddr, uint16_t val
return -ETIMEDOUT;
}
/****************************************************************************
* Function: stm32_dm9161
*
* Description:
* Special workaround for the Davicom DM9161 PHY is required. On power,
* up, the PHY is not usually configured correctly but will work after
* a powered-up reset. This is really a workaround for some more
* fundamental issue with the PHY clocking initialization, but the
* root cause has not been studied (nor will it be with this workaround).
*
* Parameters:
* priv - A reference to the private driver state structure
*
* Returned Value:
* None
*
****************************************************************************/
#ifdef CONFIG_PHY_DM9161
static inline int stm32_dm9161(FAR struct stm32_ethmac_s *priv)
{
uint16_t phyval;
int ret;
/* Read the PHYID1 register; A failure to read the PHY ID is one
* indication that check if the DM9161 PHY CHIP is not ready.
*/
ret = stm32_phyread(CONFIG_STM32_PHYADDR, MII_PHYID1, &phyval);
if (ret < 0)
{
ndbg("Failed to read the PHY ID1: %d\n", ret);
return ret;
}
/* If we failed to read the PHY ID1 register, the reset the MCU to recover */
else if (phyval == 0xffff)
{
up_systemreset();
}
nvdbg("PHY ID1: 0x%04X\n", phyval);
/* Now check the "DAVICOM Specified Configuration Register (DSCR)", Register 16 */
ret = stm32_phyread(CONFIG_STM32_PHYADDR, 16, &phyval);
if (ret < 0)
{
ndbg("Failed to read the PHY Register 0x10: %d\n", ret);
return ret;
}
/* Bit 8 of the DSCR register is zero, the the DM9161 has not selected RMII.
* If RMII is not selected, then reset the MCU to recover.
*/
else if ((phyval & (1 << 8)) == 0)
{
up_systemreset();
}
return OK;
}
#endif
/****************************************************************************
* Function: stm32_phyinit
*
@@ -2524,6 +2594,16 @@ static int stm32_phyinit(FAR struct stm32_ethmac_s *priv)
}
up_mdelay(PHY_RESET_DELAY);
/* Special workaround for the Davicom DM9161 PHY is required. */
#ifdef CONFIG_PHY_DM9161
ret = stm32_dm9161(priv);
if (ret < 0)
{
return ret;
}
#endif
/* Perform auto-negotion if so configured */
#ifdef CONFIG_STM32_AUTONEG
nuttx/arch/arm/src/stm32/stm32_gpio.c
+1
View File
@@ -128,6 +128,7 @@ static inline void stm32_gpioremap(void)
val |= AFIO_MAPR_SPI1_REMAP;
#endif
#ifdef CONFIG_STM32_SPI3_REMAP
val |= AFIO_MAPR_SPI3_REMAP;
#endif
#ifdef CONFIG_STM32_I2C1_REMAP
nuttx/arch/arm/src/stm32/stm32_rng.c
+264
View File
@@ -0,0 +1,264 @@
/****************************************************************************
* arch/arm/src/stm32/stm32_rng.c
*
* Copyright (C) 2012 Max Holtzberg. All rights reserved.
* Author: Max Holtzberg <mh@uvc.de>
*
* 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 <stdint.h>
#include <stdio.h>
#include <stdbool.h>
#include <debug.h>
#include <errno.h>
#include <nuttx/irq.h>
#include <nuttx/arch.h>
#include "up_arch.h"
#include "chip/stm32_rng.h"
#include "up_internal.h"
/****************************************************************************
* Private Function Prototypes
****************************************************************************/
static int stm32_rnginitialize(void);
static int stm32_interrupt(int irq, void *context);
static void stm32_enable(void);
static void stm32_disable(void);
static ssize_t stm32_read(struct file *filep, char *buffer, size_t);
/****************************************************************************
* Private Types
****************************************************************************/
struct rng_dev_s
{
sem_t rd_devsem; /* Threads can only exclusively access the RNG */
sem_t rd_readsem; /* To block until the buffer is filled */
char *rd_buf;
size_t rd_buflen;
uint32_t rd_lastval;
bool rd_first;
};
/****************************************************************************
* Private Data
****************************************************************************/
static struct rng_dev_s g_rngdev;
static const struct file_operations g_rngops =
{
0, /* open */
0, /* close */
stm32_read, /* read */
0, /* write */
0, /* seek */
0 /* ioctl */
#ifndef CONFIG_DISABLE_POLL
,0 /* poll */
#endif
};
/****************************************************************************
* Private functions
****************************************************************************/
static int stm32_rnginitialize()
{
uint32_t regval;
vdbg("Initializing RNG\n");
memset(&g_rngdev, 0, sizeof(struct rng_dev_s));
sem_init(&g_rngdev.rd_devsem, 0, 1);
if (irq_attach(STM32_IRQ_RNG, stm32_interrupt))
{
/* We could not attach the ISR to the interrupt */
vdbg("Could not attach IRQ.\n");
return -EAGAIN;
}
/* Enable interrupts */
regval = getreg32(STM32_RNG_CR);
regval |= RNG_CR_IE;
putreg32(regval, STM32_RNG_CR);
up_enable_irq(STM32_IRQ_RNG);
return OK;
}
static void stm32_enable()
{
uint32_t regval;
g_rngdev.rd_first = true;
regval = getreg32(STM32_RNG_CR);
regval |= RNG_CR_RNGEN;
putreg32(regval, STM32_RNG_CR);
}
static void stm32_disable()
{
uint32_t regval;
regval = getreg32(STM32_RNG_CR);
regval &= ~RNG_CR_RNGEN;
putreg32(regval, STM32_RNG_CR);
}
static int stm32_interrupt(int irq, void *context)
{
uint32_t rngsr;
uint32_t data;
rngsr = getreg32(STM32_RNG_SR);
if ((rngsr & (RNG_SR_SEIS | RNG_SR_CEIS)) /* Check for error bits */
|| !(rngsr & RNG_SR_DRDY)) /* Data ready must be set */
{
/* This random value is not valid, we will try again. */
return OK;
}
data = getreg32(STM32_RNG_DR);
/* As required by the FIPS PUB (Federal Information Processing Standard
* Publication) 140-2, the first random number generated after setting the
* RNGEN bit should not be used, but saved for comparison with the next
* generated random number. Each subsequent generated random number has to be
* compared with the previously generated number. The test fails if any two
* compared numbers are equal (continuous random number generator test).
*/
if (g_rngdev.rd_first)
{
g_rngdev.rd_first = false;
g_rngdev.rd_lastval = data;
return OK;
}
if (g_rngdev.rd_lastval == data)
{
/* Two subsequent same numbers, we will try again. */
return OK;
}
/* If we get here, the random number is valid. */
g_rngdev.rd_lastval = data;
if (g_rngdev.rd_buflen >= 4)
{
g_rngdev.rd_buflen -= 4;
*(uint32_t*)&g_rngdev.rd_buf[g_rngdev.rd_buflen] = data;
}
else
{
while (g_rngdev.rd_buflen > 0)
{
g_rngdev.rd_buf[--g_rngdev.rd_buflen] = (char)data;
data >>= 8;
}
}
if (g_rngdev.rd_buflen == 0)
{
/* Buffer filled, stop further interrupts. */
stm32_disable();
sem_post(&g_rngdev.rd_readsem);
}
return OK;
}
/****************************************************************************
* Name: stm32_read
****************************************************************************/
static ssize_t stm32_read(struct file *filep, char *buffer, size_t buflen)
{
if (sem_wait(&g_rngdev.rd_devsem) != OK)
{
return -errno;
}
else
{
/* We've got the semaphore. */
/* Initialize semaphore with 0 for blocking until the buffer is filled from
* interrupts.
*/
sem_init(&g_rngdev.rd_readsem, 0, 1);
g_rngdev.rd_buflen = buflen;
g_rngdev.rd_buf = buffer;
/* Enable RNG with interrupts */
stm32_enable();
/* Wait until the buffer is filled */
sem_wait(&g_rngdev.rd_readsem);
/* Free RNG for next use */
sem_post(&g_rngdev.rd_devsem);
return buflen;
}
}
/****************************************************************************
* Public Functions
****************************************************************************/
void up_rnginitialize()
{
stm32_rnginitialize();
register_driver("/dev/random", &g_rngops, 0444, NULL);
}
nuttx/binfmt/libnxflat/gnu-nxflat.ld → nuttx/binfmt/libnxflat/gnu-nxflat-gotoff.ld
+18 -3
View File
@@ -1,7 +1,7 @@
/****************************************************************************
* examples/nxflat/nxflat.ld
* examples/nxflat/gnu-nxflat-gotoff.ld
*
* Copyright (C) 2009 Gregory Nutt. All rights reserved.
* Copyright (C) 2009, 2012 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <gnutt@nuttx.org>
*
* Redistribution and use in source and binary forms, with or without
@@ -48,7 +48,17 @@ MEMORY
* (2) DSpace (Data Space). This is the segment that contains both
* read-write data (.data, .bss) as well as read-only data (.rodata).
* Everything in this segment should be access-able with machine
* with machine load and store instructions.
* PIC load and store instructions.
*
* Older versions of GCC (at least up to GCC 4.3.3), use GOT-relative
* addressing to access RO data. In that case, read-only data (.rodata) must
* reside in D-Space and this linker script should be used.
*
* Newer versions of GCC (at least as of GCC 4.6.3), use PC-relative
* addressing to access RO data. In that case, read-only data (.rodata) must
* reside in I-Space and this linker script should NOT be used with those
* newer tools.
*
****************************************************************************/
SECTIONS
@@ -97,11 +107,16 @@ SECTIONS
.data 0x00000000 :
{
/* In this model, .rodata is access using PC-relative addressing
* and, hence, must also reside in the .text section.
*/
__data_start = . ;
*(.rodata)
*(.rodata1)
*(.rodata.*)
*(.gnu.linkonce.r*)
*(.data)
*(.data1)
*(.data.*)
nuttx/binfmt/libnxflat/gnu-nxflat-pcrel.ld
+187
View File
@@ -0,0 +1,187 @@
/****************************************************************************
* examples/nxflat/gnu-nxflat-pcrel.ld
*
* Copyright (C) 2012 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <gnutt@nuttx.org>
*
* 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.
*
****************************************************************************/
MEMORY
{
ISPACE : ORIGIN = 0x0, LENGTH = 2097152
DSPACE : ORIGIN = 0x0, LENGTH = 2097152
}
/****************************************************************************
* The XFLAT program image is divided into two segments:
*
* (1) ISpace (Instruction Space). This is the segment that contains
* code (.text) as well as read-only data (.rodata). Everything in the
* segment should be fetch-able machine PC instructions (jump, branch,
* call, etc.) or PC-relative loads.
* (2) DSpace (Data Space). This is the segment that contains read-write
* data (.data, .bss). Everything in this segment should be access-able
* with machine PIC load and store instructions.
*
* Older versions of GCC (at least up to GCC 4.3.3), use GOT-relative
* addressing to access RO data. In that case, read-only data (.rodata) must
* reside in D-Space and this linker script should NOT be used with those
* older tools.
*
* Newer versions of GCC (at least as of GCC 4.6.3), use PC-relative
* addressing to access RO data. In that case, read-only data (.rodata) must
* reside in I-Space and this linker script should be used.
*
****************************************************************************/
SECTIONS
{
.text 0x00000000 :
{
/* ISpace is located at address 0. Every (unrelocated) ISpace
* address is an offset from the begining of this segment.
*/
text_start = . ;
*(.text)
*(.text.*)
*(.gnu.warning)
*(.stub)
*(.glue_7)
*(.glue_7t)
*(.jcr)
/* C++ support: The .init and .fini sections contain XFLAT-
* specific logic to manage static constructors and destructors.
*/
*(.gnu.linkonce.t.*)
*(.init)
*(.fini)
/* This is special code area at the end of the normal
text section. It contains a small lookup table at
the start followed by the code pointed to by entries
in the lookup table. */
. = ALIGN (4) ;
PROVIDE(__ctbp = .);
*(.call_table_data)
*(.call_table_text)
/* In this model, .rodata is access using PC-relative addressing
* and, hence, must also reside in the .text section.
*/
*(.rodata)
*(.rodata1)
*(.rodata.*)
*(.gnu.linkonce.r*)
_etext = . ;
} > ISPACE
/* DSpace is also located at address 0. Every (unrelocated) DSpace
* address is an offset from the begining of this segment.
*/
.data 0x00000000 :
{
__data_start = . ;
*(.data)
*(.data1)
*(.data.*)
*(.gnu.linkonce.d*)
*(.data1)
*(.eh_frame)
*(.gcc_except_table)
*(.gnu.linkonce.s.*)
*(__libc_atexit)
*(__libc_subinit)
*(__libc_subfreeres)
*(.note.ABI-tag)
/* C++ support. For each global and static local C++ object,
* GCC creates a small subroutine to construct the object. Pointers
* to these routines (not the routines themselves) are stored as
* simple, linear arrays in the .ctors section of the object file.
* Similarly, pointers to global/static destructor routines are
* stored in .dtors.
*/
*(.gnu.linkonce.d.*)
_ctors_start = . ;
*(.ctors)
_ctors_end = . ;
_dtors_start = . ;
*(.dtors)
_dtors_end = . ;
_edata = . ;
edata = ALIGN( 0x10 ) ;
} > DSPACE
.bss :
{
__bss_start = _edata ;
*(.dynsbss)
*(.sbss)
*(.sbss.*)
*(.scommon)
*(.dynbss)
*(.bss)
*(.bss.*)
*(.bss*)
*(.gnu.linkonce.b*)
*(COMMON)
end = ALIGN( 0x10 ) ;
_end = ALIGN( 0x10 ) ;
} > DSPACE
.got 0 : { *(.got.plt) *(.got) }
.junk 0 : { *(.rel*) *(.rela*) }
/* Stabs debugging sections. */
.stab 0 : { *(.stab) }
.stabstr 0 : { *(.stabstr) }
.stab.excl 0 : { *(.stab.excl) }
.stab.exclstr 0 : { *(.stab.exclstr) }
.stab.index 0 : { *(.stab.index) }
.stab.indexstr 0 : { *(.stab.indexstr) }
.comment 0 : { *(.comment) }
.debug_abbrev 0 : { *(.debug_abbrev) }
.debug_info 0 : { *(.debug_info) }
.debug_line 0 : { *(.debug_line) }
.debug_pubnames 0 : { *(.debug_pubnames) }
.debug_aranges 0 : { *(.debug_aranges) }
}
nuttx/configs/Kconfig
+1
View File
@@ -23,6 +23,7 @@ config ARCH_BOARD_PX4IO
depends on ARCH_CHIP_STM32F100C8
---help---
PX4 system I/O expansion board
endchoice
config ARCH_BOARD
nuttx/configs/README.txt
+12 -12
View File
@@ -1524,7 +1524,7 @@ configs/c5471evm
This is a port to the Spectrum Digital C5471 evaluation board. The
TMS320C5471 is a dual core processor from TI with an ARM7TDMI general
purpose processor and a c54 DSP. It is also known as TMS320DA180 or just DA180.
NuttX runs on the ARM core and is built with a GNU arm-elf toolchain*.
NuttX runs on the ARM core and is built with a GNU arm-nuttx-elf toolchain*.
This port is complete and verified.
configs/compal_e88 and compal_e99
@@ -1540,19 +1540,19 @@ configs/demo9s12ne64
configs/ea3131
Embedded Artists EA3131 Development board. This board is based on the
an NXP LPC3131 MCU. This OS is built with the arm-elf toolchain*.
an NXP LPC3131 MCU. This OS is built with the arm-nuttx-elf toolchain*.
STATUS: This port is complete and mature.
configs/ea3152
Embedded Artists EA3152 Development board. This board is based on the
an NXP LPC3152 MCU. This OS is built with the arm-elf toolchain*.
an NXP LPC3152 MCU. This OS is built with the arm-nuttx-elf toolchain*.
STATUS: This port is has not be exercised well, but since it is
a simple derivative of the ea3131, it should be fully functional.
configs/eagle100
Micromint Eagle-100 Development board. This board is based on the
an ARM Cortex-M3 MCU, the Luminary LM3S6918. This OS is built with the
arm-elf toolchain*. STATUS: This port is complete and mature.
arm-nuttx-elf toolchain*. STATUS: This port is complete and mature.
configs/ekk-lm3s9b96
TI/Stellaris EKK-LM3S9B96 board. This board is based on the
@@ -1591,7 +1591,7 @@ configs/lm3s6432-s2e
configs/lm3s6965-ek
Stellaris LM3S6965 Evaluation Kit. This board is based on the
an ARM Cortex-M3 MCU, the Luminary/TI LM3S6965. This OS is built with the
arm-elf toolchain*. STATUS: This port is complete and mature.
arm-nuttx-elf toolchain*. STATUS: This port is complete and mature.
configs/lm3s8962-ek
Stellaris LMS38962 Evaluation Kit.
@@ -1607,17 +1607,17 @@ configs/lpc4330-xplorer
configs/m68322evb
This is a work in progress for the venerable m68322evb board from
Motorola. This OS is also built with the arm-elf toolchain*. STATUS:
Motorola. This OS is also built with the arm-nuttx-elf toolchain*. STATUS:
This port was never completed.
configs/mbed
The configurations in this directory support the mbed board (http://mbed.org)
that features the NXP LPC1768 microcontroller. This OS is also built
with the arm-elf toolchain*. STATUS: Contributed.
with the arm-nuttx-elf toolchain*. STATUS: Contributed.
configs/mcu123-lpc214x
This port is for the NXP LPC2148 as provided on the mcu123.com
lpc214x development board. This OS is also built with the arm-elf
lpc214x development board. This OS is also built with the arm-nuttx-elf
toolchain*. The port supports serial, timer0, spi, and usb.
configs/micropendous3
@@ -1642,7 +1642,7 @@ configs/ne64badge
not yet been fully tested.
configs/ntosd-dm320
This port uses the Neuros OSD v1.0 Dev Board with a GNU arm-elf
This port uses the Neuros OSD v1.0 Dev Board with a GNU arm-nuttx-elf
toolchain*: see
http://wiki.neurostechnology.com/index.php/OSD_1.0_Developer_Home
@@ -1666,18 +1666,18 @@ configs/olimex-lpc1766stk
Linux or Cygwin. STATUS: Complete and mature.
configs/olimex-lpc2378
This port uses the Olimex-lpc2378 board and a GNU arm-elf toolchain* under
This port uses the Olimex-lpc2378 board and a GNU arm-nuttx-elf toolchain* under
Linux or Cygwin. STATUS: ostest and NSH configurations available.
This port for the NXP LPC2378 was contributed by Rommel Marcelo.
configs/olimex-stm32-p107
This port uses the Olimex STM32-P107 board (STM32F107VC) and a GNU arm-elf
This port uses the Olimex STM32-P107 board (STM32F107VC) and a GNU arm-nuttx-elf
toolchain* under Linux or Cygwin. See the https://www.olimex.com/dev/stm32-p107.html
for further information. Contributed by Max Holtzberg. STATUS: Configurations
for the basic OS test and NSH are available and verified.
configs/olimex-strp711
This port uses the Olimex STR-P711 board and a GNU arm-elf toolchain* under
This port uses the Olimex STR-P711 board and a GNU arm-nuttx-elf toolchain* under
Linux or Cygwin. See the http://www.olimex.com/dev/str-p711.html" for
further information. STATUS: Configurations for the basic OS test and NSH
are complete and verified.
nuttx/configs/px4fmu/common/Make.defs
+5 -13
View File
@@ -55,7 +55,7 @@ NM = $(CROSSDEV)nm
OBJCOPY = $(CROSSDEV)objcopy
OBJDUMP = $(CROSSDEV)objdump
MAXOPTIMIZATION = -Os
MAXOPTIMIZATION = -O3
ARCHCPUFLAGS = -mcpu=cortex-m4 \
-mthumb \
-march=armv7e-m \
@@ -116,7 +116,6 @@ ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \
ifeq ("${CONFIG_DEBUG_SYMBOLS}","y")
ARCHOPTIMIZATION += -g
ARCHSCRIPT += -g
endif
ARCHCFLAGS = -std=gnu99
@@ -149,7 +148,7 @@ ARCHDEFINES =
ARCHPICFLAGS = -fpic -msingle-pic-base -mpic-register=r10
# this seems to be the only way to add linker flags
ARCHSCRIPT += --warn-common \
EXTRA_LIBS += --warn-common \
--gc-sections
CFLAGS = $(ARCHCFLAGS) $(ARCHCWARNINGS) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -fno-common
@@ -167,13 +166,6 @@ OBJEXT = .o
LIBEXT = .a
EXEEXT =
# If VERBOSE is set, don't hide the compiler invocations.
ifeq ($(VERBOSE),YES)
_v =
else
_v = @
endif
define PREPROCESS
@echo "CPP: $1->$2"
@$(CPP) $(CPPFLAGS) $(abspath $1) -o $2
@@ -181,17 +173,17 @@ endef
define COMPILE
@echo "CC: $1"
$(_v)$(CC) -c $(CFLAGS) $(abspath $1) -o $2
$(Q)$(CC) -c $(CFLAGS) $(abspath $1) -o $2
endef
define COMPILEXX
@echo "CXX: $1"
$(_v)$(CXX) -c $(CXXFLAGS) $(abspath $1) -o $2
$(Q)$(CXX) -c $(CXXFLAGS) $(abspath $1) -o $2
endef
define ASSEMBLE
@echo "AS: $1"
$(_v)$(CC) -c $(AFLAGS) $(abspath $1) -o $2
$(Q)$(CC) -c $(AFLAGS) $(abspath $1) -o $2
endef
# produce partially-linked $1 from files in $2
nuttx/configs/px4fmu/include/drv_bma180.h
-99
View File
@@ -1,99 +0,0 @@
/*
* Copyright (C) 2012 Lorenz Meier. 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 of the author or the names of 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.
*/
/*
* Driver for the BOSCH BMA180 MEMS accelerometer
*/
/* IMPORTANT NOTES:
*
* SPI max. clock frequency: 25 Mhz
* CS has to be high before transfer,
* go low right before transfer and
* go high again right after transfer
*
*/
#include <sys/ioctl.h>
#define _BMA180BASE 0x6300
#define BMA180C(_x) _IOC(_BMA180BASE, _x)
/*
* Sets the sensor internal sampling rate, and if a buffer
* has been configured, the rate at which entries will be
* added to the buffer.
*/
#define BMA180_SETRATE BMA180C(1)
#define BMA180_RATE_LP_10HZ (0<<4)
#define BMA180_RATE_LP_20HZ (1<<4)
#define BMA180_RATE_LP_40HZ (2<<4)
#define BMA180_RATE_LP_75HZ (3<<4)
#define BMA180_RATE_LP_150HZ (4<<4)
#define BMA180_RATE_LP_300HZ (5<<4)
#define BMA180_RATE_LP_600HZ (6<<4)
#define BMA180_RATE_LP_1200HZ (7<<4)
/*
* Sets the sensor internal range.
*/
#define BMA180_SETRANGE BMA180C(2)
#define BMA180_RANGE_1G (0<<1)
#define BMA180_RANGE_1_5G (1<<1)
#define BMA180_RANGE_2G (2<<1)
#define BMA180_RANGE_3G (3<<1)
#define BMA180_RANGE_4G (4<<1)
#define BMA180_RANGE_8G (5<<1)
#define BMA180_RANGE_16G (6<<1)
/*
* Sets the address of a shared BMA180_buffer
* structure that is maintained by the driver.
*
* If zero is passed as the address, disables
* the buffer updating.
*/
#define BMA180_SETBUFFER BMA180C(3)
struct bma180_buffer {
uint32_t size; /* number of entries in the samples[] array */
uint32_t next; /* the next entry that will be populated */
struct {
uint16_t x;
uint16_t y;
uint16_t z;
uint8_t temp;
} samples[];
};
extern int bma180_attach(struct spi_dev_s *spi, int spi_id);
nuttx/configs/px4fmu/include/drv_hmc5883l.h
-100
View File
@@ -1,100 +0,0 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
/*
* Driver for the ST HMC5883L gyroscope
*/
#include <sys/ioctl.h>
#define _HMC5883LBASE 0x6100
#define HMC5883LC(_x) _IOC(_HMC5883LBASE, _x)
/*
* Sets the sensor internal sampling rate, and if a buffer
* has been configured, the rate at which entries will be
* added to the buffer.
*/
#define HMC5883L_SETRATE HMC5883LC(1)
/* set rate (configuration A register */
#define HMC5883L_RATE_0_75HZ (0 << 2) /* 0.75 Hz */
#define HMC5883L_RATE_1_50HZ (1 << 2) /* 1.5 Hz */
#define HMC5883L_RATE_3_00HZ (2 << 2) /* 3 Hz */
#define HMC5883L_RATE_7_50HZ (3 << 2) /* 7.5 Hz */
#define HMC5883L_RATE_15HZ (4 << 2) /* 15 Hz (default) */
#define HMC5883L_RATE_30HZ (5 << 2) /* 30 Hz */
#define HMC5883L_RATE_75HZ (6 << 2) /* 75 Hz */
/*
* Sets the sensor internal range.
*/
#define HMC5883L_SETRANGE HMC5883LC(2)
#define HMC5883L_RANGE_0_88GA (0 << 5)
#define HMC5883L_RANGE_1_33GA (1 << 5)
#define HMC5883L_RANGE_1_90GA (2 << 5)
#define HMC5883L_RANGE_2_50GA (3 << 5)
#define HMC5883L_RANGE_4_00GA (4 << 5)
/*
* Set the sensor measurement mode.
*/
#define HMC5883L_MODE_NORMAL (0 << 0)
#define HMC5883L_MODE_POSITIVE_BIAS (1 << 0)
#define HMC5883L_MODE_NEGATIVE_BIAS (1 << 1)
/*
* Sets the address of a shared HMC5883L_buffer
* structure that is maintained by the driver.
*
* If zero is passed as the address, disables
* the buffer updating.
*/
#define HMC5883L_SETBUFFER HMC5883LC(3)
struct hmc5883l_buffer {
uint32_t size; /* number of entries in the samples[] array */
uint32_t next; /* the next entry that will be populated */
struct {
int16_t x;
int16_t y;
int16_t z;
} samples[];
};
#define HMC5883L_RESET HMC5883LC(4)
#define HMC5883L_CALIBRATION_ON HMC5883LC(5)
#define HMC5883L_CALIBRATION_OFF HMC5883LC(6)
extern int hmc5883l_attach(struct i2c_dev_s *i2c);
nuttx/configs/px4fmu/include/drv_l3gd20.h
-108
View File
@@ -1,108 +0,0 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
/*
* Driver for the ST L3GD20 gyroscope
*/
/* IMPORTANT NOTES:
*
* SPI max. clock frequency: 10 Mhz
* CS has to be high before transfer,
* go low right before transfer and
* go high again right after transfer
*
*/
#include <sys/ioctl.h>
#define _L3GD20BASE 0x6200
#define L3GD20C(_x) _IOC(_L3GD20BASE, _x)
/*
* Sets the sensor internal sampling rate, and if a buffer
* has been configured, the rate at which entries will be
* added to the buffer.
*/
#define L3GD20_SETRATE L3GD20C(1)
#define L3GD20_RATE_95HZ_LP_12_5HZ ((0<<7) | (0<<6) | (0<<5) | (0<<4))
#define L3GD20_RATE_95HZ_LP_25HZ ((0<<7) | (0<<6) | (0<<5) | (1<<4))
#define L3GD20_RATE_190HZ_LP_12_5HZ ((0<<7) | (1<<6) | (0<<5) | (0<<4))
#define L3GD20_RATE_190HZ_LP_25HZ ((0<<7) | (1<<6) | (0<<5) | (1<<4))
#define L3GD20_RATE_190HZ_LP_50HZ ((0<<7) | (1<<6) | (1<<5) | (0<<4))
#define L3GD20_RATE_190HZ_LP_70HZ ((0<<7) | (1<<6) | (1<<5) | (1<<4))
#define L3GD20_RATE_380HZ_LP_20HZ ((1<<7) | (0<<6) | (0<<5) | (0<<4))
#define L3GD20_RATE_380HZ_LP_25HZ ((1<<7) | (0<<6) | (0<<5) | (1<<4))
#define L3GD20_RATE_380HZ_LP_50HZ ((1<<7) | (0<<6) | (1<<5) | (0<<4))
#define L3GD20_RATE_380HZ_LP_100HZ ((1<<7) | (0<<6) | (1<<5) | (1<<4))
#define L3GD20_RATE_760HZ_LP_30HZ ((1<<7) | (1<<6) | (0<<5) | (0<<4))
#define L3GD20_RATE_760HZ_LP_35HZ ((1<<7) | (1<<6) | (0<<5) | (1<<4))
#define L3GD20_RATE_760HZ_LP_50HZ ((1<<7) | (1<<6) | (1<<5) | (0<<4))
#define L3GD20_RATE_760HZ_LP_100HZ ((1<<7) | (1<<6) | (1<<5) | (1<<4))
/*
* Sets the sensor internal range.
*/
#define L3GD20_SETRANGE L3GD20C(2)
#define L3GD20_RANGE_250DPS (0<<4)
#define L3GD20_RANGE_500DPS (1<<4)
#define L3GD20_RANGE_2000DPS (3<<4)
#define L3GD20_RATE_95HZ ((0<<6) | (0<<4))
#define L3GD20_RATE_190HZ ((1<<6) | (0<<4))
#define L3GD20_RATE_380HZ ((2<<6) | (1<<4))
#define L3GD20_RATE_760HZ ((3<<6) | (2<<4))
/*
* Sets the address of a shared l3gd20_buffer
* structure that is maintained by the driver.
*
* If zero is passed as the address, disables
* the buffer updating.
*/
#define L3GD20_SETBUFFER L3GD20C(3)
struct l3gd20_buffer {
uint32_t size; /* number of entries in the samples[] array */
uint32_t next; /* the next entry that will be populated */
struct {
int16_t x;
int16_t y;
int16_t z;
} samples[];
};
extern int l3gd20_attach(struct spi_dev_s *spi, int spi_id);

Some files were not shown because too many files have changed in this diff Show More