Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-05 06:03:02 +08:00
Code Issues Actions 20 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
File diff suppressed because it is too large Load Diff

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