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

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

Browse Source
This commit is contained in:
Thomas Gubler
2012-09-29 14:21:04 +02:00
parent b1f9d5b14a fb691c9ff1
commit ebebc3a1ec
90 changed files with 4292 additions and 2554 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/ChangeLog.txt
+9
View File
@@ -336,3 +336,12 @@
can now be used to limit the server to a single thread. Option
CONFIG_NETUTILS_HTTPD_TIMEOUT can be used to generate HTTP 408 errors.
Both from Kate.
* apps/netutils/webserver/httpd.c: Improvements to HTTP parser from
Kate.
* 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
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/Kconfig
+4
View File
@@ -27,6 +27,10 @@ menu "NSH Library"
source "$APPSDIR/nshlib/Kconfig"
endmenu
menu "NxWidgets/NxWM"
source "$APPSDIR/NxWidgets/Kconfig"
endmenu
menu "System NSH Add-Ons"
source "$APPSDIR/system/Kconfig"
endmenu
apps/ardrone_interface/ardrone_interface.c
+2 -3
View File
@@ -1,7 +1,7 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Lorenz Meier <lm@inf.ethz.ch>
* Author: Lorenz Meier <lm@inf.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -140,9 +140,7 @@ static int ardrone_open_uart(char *uart_name, struct termios *uart_config_origin
int speed = B115200;
int uart;
/* open uart */
printf("[ardrone_interface] UART is %s, baud rate is%d\n",uart_name,speed);
uart = open(uart_name, O_RDWR | O_NOCTTY);
/* Try to set baud rate */
@@ -329,6 +327,7 @@ int ardrone_interface_thread_main(int argc, char *argv[])
*/
if (armed.armed && !armed.lockdown) {
ardrone_mixing_and_output(ardrone_write, &actuator_controls);
} else {
/* Silently lock down motor speeds to zero */
ardrone_write_motor_commands(ardrone_write, 0, 0, 0, 0);
apps/ardrone_interface/ardrone_motor_control.c
+4 -6
View File
@@ -306,7 +306,7 @@ int ar_init_motors(int ardrone_uart, int gpios)
return errcounter;
}
/*
/**
* Sets the leds on the motor controllers, 1 turns led on, 0 off.
*/
void ar_set_leds(int ardrone_uart, uint8_t led1_red, uint8_t led1_green, uint8_t led2_red, uint8_t led2_green, uint8_t led3_red, uint8_t led3_green, uint8_t led4_red, uint8_t led4_green)
@@ -368,10 +368,11 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls
float yaw_control = actuators->control[2];
float motor_thrust = actuators->control[3];
//printf("AMO: Roll: %4.4f, Pitch: %4.4f, Yaw: %4.4f, Thrust: %4.4f\n",roll_control, pitch_control, yaw_control, motor_thrust);
const float min_thrust = 0.02f; /**< 2% minimum thrust */
const float max_thrust = 1.0f; /**< 100% max thrust */
const float scaling = 512.0f; /**< 100% thrust equals a value of 512 */
const float scaling = 500.0f; /**< 100% thrust equals a value of 500 which works, 512 leads to cutoff */
const float min_gas = min_thrust * scaling; /**< value range sent to motors, minimum */
const float max_gas = max_thrust * scaling; /**< value range sent to motors, maximum */
@@ -387,13 +388,10 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls
if (motor_thrust <= min_thrust) {
motor_thrust = min_thrust;
output_band = 0.0f;
} else if (motor_thrust < startpoint_full_control && motor_thrust > min_thrust) {
output_band = band_factor * (motor_thrust - min_thrust);
} else if (motor_thrust >= startpoint_full_control && motor_thrust < max_thrust - band_factor * startpoint_full_control) {
output_band = band_factor * startpoint_full_control;
} else if (motor_thrust >= max_thrust - band_factor * startpoint_full_control) {
output_band = band_factor * (max_thrust - motor_thrust);
}
apps/ardrone_interface/ardrone_motor_control.h
+3
View File
@@ -87,4 +87,7 @@ int ar_init_motors(int ardrone_uart, int gpio);
*/
void ar_set_leds(int ardrone_uart, uint8_t led1_red, uint8_t led1_green, uint8_t led2_red, uint8_t led2_green, uint8_t led3_red, uint8_t led3_green, uint8_t led4_red, uint8_t led4_green);
/**
* Mix motors and output actuators
*/
void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls_s *actuators);
apps/attitude_estimator_ekf/Makefile
+5 -2
View File
@@ -33,7 +33,7 @@
APPNAME = attitude_estimator_ekf
PRIORITY = SCHED_PRIORITY_DEFAULT
STACKSIZE = 20000
STACKSIZE = 2048
CSRCS = attitude_estimator_ekf_main.c \
codegen/eye.c \
@@ -44,7 +44,10 @@ CSRCS = attitude_estimator_ekf_main.c \
codegen/rt_nonfinite.c \
codegen/rtGetInf.c \
codegen/rtGetNaN.c \
codegen/norm.c
codegen/norm.c \
codegen/find.c \
codegen/diag.c \
codegen/cross.c
# XXX this is *horribly* broken
INCLUDES += $(TOPDIR)/../mavlink/include/mavlink
apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
+195 -84
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2008-2012 PX4 Development Team. 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>
*
@@ -35,6 +35,7 @@
/*
* @file attitude_estimator_ekf_main.c
*
* Extended Kalman Filter for Attitude Estimation.
*/
@@ -54,6 +55,7 @@
#include <limits.h>
#include <math.h>
#include <uORB/uORB.h>
#include <uORB/topics/debug_key_value.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_attitude.h>
#include <arch/board/up_hrt.h>
@@ -69,34 +71,106 @@ __EXPORT int attitude_estimator_ekf_main(int argc, char *argv[]);
// #define PROJECTION_INITIALIZE_COUNTER_LIMIT 5000
// #define REPROJECTION_COUNTER_LIMIT 125
static unsigned int loop_interval_alarm = 4500; // loop interval in microseconds
static unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
static float dt = 1;
/* 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] = {0}; /**< Measurement vector */
static float x_aposteriori[12] = {0}; /**< */
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,
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, 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, 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, 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, 0, 0, 0, 100.f
static float x_aposteriori_k[9] = {0,0,0,0,0,-9.81,1,1,-1}; /**< */
static float x_aposteriori[9] = {0};
static float P_aposteriori_k[81] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0,
0, 100.f, 0, 0, 0, 0, 0, 0, 0,
0, 0, 100.f, 0, 0, 0, 0, 0, 0,
0, 0, 0, 100.f, 0, 0, 0, 0, 0,
0, 0, 0, 0, 100.f, 0, 0, 0, 0,
0, 0, 0, 0, 0, 100.f, 0, 0, 0,
0, 0, 0, 0, 0, 0, 100.f, 0, 0,
0, 0, 0, 0, 0, 0, 0, 100.f, 0,
0, 0, 0, 0, 0, 0, 0, 0, 100.f,
};
static float P_aposteriori[81] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0,
0, 100.f, 0, 0, 0, 0, 0, 0, 0,
0, 0, 100.f, 0, 0, 0, 0, 0, 0,
0, 0, 0, 100.f, 0, 0, 0, 0, 0,
0, 0, 0, 0, 100.f, 0, 0, 0, 0,
0, 0, 0, 0, 0, 100.f, 0, 0, 0,
0, 0, 0, 0, 0, 0, 100.f, 0, 0,
0, 0, 0, 0, 0, 0, 0, 100.f, 0,
0, 0, 0, 0, 0, 0, 0, 0, 100.f,
}; /**< init: diagonal matrix with big values */
static float knownConst[7] = {1, 1, 1, 1, 1, 1, 1}; /**< knownConst has the following entries [PrvaA,PrvarM,PrvarWO,PrvarW||MsvarA,MsvarM,MsvarW] */
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] */
static float Rot_matrix[9] = {1.f, 0, 0,
0, 1.f, 0,
0, 0, 1.f
}; /**< init: identity matrix */
// static float x_aposteriori_k[12] = {0};
// static float P_aposteriori_k[144] = {0};
static bool thread_should_exit = false; /**< Deamon exit flag */
static bool thread_running = false; /**< Deamon status flag */
static int attitude_estimator_ekf_task; /**< Handle of deamon task / thread */
/**
* Mainloop of attitude_estimator_ekf.
*/
int attitude_estimator_ekf_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: attitude_estimator_ekf {start|stop|status} [-p <additional params>]\n\n");
exit(1);
}
/**
* The attitude_estimator_ekf app only briefly exists to start
* the background job. The stack size assigned in the
* Makefile does only apply to this management task.
*
* The actual stack size should be set in the call
* to task_create().
*/
int attitude_estimator_ekf_main(int argc, char *argv[])
{
if (argc < 1)
usage("missing command");
if (!strcmp(argv[1], "start")) {
if (thread_running) {
printf("attitude_estimator_ekf already running\n");
/* this is not an error */
exit(0);
}
thread_should_exit = false;
attitude_estimator_ekf_task = task_create("attitude_estimator_ekf", SCHED_PRIORITY_DEFAULT, 20000, attitude_estimator_ekf_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("\tattitude_estimator_ekf app is running\n");
} else {
printf("\tattitude_estimator_ekf app not started\n");
}
exit(0);
}
usage("unrecognized command");
exit(1);
}
/*
* [Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst)
@@ -110,7 +184,7 @@ static float Rot_matrix[9] = {1.f, 0, 0,
* @param argc number of commandline arguments (plus command name)
* @param argv strings containing the arguments
*/
int attitude_estimator_ekf_main(int argc, char *argv[])
int attitude_estimator_ekf_thread_main(int argc, char *argv[])
{
// print text
printf("Extended Kalman Filter Attitude Estimator initialized..\n\n");
@@ -124,14 +198,17 @@ int attitude_estimator_ekf_main(int argc, char *argv[])
/* store start time to guard against too slow update rates */
uint64_t last_run = hrt_absolute_time();
struct sensor_combined_s raw = {0};
struct vehicle_attitude_s att = {};
struct sensor_combined_s raw;
struct vehicle_attitude_s att;
uint64_t last_data = 0;
uint64_t last_measurement = 0;
/* subscribe to raw data */
int sub_raw = orb_subscribe(ORB_ID(sensor_combined));
/* rate-limit raw data updates to 200Hz */
orb_set_interval(sub_raw, 5);
/* advertise attitude */
orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);
@@ -139,20 +216,26 @@ int attitude_estimator_ekf_main(int argc, char *argv[])
int loopcounter = 0;
int printcounter = 0;
thread_running = true;
/* advertise debug value */
struct debug_key_value_s dbg = { .key = "", .value = 0.0f };
orb_advert_t pub_dbg = orb_advertise(ORB_ID(debug_key_value), &dbg);
/* Main loop*/
while (true) {
while (!thread_should_exit) {
struct pollfd fds[1] = {
{ .fd = sub_raw, .events = POLLIN },
};
int ret = poll(fds, 1, 1000);
/* check for a timeout */
if (ret == 0) {
/* */
/* update successful, copy data on every 2nd run and execute filter */
} else if (loopcounter & 0x01) {
if (ret < 0) {
/* XXX this is seriously bad - should be an emergency */
} else if (ret == 0) {
/* XXX this means no sensor data - should be critical or emergency */
printf("[attitude estimator ekf] WARNING: Not getting sensor data - sensor app running?\n");
} else {
orb_copy(ORB_ID(sensor_combined), sub_raw, &raw);
@@ -160,24 +243,19 @@ int attitude_estimator_ekf_main(int argc, char *argv[])
dt = (raw.timestamp - last_measurement) / 1000000.0f;
last_measurement = raw.timestamp;
// XXX Read out accel range via SPI on init, assuming 4G range at 14 bit res here
float range_g = 4.0f;
float mag_offset[3] = {0};
/* scale from 14 bit to m/s2 */
z_k[3] = ((raw.accelerometer_raw[0] * range_g) / 8192.0f) / 9.81f; // = accel * (1 / 32768.0f / 8.0f * 9.81f);
z_k[4] = ((raw.accelerometer_raw[1] * range_g) / 8192.0f) / 9.81f; // = accel * (1 / 32768.0f / 8.0f * 9.81f);
z_k[5] = ((raw.accelerometer_raw[2] * range_g) / 8192.0f) / 9.81f; // = accel * (1 / 32768.0f / 8.0f * 9.81f);
// XXX Read out mag range via I2C on init, assuming 0.88 Ga and 12 bit res here
z_k[0] = (raw.magnetometer_raw[0] - mag_offset[0]) * 0.01f;
z_k[1] = (raw.magnetometer_raw[1] - mag_offset[1]) * 0.01f;
z_k[2] = (raw.magnetometer_raw[2] - mag_offset[2]) * 0.01f;
/* Fill in gyro measurements */
z_k[6] = raw.gyro_raw[0] * 0.00026631611f /* = gyro * (500.0f / 180.0f * pi / 32768.0f ) */;
z_k[7] = raw.gyro_raw[1] * 0.00026631611f /* = gyro * (500.0f / 180.0f * pi / 32768.0f ) */;
z_k[8] = raw.gyro_raw[2] * 0.00026631611f /* = gyro * (500.0f / 180.0f * pi / 32768.0f ) */;
z_k[0] = raw.gyro_rad_s[0];
z_k[1] = raw.gyro_rad_s[1];
z_k[2] = raw.gyro_rad_s[2];
/* 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];
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;
@@ -186,60 +264,97 @@ int attitude_estimator_ekf_main(int argc, char *argv[])
if (time_elapsed > loop_interval_alarm) {
//TODO: add warning, cpu overload here
if (overloadcounter == 20) {
printf("CPU OVERLOAD DETECTED IN ATTITUDE ESTIMATOR BLACK MAGIC (%lu > %lu)\n", time_elapsed, loop_interval_alarm);
printf("CPU OVERLOAD DETECTED IN ATTITUDE ESTIMATOR EKF (%lu > %lu)\n", time_elapsed, loop_interval_alarm);
overloadcounter = 0;
}
overloadcounter++;
}
// now = hrt_absolute_time();
/* filter values */
/*
* function [Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst)
*/
int8_t update_vect[9] = {1, 1, 1, 1, 1, 1, 1, 1, 1};
float euler[3];
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;
knownConst[0] = 0.6f*0.6f*dt;
knownConst[1] = 0.6f*0.6f*dt;
knownConst[2] = 0.2f*0.2f*dt;
knownConst[3] = 0.2f*0.2f*dt;
knownConst[4] = 0.000001f*0.000001f*dt; // -9.81,1,1,-1};
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] = z_k[3];
x_aposteriori_k[4] = z_k[4];
x_aposteriori_k[5] = z_k[5];
x_aposteriori_k[6] = z_k[6];
x_aposteriori_k[7] = z_k[7];
x_aposteriori_k[8] = z_k[8];
const_initialized = true;
}
/* do not execute the filter if not initialized */
if (!const_initialized) {
continue;
}
uint64_t timing_start = hrt_absolute_time();
attitudeKalmanfilter(dt, z_k, x_aposteriori, P_aposteriori, knownConst, Rot_matrix, x_aposteriori, P_aposteriori);
attitudeKalmanfilter(dt, update_vect, z_k, &z_k_sizes, u, x_aposteriori_k, P_aposteriori_k, knownConst, 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("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("\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));
if (printcounter % 2 == 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", (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++;
// time_elapsed = hrt_absolute_time() - now;
// if (blubb == 20)
// {
// printf("Estimator: %lu\n", time_elapsed);
// blubb = 0;
// }
// blubb++;
if (last_data > 0 && raw.timestamp - last_data > 12000) printf("[attitude estimator ekf] sensor data missed! (%llu)\n", raw.timestamp - last_data);
if (last_data > 0 && raw.timestamp - last_data > 8000) printf("sensor data missed! (%llu)\n", raw.timestamp - last_data);
// printf("%llu -> %llu = %llu\n", last_data, raw.timestamp, raw.timestamp - last_data);
last_data = raw.timestamp;
/* send out */
att.timestamp = raw.timestamp;
// att.roll = euler.x;
// att.pitch = euler.y;
// att.yaw = euler.z + F_M_PI;
att.roll = euler[0];
att.pitch = euler[1];
att.yaw = euler[2];
// if (att.yaw > 2 * F_M_PI) {
// att.yaw -= 2 * F_M_PI;
// }
// att.rollspeed = rates.x;
// att.pitchspeed = rates.y;
// att.yawspeed = rates.z;
// memcpy()R
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);
@@ -248,11 +363,7 @@ int attitude_estimator_ekf_main(int argc, char *argv[])
loopcounter++;
}
/* Should never reach here */
thread_running = false;
return 0;
}
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c
+556 -475
View File
File diff suppressed because it is too large Load Diff
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h
+4 -2
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Wed Jul 11 08:38:35 2012
* C source code generated on: Fri Sep 21 13:56:43 2012
*
*/
@@ -14,6 +14,8 @@
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "rt_defines.h"
#include "rt_nonfinite.h"
#include "rtwtypes.h"
#include "attitudeKalmanfilter_types.h"
@@ -27,6 +29,6 @@
/* Variable Definitions */
/* Function Declarations */
extern void attitudeKalmanfilter(real32_T dt, const real32_T z_k[9], const real32_T x_aposteriori_k[12], const real32_T P_aposteriori_k[144], const real32_T knownConst[7], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T P_aposteriori[144]);
extern void attitudeKalmanfilter(real32_T dt, const int8_T updVect[9], const real32_T z_k_data[9], const int32_T z_k_sizes[1], const real32_T u[4], const real32_T x_aposteriori_k[9], const real32_T P_aposteriori_k[81], const real32_T knownConst[20], real32_T eulerAngles[3], real32_T Rot_matrix[9], real32_T x_aposteriori[9], real32_T P_aposteriori[81]);
#endif
/* End of code generation (attitudeKalmanfilter.h) */
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c
+1 -1
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter_initialize'
*
* C source code generated on: Wed Jul 11 08:38:35 2012
* C source code generated on: Fri Sep 21 13:56:44 2012
*
*/
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h
+3 -1
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter_initialize'
*
* C source code generated on: Wed Jul 11 08:38:35 2012
* C source code generated on: Fri Sep 21 13:56:44 2012
*
*/
@@ -14,6 +14,8 @@
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "rt_defines.h"
#include "rt_nonfinite.h"
#include "rtwtypes.h"
#include "attitudeKalmanfilter_types.h"
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_ref.rsp
View File
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_rtw.bat
-11
View File
@@ -1,11 +0,0 @@
@echo off
call "%VS100COMNTOOLS%..\..\VC\vcvarsall.bat" AMD64
cd .
nmake -f attitudeKalmanfilter_rtw.mk GENERATE_REPORT=1 ADD_MDL_NAME_TO_GLOBALS=0
@if errorlevel 1 goto error_exit
exit /B 0
:error_exit
echo The make command returned an error of %errorlevel%
An_error_occurred_during_the_call_to_make
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_rtw.mk
-328
View File
@@ -1,328 +0,0 @@
# Copyright 1994-2010 The MathWorks, Inc.
#
# File : xrt_vcx64.tmf $Revision: 1.1.6.1 $
#
# Abstract:
# Code generation template makefile for building a Windows-based,
# stand-alone real-time version of MATLAB-code using generated C/C++
# code and the
# Microsoft Visual C/C++ compiler version 8, 9, 10 for x64
#
# Note that this template is automatically customized by the
# code generation build procedure to create "<model>.mk"
#
# The following defines can be used to modify the behavior of the
# build:
#
# OPT_OPTS - Optimization option. See DEFAULT_OPT_OPTS in
# vctools.mak for default.
# OPTS - User specific options.
# CPP_OPTS - C++ compiler options.
# USER_SRCS - Additional user sources, such as files needed by
# S-functions.
# USER_INCLUDES - Additional include paths
# (i.e. USER_INCLUDES="-Iwhere-ever -Iwhere-ever2")
#
# To enable debugging:
# set OPT_OPTS=-Zi (may vary with compiler version, see compiler doc)
# modify tmf LDFLAGS to include /DEBUG
#
#------------------------ Macros read by make_rtw -----------------------------
#
# The following macros are read by the code generation build procedure:
#
# MAKECMD - This is the command used to invoke the make utility
# HOST - What platform this template makefile is targeted for
# (i.e. PC or UNIX)
# BUILD - Invoke make from the code generation build procedure
# (yes/no)?
# SYS_TARGET_FILE - Name of system target file.
MAKECMD = nmake
HOST = PC
BUILD = yes
SYS_TARGET_FILE = ert.tlc
BUILD_SUCCESS = ^#^#^# Created
COMPILER_TOOL_CHAIN = vcx64
#---------------------- Tokens expanded by make_rtw ---------------------------
#
# The following tokens, when wrapped with "|>" and "<|" are expanded by the
# code generation build procedure.
#
# MODEL_NAME - Name of the Simulink block diagram
# MODEL_MODULES - Any additional generated source modules
# MAKEFILE_NAME - Name of makefile created from template makefile <model>.mk
# MATLAB_ROOT - Path to where MATLAB is installed.
# MATLAB_BIN - Path to MATLAB executable.
# S_FUNCTIONS - List of S-functions.
# S_FUNCTIONS_LIB - List of S-functions libraries to link.
# SOLVER - Solver source file name
# NUMST - Number of sample times
# TID01EQ - yes (1) or no (0): Are sampling rates of continuous task
# (tid=0) and 1st discrete task equal.
# NCSTATES - Number of continuous states
# BUILDARGS - Options passed in at the command line.
# MULTITASKING - yes (1) or no (0): Is solver mode multitasking
# EXT_MODE - yes (1) or no (0): Build for external mode
# TMW_EXTMODE_TESTING - yes (1) or no (0): Build ext_test.c for external mode
# testing.
# EXTMODE_TRANSPORT - Index of transport mechanism (e.g. tcpip, serial) for extmode
# EXTMODE_STATIC - yes (1) or no (0): Use static instead of dynamic mem alloc.
# EXTMODE_STATIC_SIZE - Size of static memory allocation buffer.
MODEL = attitudeKalmanfilter
MODULES = attitudeKalmanfilter.c eye.c mrdivide.c norm.c attitudeKalmanfilter_initialize.c attitudeKalmanfilter_terminate.c rt_nonfinite.c rtGetNaN.c rtGetInf.c
MAKEFILE = attitudeKalmanfilter_rtw.mk
MATLAB_ROOT = C:\Program Files\MATLAB\R2011a
ALT_MATLAB_ROOT = C:\PROGRA~1\MATLAB\R2011a
MATLAB_BIN = C:\Program Files\MATLAB\R2011a\bin
ALT_MATLAB_BIN = C:\PROGRA~1\MATLAB\R2011a\bin
S_FUNCTIONS =
S_FUNCTIONS_LIB =
SOLVER =
NUMST = 1
TID01EQ = 0
NCSTATES = 0
BUILDARGS = GENERATE_REPORT=1 ADD_MDL_NAME_TO_GLOBALS=0
MULTITASKING = 0
EXT_MODE = 0
TMW_EXTMODE_TESTING = 0
EXTMODE_TRANSPORT = 0
EXTMODE_STATIC = 0
EXTMODE_STATIC_SIZE = 1000000
MODELREFS =
SHARED_SRC =
SHARED_SRC_DIR =
SHARED_BIN_DIR =
SHARED_LIB =
TARGET_LANG_EXT = c
OPTIMIZATION_FLAGS = /O2 /Oy-
ADDITIONAL_LDFLAGS =
#--------------------------- Model and reference models -----------------------
MODELLIB = attitudeKalmanfilter.lib
MODELREF_LINK_LIBS =
MODELREF_LINK_RSPFILE = attitudeKalmanfilter_ref.rsp
MODELREF_INC_PATH =
RELATIVE_PATH_TO_ANCHOR = F:\CODEGE~1
MODELREF_TARGET_TYPE = RTW
!if "$(MATLAB_ROOT)" != "$(ALT_MATLAB_ROOT)"
MATLAB_ROOT = $(ALT_MATLAB_ROOT)
!endif
!if "$(MATLAB_BIN)" != "$(ALT_MATLAB_BIN)"
MATLAB_BIN = $(ALT_MATLAB_BIN)
!endif
#--------------------------- Tool Specifications ------------------------------
CPU = AMD64
!include $(MATLAB_ROOT)\rtw\c\tools\vctools.mak
APPVER = 5.02
PERL = $(MATLAB_ROOT)\sys\perl\win32\bin\perl
#------------------------------ Include/Lib Path ------------------------------
MATLAB_INCLUDES = $(MATLAB_ROOT)\simulink\include
MATLAB_INCLUDES = $(MATLAB_INCLUDES);$(MATLAB_ROOT)\extern\include
MATLAB_INCLUDES = $(MATLAB_INCLUDES);$(MATLAB_ROOT)\rtw\c\src
MATLAB_INCLUDES = $(MATLAB_INCLUDES);$(MATLAB_ROOT)\rtw\c\src\ext_mode\common
# Additional file include paths
MATLAB_INCLUDES = $(MATLAB_INCLUDES);F:\codegenerationMatlabAttFilter\codegen\lib\attitudeKalmanfilter
MATLAB_INCLUDES = $(MATLAB_INCLUDES);F:\codegenerationMatlabAttFilter
INCLUDE = .;$(RELATIVE_PATH_TO_ANCHOR);$(MATLAB_INCLUDES);$(INCLUDE);$(MODELREF_INC_PATH)
!if "$(SHARED_SRC_DIR)" != ""
INCLUDE = $(INCLUDE);$(SHARED_SRC_DIR)
!endif
#------------------------ External mode ---------------------------------------
# Uncomment -DVERBOSE to have information printed to stdout
# To add a new transport layer, see the comments in
# <matlabroot>/toolbox/simulink/simulink/extmode_transports.m
!if $(EXT_MODE) == 1
EXT_CC_OPTS = -DEXT_MODE # -DVERBOSE
!if $(EXTMODE_TRANSPORT) == 0 #tcpip
EXT_SRC = updown.c rtiostream_interface.c rtiostream_tcpip.c
EXT_LIB = wsock32.lib
!endif
!if $(EXTMODE_TRANSPORT) == 1 #serial_win32
EXT_SRC = ext_svr.c updown.c ext_work.c ext_svr_serial_transport.c
EXT_SRC = $(EXT_SRC) ext_serial_pkt.c ext_serial_win32_port.c
EXT_LIB =
!endif
!if $(TMW_EXTMODE_TESTING) == 1
EXT_SRC = $(EXT_SRC) ext_test.c
EXT_CC_OPTS = $(EXT_CC_OPTS) -DTMW_EXTMODE_TESTING
!endif
!if $(EXTMODE_STATIC) == 1
EXT_SRC = $(EXT_SRC) mem_mgr.c
EXT_CC_OPTS = $(EXT_CC_OPTS) -DEXTMODE_STATIC -DEXTMODE_STATIC_SIZE=$(EXTMODE_STATIC_SIZE)
!endif
!else
EXT_SRC =
EXT_CC_OPTS =
EXT_LIB =
!endif
#------------------------ rtModel ----------------------------------------------
RTM_CC_OPTS = -DUSE_RTMODEL
#----------------- Compiler and Linker Options --------------------------------
# Optimization Options
# Set OPT_OPTS=-Zi for debugging (may depend on compiler version)
OPT_OPTS = $(DEFAULT_OPT_OPTS)
# General User Options
OPTS =
!if "$(OPTIMIZATION_FLAGS)" != ""
CC_OPTS = $(OPTS) $(EXT_CC_OPTS) $(RTM_CC_OPTS) $(OPTIMIZATION_FLAGS)
!else
CC_OPTS = $(OPT_OPTS) $(OPTS) $(EXT_CC_OPTS) $(RTM_CC_OPTS)
!endif
CPP_REQ_DEFINES = -DMODEL=$(MODEL) -DRT -DNUMST=$(NUMST) \
-DTID01EQ=$(TID01EQ) -DNCSTATES=$(NCSTATES) \
-DMT=$(MULTITASKING) -DHAVESTDIO
# Uncomment this line to move warning level to W4
# cflags = $(cflags:W3=W4)
CFLAGS = $(CC_OPTS) $(CPP_REQ_DEFINES) $(USER_INCLUDES) \
$(cflags) $(cvarsmt) /wd4996
CPPFLAGS = $(CPP_OPTS) $(CC_OPTS) $(CPP_REQ_DEFINES) $(USER_INCLUDES) \
$(cflags) $(cvarsmt) /wd4996 /EHsc-
LDFLAGS = $(ldebug) $(conflags) $(EXT_LIB) $(conlibs) libcpmt.lib $(ADDITIONAL_LDFLAGS)
# libcpmt.lib is the multi-threaded, static lib version of the C++ standard lib
#----------------------------- Source Files -----------------------------------
#Standalone executable
!if "$(MODELREF_TARGET_TYPE)" == "NONE"
PRODUCT = $(RELATIVE_PATH_TO_ANCHOR)\$(MODEL).exe
REQ_SRCS = $(MODULES) $(EXT_SRC)
#Model Reference Target
!else
PRODUCT = $(MODELLIB)
REQ_SRCS = $(MODULES) $(EXT_SRC)
!endif
USER_SRCS =
SRCS = $(REQ_SRCS) $(USER_SRCS) $(S_FUNCTIONS)
OBJS_CPP_UPPER = $(SRCS:.CPP=.obj)
OBJS_CPP_LOWER = $(OBJS_CPP_UPPER:.cpp=.obj)
OBJS_C_UPPER = $(OBJS_CPP_LOWER:.C=.obj)
OBJS = $(OBJS_C_UPPER:.c=.obj)
SHARED_OBJS = $(SHARED_SRC:.c=.obj)
# ------------------------- Additional Libraries ------------------------------
LIBS =
LIBS = $(LIBS)
# ---------------------------- Linker Script ----------------------------------
CMD_FILE = $(MODEL).lnk
GEN_LNK_SCRIPT = $(MATLAB_ROOT)\rtw\c\tools\mkvc_lnk.pl
#--------------------------------- Rules --------------------------------------
all: set_environment_variables $(PRODUCT)
!if "$(MODELREF_TARGET_TYPE)" == "NONE"
#--- Stand-alone model ---
$(PRODUCT) : $(OBJS) $(SHARED_LIB) $(LIBS) $(MODELREF_LINK_LIBS)
@cmd /C "echo ### Linking ..."
$(PERL) $(GEN_LNK_SCRIPT) $(CMD_FILE) $(OBJS)
$(LD) $(LDFLAGS) $(S_FUNCTIONS_LIB) $(SHARED_LIB) $(LIBS) $(MAT_LIBS) @$(CMD_FILE) @$(MODELREF_LINK_RSPFILE) -out:$@
@del $(CMD_FILE)
@cmd /C "echo $(BUILD_SUCCESS) executable $(MODEL).exe"
!else
#--- Model reference code generation Target ---
$(PRODUCT) : $(OBJS) $(SHARED_LIB)
@cmd /C "echo ### Linking ..."
$(PERL) $(GEN_LNK_SCRIPT) $(CMD_FILE) $(OBJS)
$(LD) -lib /OUT:$(PRODUCT) @$(CMD_FILE) $(S_FUNCTIONS_LIB)
@cmd /C "echo $(BUILD_SUCCESS) static library $(MODELLIB)"
!endif
{$(MATLAB_ROOT)\rtw\c\src}.c.obj :
@cmd /C "echo ### Compiling $<"
$(CC) $(CFLAGS) $<
{$(MATLAB_ROOT)\rtw\c\src\ext_mode\common}.c.obj :
@cmd /C "echo ### Compiling $<"
$(CC) $(CFLAGS) $<
{$(MATLAB_ROOT)\rtw\c\src\rtiostream\rtiostreamtcpip}.c.obj :
@cmd /C "echo ### Compiling $<"
$(CC) $(CFLAGS) $<
{$(MATLAB_ROOT)\rtw\c\src\ext_mode\serial}.c.obj :
@cmd /C "echo ### Compiling $<"
$(CC) $(CFLAGS) $<
{$(MATLAB_ROOT)\rtw\c\src\ext_mode\custom}.c.obj :
@cmd /C "echo ### Compiling $<"
$(CC) $(CFLAGS) $<
# Additional sources
# Put these rule last, otherwise nmake will check toolboxes first
{$(RELATIVE_PATH_TO_ANCHOR)}.c.obj :
@cmd /C "echo ### Compiling $<"
$(CC) $(CFLAGS) $<
{$(RELATIVE_PATH_TO_ANCHOR)}.cpp.obj :
@cmd /C "echo ### Compiling $<"
$(CC) $(CPPFLAGS) $<
.c.obj :
@cmd /C "echo ### Compiling $<"
$(CC) $(CFLAGS) $<
.cpp.obj :
@cmd /C "echo ### Compiling $<"
$(CC) $(CPPFLAGS) $<
!if "$(SHARED_LIB)" != ""
$(SHARED_LIB) : $(SHARED_SRC)
@cmd /C "echo ### Creating $@"
@$(CC) $(CFLAGS) -Fo$(SHARED_BIN_DIR)\ @<<
$?
<<
@$(LIBCMD) /nologo /out:$@ $(SHARED_OBJS)
@cmd /C "echo ### $@ Created"
!endif
set_environment_variables:
@set INCLUDE=$(INCLUDE)
@set LIB=$(LIB)
# Libraries:
#----------------------------- Dependencies -----------------------------------
$(OBJS) : $(MAKEFILE) rtw_proj.tmw
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c
+1 -1
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter_terminate'
*
* C source code generated on: Wed Jul 11 08:38:35 2012
* C source code generated on: Fri Sep 21 13:56:44 2012
*
*/
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h
+3 -1
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter_terminate'
*
* C source code generated on: Wed Jul 11 08:38:35 2012
* C source code generated on: Fri Sep 21 13:56:44 2012
*
*/
@@ -14,6 +14,8 @@
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "rt_defines.h"
#include "rt_nonfinite.h"
#include "rtwtypes.h"
#include "attitudeKalmanfilter_types.h"
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h
+1 -1
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Wed Jul 11 08:38:35 2012
* C source code generated on: Fri Sep 21 13:56:42 2012
*
*/
apps/attitude_estimator_ekf/codegen/buildInfo.mat
BIN
View File
Binary file not shown.
apps/attitude_estimator_ekf/codegen/cross.c
Executable
+37
View File
@@ -0,0 +1,37 @@
/*
* cross.c
*
* Code generation for function 'cross'
*
* C source code generated on: Fri Sep 21 13:56:44 2012
*
*/
/* Include files */
#include "rt_nonfinite.h"
#include "attitudeKalmanfilter.h"
#include "cross.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
/* Function Definitions */
/*
*
*/
void cross(const real32_T a[3], const real32_T b[3], real32_T c[3])
{
c[0] = a[1] * b[2] - a[2] * b[1];
c[1] = a[2] * b[0] - a[0] * b[2];
c[2] = a[0] * b[1] - a[1] * b[0];
}
/* End of code generation (cross.c) */
apps/attitude_estimator_ekf/codegen/cross.h
Executable
+34
View File
@@ -0,0 +1,34 @@
/*
* cross.h
*
* Code generation for function 'cross'
*
* C source code generated on: Fri Sep 21 13:56:44 2012
*
*/
#ifndef __CROSS_H__
#define __CROSS_H__
/* Include files */
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "rt_defines.h"
#include "rt_nonfinite.h"
#include "rtwtypes.h"
#include "attitudeKalmanfilter_types.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
extern void cross(const real32_T a[3], const real32_T b[3], real32_T c[3]);
#endif
/* End of code generation (cross.h) */
apps/attitude_estimator_ekf/codegen/diag.c
Executable
+42
View File
@@ -0,0 +1,42 @@
/*
* diag.c
*
* Code generation for function 'diag'
*
* C source code generated on: Fri Sep 21 13:56:43 2012
*
*/
/* Include files */
#include "rt_nonfinite.h"
#include "attitudeKalmanfilter.h"
#include "diag.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
/* Function Definitions */
/*
*
*/
void diag(const real32_T v[3], real32_T d[9])
{
int32_T j;
for (j = 0; j < 9; j++) {
d[j] = 0.0F;
}
for (j = 0; j < 3; j++) {
d[j + 3 * j] = v[j];
}
}
/* End of code generation (diag.c) */
apps/attitude_estimator_ekf/codegen/diag.h
Executable
+34
View File
@@ -0,0 +1,34 @@
/*
* diag.h
*
* Code generation for function 'diag'
*
* C source code generated on: Fri Sep 21 13:56:43 2012
*
*/
#ifndef __DIAG_H__
#define __DIAG_H__
/* Include files */
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "rt_defines.h"
#include "rt_nonfinite.h"
#include "rtwtypes.h"
#include "attitudeKalmanfilter_types.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
extern void diag(const real32_T v[3], real32_T d[9]);
#endif
/* End of code generation (diag.h) */
apps/attitude_estimator_ekf/codegen/eye.c
+5 -5
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'eye'
*
* C source code generated on: Wed Jul 11 08:38:35 2012
* C source code generated on: Fri Sep 21 13:56:43 2012
*
*/
@@ -27,12 +27,12 @@
/*
*
*/
void b_eye(real_T I[144])
void b_eye(real_T I[81])
{
int32_T i;
memset((void *)&I[0], 0, 144U * sizeof(real_T));
for (i = 0; i < 12; i++) {
I[i + 12 * i] = 1.0;
memset((void *)&I[0], 0, 81U * sizeof(real_T));
for (i = 0; i < 9; i++) {
I[i + 9 * i] = 1.0;
}
}
apps/attitude_estimator_ekf/codegen/eye.h
+4 -2
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'eye'
*
* C source code generated on: Wed Jul 11 08:38:35 2012
* C source code generated on: Fri Sep 21 13:56:43 2012
*
*/
@@ -14,6 +14,8 @@
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "rt_defines.h"
#include "rt_nonfinite.h"
#include "rtwtypes.h"
#include "attitudeKalmanfilter_types.h"
@@ -27,7 +29,7 @@
/* Variable Definitions */
/* Function Declarations */
extern void b_eye(real_T I[144]);
extern void b_eye(real_T I[81]);
extern void eye(real_T I[9]);
#endif
/* End of code generation (eye.h) */
apps/attitude_estimator_ekf/codegen/find.c
Executable
+77
View File
@@ -0,0 +1,77 @@
/*
* find.c
*
* Code generation for function 'find'
*
* C source code generated on: Fri Sep 21 13:56:43 2012
*
*/
/* Include files */
#include "rt_nonfinite.h"
#include "attitudeKalmanfilter.h"
#include "find.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
/* Function Definitions */
/*
*
*/
void find(const int8_T x[9], real_T i_data[9], int32_T i_sizes[1])
{
int32_T idx;
int32_T ii;
boolean_T exitg1;
boolean_T guard1 = FALSE;
int32_T i2;
int8_T b_i_data[9];
idx = 0;
i_sizes[0] = 9;
ii = 1;
exitg1 = 0U;
while ((exitg1 == 0U) && (ii <= 9)) {
guard1 = FALSE;
if (x[ii - 1] != 0) {
idx++;
i_data[idx - 1] = (real_T)ii;
if (idx >= 9) {
exitg1 = 1U;
} else {
guard1 = TRUE;
}
} else {
guard1 = TRUE;
}
if (guard1 == TRUE) {
ii++;
}
}
if (1 > idx) {
idx = 0;
}
ii = idx - 1;
for (i2 = 0; i2 <= ii; i2++) {
b_i_data[i2] = (int8_T)i_data[i2];
}
i_sizes[0] = idx;
ii = idx - 1;
for (i2 = 0; i2 <= ii; i2++) {
i_data[i2] = (real_T)b_i_data[i2];
}
}
/* End of code generation (find.c) */
apps/attitude_estimator_ekf/codegen/find.h
Executable
+34
View File
@@ -0,0 +1,34 @@
/*
* find.h
*
* Code generation for function 'find'
*
* C source code generated on: Fri Sep 21 13:56:43 2012
*
*/
#ifndef __FIND_H__
#define __FIND_H__
/* Include files */
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "rt_defines.h"
#include "rt_nonfinite.h"
#include "rtwtypes.h"
#include "attitudeKalmanfilter_types.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
extern void find(const int8_T x[9], real_T i_data[9], int32_T i_sizes[1]);
#endif
/* End of code generation (find.h) */
apps/attitude_estimator_ekf/codegen/mrdivide.c
+667 -84
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'mrdivide'
*
* C source code generated on: Wed Jul 11 08:38:35 2012
* C source code generated on: Fri Sep 21 13:56:44 2012
*
*/
@@ -21,136 +21,719 @@
/* Variable Definitions */
/* Function Declarations */
static real32_T b_eml_matlab_zlarfg(real32_T *alpha1, const real32_T *x);
static void eml_lusolve(const real32_T A_data[81], const int32_T A_sizes[2],
real32_T B_data[81], int32_T B_sizes[2]);
static real32_T eml_matlab_zlarfg(int32_T n, real32_T *alpha1, real32_T x_data
[81], int32_T x_sizes[2], int32_T ix0);
static void eml_qrsolve(const real32_T A_data[81], const int32_T A_sizes[2],
real32_T B_data[81], int32_T B_sizes[2], real32_T Y_data[81], int32_T Y_sizes
[2]);
static real32_T eml_xnrm2(int32_T n, const real32_T x_data[81], const int32_T
x_sizes[2], int32_T ix0);
/* Function Definitions */
/*
*
*/
void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108])
static real32_T b_eml_matlab_zlarfg(real32_T *alpha1, const real32_T *x)
{
int32_T jy;
return 0.0F;
}
/*
*
*/
static void eml_lusolve(const real32_T A_data[81], const int32_T A_sizes[2],
real32_T B_data[81], int32_T B_sizes[2])
{
int32_T loop_ub;
int32_T iy;
real32_T b_A[81];
int8_T ipiv[9];
real32_T b_A_data[81];
int32_T jA;
int32_T tmp_data[9];
int32_T ipiv_data[9];
int32_T ldap1;
int32_T j;
int32_T mmj;
int32_T jj;
int32_T jp1j;
int32_T c;
int32_T ix;
real32_T temp;
int32_T k;
real32_T s;
int32_T loop_ub;
real32_T Y[108];
for (jy = 0; jy < 9; jy++) {
for (iy = 0; iy < 9; iy++) {
b_A[iy + 9 * jy] = B[jy + 9 * iy];
}
ipiv[jy] = (int8_T)(1 + jy);
int32_T jrow;
int32_T b_loop_ub;
int32_T c;
loop_ub = A_sizes[0] * A_sizes[1] - 1;
for (iy = 0; iy <= loop_ub; iy++) {
b_A_data[iy] = A_data[iy];
}
for (j = 0; j < 8; j++) {
mmj = -j;
jj = j * 10;
jp1j = jj + 1;
c = mmj + 9;
jy = 0;
ix = jj;
temp = fabsf(b_A[jj]);
for (k = 2; k <= c; k++) {
ix++;
s = fabsf(b_A[ix]);
if (s > temp) {
jy = k - 1;
temp = s;
iy = A_sizes[1];
jA = A_sizes[1];
jA = iy <= jA ? iy : jA;
if (jA < 1) {
} else {
loop_ub = jA - 1;
for (iy = 0; iy <= loop_ub; iy++) {
tmp_data[iy] = 1 + iy;
}
loop_ub = jA - 1;
for (iy = 0; iy <= loop_ub; iy++) {
ipiv_data[iy] = tmp_data[iy];
}
}
ldap1 = A_sizes[1] + 1;
iy = A_sizes[1] - 1;
jA = A_sizes[1];
loop_ub = iy <= jA ? iy : jA;
for (j = 1; j <= loop_ub; j++) {
mmj = (A_sizes[1] - j) + 1;
jj = (j - 1) * ldap1;
if (mmj < 1) {
jA = -1;
} else {
jA = 0;
if (mmj > 1) {
ix = jj;
temp = (real32_T)fabs(b_A_data[jj]);
for (k = 1; k + 1 <= mmj; k++) {
ix++;
s = (real32_T)fabs(b_A_data[ix]);
if (s > temp) {
jA = k;
temp = s;
}
}
}
}
if ((real_T)b_A[jj + jy] != 0.0) {
if (jy != 0) {
ipiv[j] = (int8_T)((j + jy) + 1);
ix = j;
iy = j + jy;
for (k = 0; k < 9; k++) {
temp = b_A[ix];
b_A[ix] = b_A[iy];
b_A[iy] = temp;
ix += 9;
iy += 9;
if ((real_T)b_A_data[jj + jA] != 0.0) {
if (jA != 0) {
ipiv_data[j - 1] = j + jA;
jrow = j - 1;
iy = jrow + jA;
for (k = 1; k <= A_sizes[1]; k++) {
temp = b_A_data[jrow];
b_A_data[jrow] = b_A_data[iy];
b_A_data[iy] = temp;
jrow += A_sizes[1];
iy += A_sizes[1];
}
}
loop_ub = (jp1j + mmj) + 8;
for (iy = jp1j; iy + 1 <= loop_ub; iy++) {
b_A[iy] /= b_A[jj];
b_loop_ub = jj + mmj;
for (jA = jj + 1; jA + 1 <= b_loop_ub; jA++) {
b_A_data[jA] /= b_A_data[jj];
}
}
c = 8 - j;
jy = jj + 9;
for (iy = 1; iy <= c; iy++) {
if ((real_T)b_A[jy] != 0.0) {
temp = b_A[jy] * -1.0F;
ix = jp1j;
loop_ub = (mmj + jj) + 18;
for (k = 10 + jj; k + 1 <= loop_ub; k++) {
b_A[k] += b_A[ix] * temp;
c = A_sizes[1] - j;
jA = jj + ldap1;
iy = jj + A_sizes[1];
for (jrow = 1; jrow <= c; jrow++) {
if ((real_T)b_A_data[iy] != 0.0) {
temp = b_A_data[iy] * -1.0F;
ix = jj;
b_loop_ub = (mmj + jA) - 1;
for (k = jA; k + 1 <= b_loop_ub; k++) {
b_A_data[k] += b_A_data[ix + 1] * temp;
ix++;
}
}
jy += 9;
jj += 9;
iy += A_sizes[1];
jA += A_sizes[1];
}
}
for (jy = 0; jy < 12; jy++) {
for (iy = 0; iy < 9; iy++) {
Y[iy + 9 * jy] = A[jy + 12 * iy];
}
}
for (iy = 0; iy < 9; iy++) {
if (ipiv[iy] != iy + 1) {
for (j = 0; j < 12; j++) {
temp = Y[iy + 9 * j];
Y[iy + 9 * j] = Y[(ipiv[iy] + 9 * j) - 1];
Y[(ipiv[iy] + 9 * j) - 1] = temp;
for (jA = 0; jA + 1 <= A_sizes[1]; jA++) {
if (ipiv_data[jA] != jA + 1) {
for (j = 0; j < 9; j++) {
temp = B_data[jA + B_sizes[0] * j];
B_data[jA + B_sizes[0] * j] = B_data[(ipiv_data[jA] + B_sizes[0] * j) -
1];
B_data[(ipiv_data[jA] + B_sizes[0] * j) - 1] = temp;
}
}
}
for (j = 0; j < 12; j++) {
c = 9 * j;
for (k = 0; k < 9; k++) {
jy = 9 * k;
if ((real_T)Y[k + c] != 0.0) {
for (iy = k + 2; iy < 10; iy++) {
Y[(iy + c) - 1] -= Y[k + c] * b_A[(iy + jy) - 1];
if (B_sizes[0] == 0) {
} else {
for (j = 0; j < 9; j++) {
c = A_sizes[1] * j;
for (k = 0; k + 1 <= A_sizes[1]; k++) {
iy = A_sizes[1] * k;
if ((real_T)B_data[k + c] != 0.0) {
for (jA = k + 1; jA + 1 <= A_sizes[1]; jA++) {
B_data[jA + c] -= B_data[k + c] * b_A_data[jA + iy];
}
}
}
}
}
for (j = 0; j < 12; j++) {
c = 9 * j;
for (k = 8; k > -1; k += -1) {
jy = 9 * k;
if ((real_T)Y[k + c] != 0.0) {
Y[k + c] /= b_A[k + jy];
for (iy = 0; iy + 1 <= k; iy++) {
Y[iy + c] -= Y[k + c] * b_A[iy + jy];
if (B_sizes[0] == 0) {
} else {
for (j = 0; j < 9; j++) {
c = A_sizes[1] * j;
for (k = A_sizes[1] - 1; k + 1 > 0; k--) {
iy = A_sizes[1] * k;
if ((real_T)B_data[k + c] != 0.0) {
B_data[k + c] /= b_A_data[k + iy];
for (jA = 0; jA + 1 <= k; jA++) {
B_data[jA + c] -= B_data[k + c] * b_A_data[jA + iy];
}
}
}
}
}
}
/*
*
*/
static real32_T eml_matlab_zlarfg(int32_T n, real32_T *alpha1, real32_T x_data
[81], int32_T x_sizes[2], int32_T ix0)
{
real32_T tau;
int32_T nm1;
real32_T xnorm;
real32_T a;
int32_T knt;
int32_T loop_ub;
int32_T k;
tau = 0.0F;
if (n <= 0) {
} else {
nm1 = n - 2;
xnorm = eml_xnrm2(nm1 + 1, x_data, x_sizes, ix0);
if ((real_T)xnorm != 0.0) {
a = (real32_T)fabs(*alpha1);
xnorm = (real32_T)fabs(xnorm);
if (a < xnorm) {
a /= xnorm;
xnorm *= (real32_T)sqrt(a * a + 1.0F);
} else if (a > xnorm) {
xnorm /= a;
xnorm = (real32_T)sqrt(xnorm * xnorm + 1.0F) * a;
} else if (rtIsNaNF(xnorm)) {
} else {
xnorm = a * 1.41421354F;
}
if ((real_T)*alpha1 >= 0.0) {
xnorm = -xnorm;
}
if ((real32_T)fabs(xnorm) < 9.86076132E-32F) {
knt = 0;
do {
knt++;
loop_ub = ix0 + nm1;
for (k = ix0; k <= loop_ub; k++) {
x_data[k - 1] *= 1.01412048E+31F;
}
xnorm *= 1.01412048E+31F;
*alpha1 *= 1.01412048E+31F;
} while (!((real32_T)fabs(xnorm) >= 9.86076132E-32F));
xnorm = eml_xnrm2(nm1 + 1, x_data, x_sizes, ix0);
a = (real32_T)fabs(*alpha1);
xnorm = (real32_T)fabs(xnorm);
if (a < xnorm) {
a /= xnorm;
xnorm *= (real32_T)sqrt(a * a + 1.0F);
} else if (a > xnorm) {
xnorm /= a;
xnorm = (real32_T)sqrt(xnorm * xnorm + 1.0F) * a;
} else if (rtIsNaNF(xnorm)) {
} else {
xnorm = a * 1.41421354F;
}
if ((real_T)*alpha1 >= 0.0) {
xnorm = -xnorm;
}
tau = (xnorm - *alpha1) / xnorm;
*alpha1 = 1.0F / (*alpha1 - xnorm);
loop_ub = ix0 + nm1;
for (k = ix0; k <= loop_ub; k++) {
x_data[k - 1] *= *alpha1;
}
for (k = 1; k <= knt; k++) {
xnorm *= 9.86076132E-32F;
}
*alpha1 = xnorm;
} else {
tau = (xnorm - *alpha1) / xnorm;
*alpha1 = 1.0F / (*alpha1 - xnorm);
loop_ub = ix0 + nm1;
for (k = ix0; k <= loop_ub; k++) {
x_data[k - 1] *= *alpha1;
}
*alpha1 = xnorm;
}
}
}
return tau;
}
/*
*
*/
static void eml_qrsolve(const real32_T A_data[81], const int32_T A_sizes[2],
real32_T B_data[81], int32_T B_sizes[2], real32_T Y_data[81], int32_T Y_sizes
[2])
{
real_T rankR;
real_T u1;
int32_T mn;
int32_T b_A_sizes[2];
int32_T loop_ub;
int32_T k;
real32_T b_A_data[81];
int32_T pvt;
int32_T b_mn;
int32_T tmp_data[9];
int32_T jpvt_data[9];
int8_T unnamed_idx_0;
real32_T work_data[9];
int32_T nmi;
real32_T vn1_data[9];
real32_T vn2_data[9];
int32_T i;
int32_T i_i;
int32_T mmi;
int32_T ix;
real32_T smax;
real32_T s;
int32_T iy;
real32_T atmp;
real32_T tau_data[9];
int32_T i_ip1;
int32_T lastv;
int32_T lastc;
boolean_T exitg2;
int32_T exitg1;
boolean_T firstNonZero;
real32_T t;
rankR = (real_T)A_sizes[0];
u1 = (real_T)A_sizes[1];
rankR = rankR <= u1 ? rankR : u1;
mn = (int32_T)rankR;
b_A_sizes[0] = A_sizes[0];
b_A_sizes[1] = A_sizes[1];
loop_ub = A_sizes[0] * A_sizes[1] - 1;
for (k = 0; k <= loop_ub; k++) {
b_A_data[k] = A_data[k];
}
k = A_sizes[0];
pvt = A_sizes[1];
b_mn = k <= pvt ? k : pvt;
if (A_sizes[1] < 1) {
} else {
loop_ub = A_sizes[1] - 1;
for (k = 0; k <= loop_ub; k++) {
tmp_data[k] = 1 + k;
}
loop_ub = A_sizes[1] - 1;
for (k = 0; k <= loop_ub; k++) {
jpvt_data[k] = tmp_data[k];
}
}
if ((A_sizes[0] == 0) || (A_sizes[1] == 0)) {
} else {
unnamed_idx_0 = (int8_T)A_sizes[1];
loop_ub = unnamed_idx_0 - 1;
for (k = 0; k <= loop_ub; k++) {
work_data[k] = 0.0F;
}
k = 1;
for (nmi = 0; nmi + 1 <= A_sizes[1]; nmi++) {
vn1_data[nmi] = eml_xnrm2(A_sizes[0], A_data, A_sizes, k);
vn2_data[nmi] = vn1_data[nmi];
k += A_sizes[0];
}
for (i = 0; i + 1 <= b_mn; i++) {
i_i = i + i * A_sizes[0];
nmi = A_sizes[1] - i;
mmi = (A_sizes[0] - i) - 1;
if (nmi < 1) {
pvt = -1;
} else {
pvt = 0;
if (nmi > 1) {
ix = i;
smax = (real32_T)fabs(vn1_data[i]);
for (k = 1; k + 1 <= nmi; k++) {
ix++;
s = (real32_T)fabs(vn1_data[ix]);
if (s > smax) {
pvt = k;
smax = s;
}
}
}
}
pvt += i;
if (pvt + 1 != i + 1) {
ix = A_sizes[0] * pvt;
iy = A_sizes[0] * i;
for (k = 1; k <= A_sizes[0]; k++) {
s = b_A_data[ix];
b_A_data[ix] = b_A_data[iy];
b_A_data[iy] = s;
ix++;
iy++;
}
k = jpvt_data[pvt];
jpvt_data[pvt] = jpvt_data[i];
jpvt_data[i] = k;
vn1_data[pvt] = vn1_data[i];
vn2_data[pvt] = vn2_data[i];
}
if (i + 1 < A_sizes[0]) {
atmp = b_A_data[i_i];
smax = eml_matlab_zlarfg(mmi + 1, &atmp, b_A_data, b_A_sizes, i_i + 2);
tau_data[i] = smax;
} else {
atmp = b_A_data[i_i];
smax = b_A_data[i_i];
s = b_eml_matlab_zlarfg(&atmp, &smax);
b_A_data[i_i] = smax;
tau_data[i] = s;
}
b_A_data[i_i] = atmp;
if (i + 1 < A_sizes[1]) {
atmp = b_A_data[i_i];
b_A_data[i_i] = 1.0F;
i_ip1 = (i + (i + 1) * A_sizes[0]) + 1;
if ((real_T)tau_data[i] != 0.0) {
lastv = mmi;
pvt = i_i + mmi;
while ((lastv + 1 > 0) && ((real_T)b_A_data[pvt] == 0.0)) {
lastv--;
pvt--;
}
lastc = nmi - 1;
exitg2 = 0U;
while ((exitg2 == 0U) && (lastc > 0)) {
k = i_ip1 + (lastc - 1) * A_sizes[0];
pvt = k + lastv;
do {
exitg1 = 0U;
if (k <= pvt) {
if ((real_T)b_A_data[k - 1] != 0.0) {
exitg1 = 1U;
} else {
k++;
}
} else {
lastc--;
exitg1 = 2U;
}
} while (exitg1 == 0U);
if (exitg1 == 1U) {
exitg2 = 1U;
}
}
} else {
lastv = -1;
lastc = 0;
}
if (lastv + 1 > 0) {
if (lastc == 0) {
} else {
for (iy = 1; iy <= lastc; iy++) {
work_data[iy - 1] = 0.0F;
}
iy = 0;
loop_ub = i_ip1 + A_sizes[0] * (lastc - 1);
pvt = i_ip1;
while ((A_sizes[0] > 0) && (pvt <= loop_ub)) {
ix = i_i;
smax = 0.0F;
k = pvt + lastv;
for (nmi = pvt; nmi <= k; nmi++) {
smax += b_A_data[nmi - 1] * b_A_data[ix];
ix++;
}
work_data[iy] += smax;
iy++;
pvt += A_sizes[0];
}
}
smax = -tau_data[i];
if ((real_T)smax == 0.0) {
} else {
pvt = 0;
for (nmi = 1; nmi <= lastc; nmi++) {
if ((real_T)work_data[pvt] != 0.0) {
s = work_data[pvt] * smax;
ix = i_i;
loop_ub = lastv + i_ip1;
for (k = i_ip1 - 1; k + 1 <= loop_ub; k++) {
b_A_data[k] += b_A_data[ix] * s;
ix++;
}
}
pvt++;
i_ip1 += A_sizes[0];
}
}
}
b_A_data[i_i] = atmp;
}
for (nmi = i + 1; nmi + 1 <= A_sizes[1]; nmi++) {
if ((real_T)vn1_data[nmi] != 0.0) {
smax = (real32_T)fabs(b_A_data[i + b_A_sizes[0] * nmi]) / vn1_data[nmi];
smax = 1.0F - smax * smax;
if ((real_T)smax < 0.0) {
smax = 0.0F;
}
s = vn1_data[nmi] / vn2_data[nmi];
if (smax * (s * s) <= 0.000345266977F) {
if (i + 1 < A_sizes[0]) {
k = (i + A_sizes[0] * nmi) + 1;
smax = 0.0F;
if (mmi < 1) {
} else if (mmi == 1) {
smax = (real32_T)fabs(b_A_data[k]);
} else {
s = 0.0F;
firstNonZero = TRUE;
pvt = k + mmi;
while (k + 1 <= pvt) {
if (b_A_data[k] != 0.0F) {
atmp = (real32_T)fabs(b_A_data[k]);
if (firstNonZero) {
s = atmp;
smax = 1.0F;
firstNonZero = FALSE;
} else if (s < atmp) {
t = s / atmp;
smax = 1.0F + smax * t * t;
s = atmp;
} else {
t = atmp / s;
smax += t * t;
}
}
k++;
}
smax = s * (real32_T)sqrt(smax);
}
vn1_data[nmi] = smax;
vn2_data[nmi] = vn1_data[nmi];
} else {
vn1_data[nmi] = 0.0F;
vn2_data[nmi] = 0.0F;
}
} else {
vn1_data[nmi] *= (real32_T)sqrt(smax);
}
}
}
}
}
for (jy = 0; jy < 9; jy++) {
for (iy = 0; iy < 12; iy++) {
y[iy + 12 * jy] = Y[jy + 9 * iy];
rankR = (real_T)A_sizes[0];
u1 = (real_T)A_sizes[1];
rankR = rankR >= u1 ? rankR : u1;
smax = (real32_T)rankR * (real32_T)fabs(b_A_data[0]) * 1.1920929E-7F;
rankR = 0.0;
k = 0;
while ((k + 1 <= mn) && (!((real32_T)fabs(b_A_data[k + b_A_sizes[0] * k]) <=
smax))) {
rankR++;
k++;
}
unnamed_idx_0 = (int8_T)A_sizes[1];
Y_sizes[0] = (int32_T)unnamed_idx_0;
Y_sizes[1] = 9;
loop_ub = unnamed_idx_0 * 9 - 1;
for (k = 0; k <= loop_ub; k++) {
Y_data[k] = 0.0F;
}
for (nmi = 0; nmi + 1 <= mn; nmi++) {
if ((real_T)tau_data[nmi] != 0.0) {
for (k = 0; k < 9; k++) {
smax = B_data[nmi + B_sizes[0] * k];
for (i = nmi + 1; i + 1 <= A_sizes[0]; i++) {
smax += b_A_data[i + b_A_sizes[0] * nmi] * B_data[i + B_sizes[0] * k];
}
smax *= tau_data[nmi];
if ((real_T)smax != 0.0) {
B_data[nmi + B_sizes[0] * k] -= smax;
for (i = nmi + 1; i + 1 <= A_sizes[0]; i++) {
B_data[i + B_sizes[0] * k] -= b_A_data[i + b_A_sizes[0] * nmi] *
smax;
}
}
}
}
}
for (k = 0; k < 9; k++) {
for (i = 0; (real_T)(i + 1) <= rankR; i++) {
Y_data[(jpvt_data[i] + Y_sizes[0] * k) - 1] = B_data[i + B_sizes[0] * k];
}
for (nmi = (int32_T)rankR - 1; nmi + 1 >= 1; nmi--) {
Y_data[(jpvt_data[nmi] + Y_sizes[0] * k) - 1] /= b_A_data[nmi + b_A_sizes
[0] * nmi];
for (i = 0; i + 1 <= nmi; i++) {
Y_data[(jpvt_data[i] + Y_sizes[0] * k) - 1] -= Y_data[(jpvt_data[nmi] +
Y_sizes[0] * k) - 1] * b_A_data[i + b_A_sizes[0] * nmi];
}
}
}
}
/*
*
*/
static real32_T eml_xnrm2(int32_T n, const real32_T x_data[81], const int32_T
x_sizes[2], int32_T ix0)
{
real32_T y;
real32_T scale;
boolean_T firstNonZero;
int32_T kend;
int32_T k;
real32_T absxk;
real32_T t;
y = 0.0F;
if (n < 1) {
} else if (n == 1) {
y = (real32_T)fabs(x_data[ix0 - 1]);
} else {
scale = 0.0F;
firstNonZero = TRUE;
kend = (ix0 + n) - 1;
for (k = ix0 - 1; k + 1 <= kend; k++) {
if (x_data[k] != 0.0F) {
absxk = (real32_T)fabs(x_data[k]);
if (firstNonZero) {
scale = absxk;
y = 1.0F;
firstNonZero = FALSE;
} else if (scale < absxk) {
t = scale / absxk;
y = 1.0F + y * t * t;
scale = absxk;
} else {
t = absxk / scale;
y += t * t;
}
}
}
y = scale * (real32_T)sqrt(y);
}
return y;
}
/*
*
*/
void mrdivide(const real32_T A_data[81], const int32_T A_sizes[2], const
real32_T B_data[81], const int32_T B_sizes[2], real32_T y_data[81],
int32_T y_sizes[2])
{
int32_T b_A_sizes[2];
int32_T loop_ub;
int32_T i3;
int32_T b_loop_ub;
int32_T i4;
real32_T b_A_data[81];
int32_T b_B_sizes[2];
real32_T b_B_data[81];
int8_T unnamed_idx_0;
int32_T c_B_sizes[2];
real32_T c_B_data[81];
b_A_sizes[0] = B_sizes[1];
b_A_sizes[1] = B_sizes[0];
loop_ub = B_sizes[0] - 1;
for (i3 = 0; i3 <= loop_ub; i3++) {
b_loop_ub = B_sizes[1] - 1;
for (i4 = 0; i4 <= b_loop_ub; i4++) {
b_A_data[i4 + b_A_sizes[0] * i3] = B_data[i3 + B_sizes[0] * i4];
}
}
b_B_sizes[0] = A_sizes[1];
b_B_sizes[1] = 9;
for (i3 = 0; i3 < 9; i3++) {
loop_ub = A_sizes[1] - 1;
for (i4 = 0; i4 <= loop_ub; i4++) {
b_B_data[i4 + b_B_sizes[0] * i3] = A_data[i3 + A_sizes[0] * i4];
}
}
if ((b_A_sizes[0] == 0) || (b_A_sizes[1] == 0) || (b_B_sizes[0] == 0)) {
unnamed_idx_0 = (int8_T)b_A_sizes[1];
b_B_sizes[0] = (int32_T)unnamed_idx_0;
b_B_sizes[1] = 9;
loop_ub = unnamed_idx_0 * 9 - 1;
for (i3 = 0; i3 <= loop_ub; i3++) {
b_B_data[i3] = 0.0F;
}
} else if (b_A_sizes[0] == b_A_sizes[1]) {
eml_lusolve(b_A_data, b_A_sizes, b_B_data, b_B_sizes);
} else {
c_B_sizes[0] = b_B_sizes[0];
c_B_sizes[1] = 9;
loop_ub = b_B_sizes[0] * 9 - 1;
for (i3 = 0; i3 <= loop_ub; i3++) {
c_B_data[i3] = b_B_data[i3];
}
eml_qrsolve(b_A_data, b_A_sizes, c_B_data, c_B_sizes, b_B_data, b_B_sizes);
}
y_sizes[0] = 9;
y_sizes[1] = b_B_sizes[0];
loop_ub = b_B_sizes[0] - 1;
for (i3 = 0; i3 <= loop_ub; i3++) {
for (i4 = 0; i4 < 9; i4++) {
y_data[i4 + 9 * i3] = b_B_data[i3 + b_B_sizes[0] * i4];
}
}
}
apps/attitude_estimator_ekf/codegen/mrdivide.h
+4 -2
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'mrdivide'
*
* C source code generated on: Wed Jul 11 08:38:35 2012
* C source code generated on: Fri Sep 21 13:56:44 2012
*
*/
@@ -14,6 +14,8 @@
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "rt_defines.h"
#include "rt_nonfinite.h"
#include "rtwtypes.h"
#include "attitudeKalmanfilter_types.h"
@@ -27,6 +29,6 @@
/* Variable Definitions */
/* Function Declarations */
extern void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108]);
extern void mrdivide(const real32_T A_data[81], const int32_T A_sizes[2], const real32_T B_data[81], const int32_T B_sizes[2], real32_T y_data[81], int32_T y_sizes[2]);
#endif
/* End of code generation (mrdivide.h) */
apps/attitude_estimator_ekf/codegen/norm.c
+3 -3
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'norm'
*
* C source code generated on: Wed Jul 11 08:38:35 2012
* C source code generated on: Fri Sep 21 13:56:43 2012
*
*/
@@ -40,7 +40,7 @@ real32_T norm(const real32_T x[3])
firstNonZero = TRUE;
for (k = 0; k < 3; k++) {
if (x[k] != 0.0F) {
absxk = fabsf(x[k]);
absxk = (real32_T)fabsf(x[k]);
if (firstNonZero) {
scale = absxk;
y = 1.0F;
@@ -56,7 +56,7 @@ real32_T norm(const real32_T x[3])
}
}
return scale * sqrtf(y);
return scale * (real32_T)sqrtf(y);
}
/* End of code generation (norm.c) */
apps/attitude_estimator_ekf/codegen/norm.h
+3 -1
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'norm'
*
* C source code generated on: Wed Jul 11 08:38:35 2012
* C source code generated on: Fri Sep 21 13:56:44 2012
*
*/
@@ -14,6 +14,8 @@
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "rt_defines.h"
#include "rt_nonfinite.h"
#include "rtwtypes.h"
#include "attitudeKalmanfilter_types.h"
apps/attitude_estimator_ekf/codegen/rtGetInf.c
+1 -1
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Wed Jul 11 08:38:35 2012
* C source code generated on: Fri Sep 21 13:56:45 2012
*
*/
apps/attitude_estimator_ekf/codegen/rtGetInf.h
+1 -1
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Wed Jul 11 08:38:35 2012
* C source code generated on: Fri Sep 21 13:56:45 2012
*
*/
apps/attitude_estimator_ekf/codegen/rtGetNaN.c
+1 -1
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Wed Jul 11 08:38:35 2012
* C source code generated on: Fri Sep 21 13:56:45 2012
*
*/
apps/attitude_estimator_ekf/codegen/rtGetNaN.h
+1 -1
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Wed Jul 11 08:38:35 2012
* C source code generated on: Fri Sep 21 13:56:44 2012
*
*/
apps/attitude_estimator_ekf/codegen/rt_defines.h
Executable
+24
View File
@@ -0,0 +1,24 @@
/*
* rt_defines.h
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Fri Sep 21 13:56:44 2012
*
*/
#ifndef __RT_DEFINES_H__
#define __RT_DEFINES_H__
#include <stdlib.h>
#define RT_PI 3.14159265358979323846
#define RT_PIF 3.1415927F
#define RT_LN_10 2.30258509299404568402
#define RT_LN_10F 2.3025851F
#define RT_LOG10E 0.43429448190325182765
#define RT_LOG10EF 0.43429449F
#define RT_E 2.7182818284590452354
#define RT_EF 2.7182817F
#endif
/* End of code generation (rt_defines.h) */
apps/attitude_estimator_ekf/codegen/rt_nonfinite.c
+1 -1
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Wed Jul 11 08:38:35 2012
* C source code generated on: Fri Sep 21 13:56:44 2012
*
*/
apps/attitude_estimator_ekf/codegen/rt_nonfinite.h
+1 -1
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Wed Jul 11 08:38:35 2012
* C source code generated on: Fri Sep 21 13:56:44 2012
*
*/
apps/attitude_estimator_ekf/codegen/rtw_proj.tmw
-1
View File
@@ -1 +0,0 @@
Code generation project for attitudeKalmanfilter using C:\Program Files\MATLAB\R2011a\toolbox\coder\coder\rtw\c\xrt\xrt_vcx64.tmf. MATLAB root = C:\Program Files\MATLAB\R2011a.
apps/attitude_estimator_ekf/codegen/rtwtypes.h
+1 -1
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Wed Jul 11 08:38:35 2012
* C source code generated on: Fri Sep 21 13:56:44 2012
*
*/
apps/commander/commander.c
+261 -246
View File
@@ -65,7 +65,8 @@
#include <poll.h>
#include <uORB/uORB.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/rc_channels.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/offboard_control_setpoint.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/subsystem_info.h>
@@ -83,7 +84,8 @@
#include <drivers/drv_baro.h>
PARAM_DEFINE_INT32(SYS_FAILSAVE_LL, 0); /**< Go into low-level failsafe after 0 ms */
//PARAM_DEFINE_INT32(SYS_FAILSAVE_HL, 0); /**< Go into high-level failsafe after 0 ms */
#include <arch/board/up_cpuload.h>
extern struct system_load_s system_load;
@@ -94,14 +96,15 @@ extern struct system_load_s system_load;
#define LOW_VOLTAGE_BATTERY_COUNTER_LIMIT (LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
#define CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT (CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
#define STICK_ON_OFF_LIMIT 7500
#define STICK_THRUST_RANGE 20000
#define STICK_ON_OFF_LIMIT 0.75f
#define STICK_THRUST_RANGE 1.0f
#define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000
#define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
#define GPS_FIX_TYPE_2D 2
#define GPS_FIX_TYPE_3D 3
#define GPS_QUALITY_GOOD_COUNTER_LIMIT 50
#define GPS_QUALITY_GOOD_HYSTERIS_TIME_MS 5000
#define GPS_QUALITY_GOOD_COUNTER_LIMIT (GPS_QUALITY_GOOD_HYSTERIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
/* File descriptors */
static int leds;
@@ -114,6 +117,8 @@ static orb_advert_t stat_pub;
static uint16_t nofix_counter = 0;
static uint16_t gotfix_counter = 0;
static unsigned int failsafe_lowlevel_timeout_ms;
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 */
@@ -251,22 +256,6 @@ int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, u
return 0;
}
static void cal_bsort(float a[], int n)
{
int i,j,t;
for(i=0;i<n-1;i++)
{
for(j=0;j<n-i-1;j++)
{
if(a[j]>a[j+1]) {
t=a[j];
a[j]=a[j+1];
a[j+1]=t;
}
}
}
}
static const char *parameter_file = "/eeprom/parameters";
static int pm_save_eeprom(bool only_unsaved)
@@ -307,30 +296,8 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
const uint64_t calibration_interval_us = 45 * 1000000;
unsigned int calibration_counter = 0;
const int peak_samples = 2000;
/* Get rid of 10% */
const int outlier_margin = (peak_samples) / 10;
float *mag_maxima[3];
mag_maxima[0] = (float*)malloc(peak_samples * sizeof(float));
mag_maxima[1] = (float*)malloc(peak_samples * sizeof(float));
mag_maxima[2] = (float*)malloc(peak_samples * sizeof(float));
float *mag_minima[3];
mag_minima[0] = (float*)malloc(peak_samples * sizeof(float));
mag_minima[1] = (float*)malloc(peak_samples * sizeof(float));
mag_minima[2] = (float*)malloc(peak_samples * sizeof(float));
/* initialize data table */
for (int i = 0; i < peak_samples; i++) {
mag_maxima[0][i] = FLT_MIN;
mag_maxima[1][i] = FLT_MIN;
mag_maxima[2][i] = FLT_MIN;
mag_minima[0][i] = FLT_MAX;
mag_minima[1][i] = FLT_MAX;
mag_minima[2][i] = FLT_MAX;
}
float mag_max[3] = {-FLT_MAX, -FLT_MAX, -FLT_MAX};
float mag_min[3] = {FLT_MAX, FLT_MAX, FLT_MAX};
int fd = open(MAG_DEVICE_PATH, 0);
struct mag_scale mscale_null = {
@@ -357,34 +324,32 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw);
/* get min/max values */
/* iterate through full list */
for (int i = 0; i < peak_samples; i++) {
/* x minimum */
if (raw.magnetometer_raw[0] < mag_minima[0][i])
mag_minima[0][i] = raw.magnetometer_ga[0];
/* y minimum */
if (raw.magnetometer_raw[1] < mag_minima[1][i])
mag_minima[1][i] = raw.magnetometer_ga[1];
/* z minimum */
if (raw.magnetometer_raw[2] < mag_minima[2][i])
mag_minima[2][i] = raw.magnetometer_ga[2];
if (raw.magnetometer_ga[0] < mag_min[0]) {
mag_min[0] = raw.magnetometer_ga[0];
}
else if (raw.magnetometer_ga[0] > mag_max[0]) {
mag_max[0] = raw.magnetometer_ga[0];
}
/* x maximum */
if (raw.magnetometer_raw[0] > mag_maxima[0][i])
mag_maxima[0][i] = raw.magnetometer_ga[0];
/* y maximum */
if (raw.magnetometer_raw[1] > mag_maxima[1][i])
mag_maxima[1][i] = raw.magnetometer_ga[1];
/* z maximum */
if (raw.magnetometer_raw[2] > mag_maxima[2][i])
mag_maxima[2][i] = raw.magnetometer_ga[2];
if (raw.magnetometer_ga[1] < mag_min[1]) {
mag_min[1] = raw.magnetometer_ga[1];
}
else if (raw.magnetometer_ga[1] > mag_max[1]) {
mag_max[1] = raw.magnetometer_ga[1];
}
if (raw.magnetometer_ga[2] < mag_min[2]) {
mag_min[2] = raw.magnetometer_ga[2];
}
else if (raw.magnetometer_ga[2] > mag_max[2]) {
mag_max[2] = raw.magnetometer_ga[2];
}
calibration_counter++;
} else {
/* any poll failure for 1s is a reason to abort */
//mavlink_log_info(mavlink_fd, "[commander] mag calibration aborted, please retry.");
//break;
mavlink_log_info(mavlink_fd, "[commander] mag calibration aborted, please retry.");
break;
}
}
@@ -392,67 +357,6 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
status->flag_preflight_mag_calibration = false;
state_machine_publish(status_pub, status, mavlink_fd);
/* sort values */
cal_bsort(mag_minima[0], peak_samples);
cal_bsort(mag_minima[1], peak_samples);
cal_bsort(mag_minima[2], peak_samples);
cal_bsort(mag_maxima[0], peak_samples);
cal_bsort(mag_maxima[1], peak_samples);
cal_bsort(mag_maxima[2], peak_samples);
float min_avg[3] = { 0.0f, 0.0f, 0.0f };
float max_avg[3] = { 0.0f, 0.0f, 0.0f };
// printf("start:\n");
// for (int i = 0; i < 10; i++) {
// printf("mag min: %8.4f\t%8.4f\t%8.4f\tmax: %8.4f\t%8.4f\t%8.4f\n",
// mag_minima[0][i],
// mag_minima[1][i],
// mag_minima[2][i],
// mag_maxima[0][i],
// mag_maxima[1][i],
// mag_maxima[2][i]);
// usleep(10000);
// }
// printf("-----\n");
// for (int i = (peak_samples - outlier_margin)-10; i < (peak_samples - outlier_margin); i++) {
// printf("mag min: %8.4f\t%8.4f\t%8.4f\tmax: %8.4f\t%8.4f\t%8.4f\n",
// mag_minima[0][i],
// mag_minima[1][i],
// mag_minima[2][i],
// mag_maxima[0][i],
// mag_maxima[1][i],
// mag_maxima[2][i]);
// usleep(10000);
// }
// printf("end\n");
/* take average of center value group */
for (int i = 0; i < (peak_samples - outlier_margin); i++) {
min_avg[0] += mag_minima[0][i+outlier_margin];
min_avg[1] += mag_minima[1][i+outlier_margin];
min_avg[2] += mag_minima[2][i+outlier_margin];
max_avg[0] += mag_maxima[0][i];
max_avg[1] += mag_maxima[1][i];
max_avg[2] += mag_maxima[2][i];
}
min_avg[0] /= (peak_samples - outlier_margin);
min_avg[1] /= (peak_samples - outlier_margin);
min_avg[2] /= (peak_samples - outlier_margin);
max_avg[0] /= (peak_samples - outlier_margin);
max_avg[1] /= (peak_samples - outlier_margin);
max_avg[2] /= (peak_samples - outlier_margin);
// printf("\nFINAL:\nmag min: %8.4f\t%8.4f\t%8.4f\nmax: %8.4f\t%8.4f\t%8.4f\n", (double)min_avg[0],
// (double)min_avg[1], (double)min_avg[2], (double)max_avg[0], (double)max_avg[1], (double)max_avg[2]);
float mag_offset[3];
/**
@@ -467,31 +371,24 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
* offset = (max + min) / 2.0f
*/
mag_offset[0] = (max_avg[0] + min_avg[0]) / 2.0f;
mag_offset[1] = (max_avg[1] + min_avg[1]) / 2.0f;
mag_offset[2] = (max_avg[2] + min_avg[2]) / 2.0f;
mag_offset[0] = (mag_max[0] + mag_min[0]) / 2.0f;
mag_offset[1] = (mag_max[1] + mag_min[1]) / 2.0f;
mag_offset[2] = (mag_max[2] + mag_min[2]) / 2.0f;
if (!isfinite(mag_offset[1]) || !isfinite(mag_offset[1]) || !isfinite(mag_offset[2])) {
printf("mag off x: %4.4f, y: %4.4f, z: %4.4f\n",(double)mag_offset[0],(double)mag_offset[0],(double)mag_offset[2]);
mavlink_log_critical(mavlink_fd, "[commander] MAG calibration failed (INF/NAN)");
} else {
/* announce and set new offset */
/* announce and set new offset */
// char offset_output[50];
// sprintf(offset_output, "[commander] mag cal: %8.4f %8.4f %8.4f", (double)mag_offset[0], (double)mag_offset[1], (double)mag_offset[2]);
// mavlink_log_info(mavlink_fd, offset_output);
if (param_set(param_find("SENS_MAG_XOFF"), &(mag_offset[0]))) {
fprintf(stderr, "[commander] Setting X mag offset failed!\n");
}
if (param_set(param_find("SENS_MAG_XOFF"), &(mag_offset[0]))) {
fprintf(stderr, "[commander] Setting X mag offset failed!\n");
}
if (param_set(param_find("SENS_MAG_YOFF"), &(mag_offset[1]))) {
fprintf(stderr, "[commander] Setting Y mag offset failed!\n");
}
if (param_set(param_find("SENS_MAG_YOFF"), &(mag_offset[1]))) {
fprintf(stderr, "[commander] Setting Y mag offset failed!\n");
}
if (param_set(param_find("SENS_MAG_ZOFF"), &(mag_offset[2]))) {
fprintf(stderr, "[commander] Setting Z mag offset failed!\n");
}
if (param_set(param_find("SENS_MAG_ZOFF"), &(mag_offset[2]))) {
fprintf(stderr, "[commander] Setting Z mag offset failed!\n");
}
fd = open(MAG_DEVICE_PATH, 0);
@@ -507,14 +404,6 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
warn("WARNING: failed to set scale / offsets for mag");
close(fd);
free(mag_maxima[0]);
free(mag_maxima[1]);
free(mag_maxima[2]);
free(mag_minima[0]);
free(mag_minima[1]);
free(mag_minima[2]);
/* auto-save to EEPROM */
int save_ret = pm_save_eeprom(false);
if(save_ret != 0) {
@@ -567,7 +456,7 @@ void do_gyro_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] gyro calibration aborted, please retry.");
mavlink_log_info(mavlink_fd, "[commander] gyro calibration aborted, retry");
return;
}
}
@@ -614,9 +503,7 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status)
mavlink_log_info(mavlink_fd, "[commander] gyro calibration finished");
// char offset_output[50];
// sprintf(offset_output, "[commander] gyro cal: x:%8.4f y:%8.4f z:%8.4f", (double)gyro_offset[0], (double)gyro_offset[1], (double)gyro_offset[2]);
// mavlink_log_info(mavlink_fd, offset_output);
printf("[commander] gyro cal: x:%8.4f y:%8.4f z:%8.4f\n", (double)gyro_offset[0], (double)gyro_offset[1], (double)gyro_offset[2]);
close(sub_sensor_combined);
}
@@ -624,9 +511,8 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status)
void do_accel_calibration(int status_pub, struct vehicle_status_s *status)
{
/* announce change */
usleep(5000);
mavlink_log_info(mavlink_fd, "[commander] The system should be level and not moved");
mavlink_log_info(mavlink_fd, "[commander] keep it level and still");
/* set to accel calibration mode */
status->flag_preflight_accel_calibration = true;
state_machine_publish(status_pub, status, mavlink_fd);
@@ -651,7 +537,6 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status)
if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale_null))
warn("WARNING: failed to set scale / offsets for accel");
close(fd);
while (calibration_counter < calibration_count) {
/* wait blocking for new data */
@@ -665,11 +550,10 @@ void do_accel_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] gyro calibration aborted, please retry.");
mavlink_log_info(mavlink_fd, "[commander] acceleration calibration aborted");
return;
}
}
accel_offset[0] = accel_offset[0] / calibration_count;
accel_offset[1] = accel_offset[1] / calibration_count;
accel_offset[2] = accel_offset[2] / calibration_count;
@@ -724,18 +608,11 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status)
if(save_ret != 0) {
warn("WARNING: auto-save of params to EEPROM failed");
}
/* exit to gyro calibration mode */
status->flag_preflight_accel_calibration = false;
state_machine_publish(status_pub, status, mavlink_fd);
mavlink_log_info(mavlink_fd, "[commander] acceleration calibration finished");
// char offset_output[50];
// sprintf(offset_output, "[commander] accel cal: x:%8.4f y:%8.4f z:%8.4f", (double)accel_offset[0],
// (double)accel_offset[1], (double)accel_offset[2]);
// mavlink_log_info(mavlink_fd, offset_output);
printf("[commander] accel calibration: x:%8.4f y:%8.4f z:%8.4f\n", (double)accel_offset[0],(double)accel_offset[1], (double)accel_offset[2]);
close(sub_sensor_combined);
}
@@ -794,6 +671,31 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
}
break;
case PX4_CMD_CONTROLLER_SELECTION: {
bool changed = false;
if ((int)cmd->param1 != (int)current_vehicle_status->flag_control_rates_enabled) {
current_vehicle_status->flag_control_rates_enabled = cmd->param1;
changed = true;
}
if ((int)cmd->param2 != (int)current_vehicle_status->flag_control_attitude_enabled) {
current_vehicle_status->flag_control_attitude_enabled = cmd->param2;
changed = true;
}
if ((int)cmd->param3 != (int)current_vehicle_status->flag_control_velocity_enabled) {
current_vehicle_status->flag_control_velocity_enabled = cmd->param3;
changed = true;
}
if ((int)cmd->param4 != (int)current_vehicle_status->flag_control_position_enabled) {
current_vehicle_status->flag_control_position_enabled = cmd->param4;
changed = true;
}
if (changed) {
/* publish current state machine */
state_machine_publish(status_pub, current_vehicle_status, mavlink_fd);
}
}
// /* request to land */
// case MAV_CMD_NAV_LAND:
// {
@@ -878,7 +780,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
/* none found */
if (!handled) {
//fprintf(stderr, "[commander] refusing unsupported calibration request\n");
mavlink_log_critical(mavlink_fd, "[commander] CMD refusing unsupported calibration request");
mavlink_log_critical(mavlink_fd, "[commander] CMD refusing unsup. calib. request");
result = MAV_RESULT_UNSUPPORTED;
}
}
@@ -1075,6 +977,10 @@ int commander_thread_main(int argc, char *argv[])
/* not yet initialized */
commander_initialized = false;
/* set parameters */
failsafe_lowlevel_timeout_ms = 0;
param_get(param_find("SYS_FAILSAVE_LL"), &failsafe_lowlevel_timeout_ms);
/* welcome user */
printf("[commander] I am in command now!\n");
@@ -1101,6 +1007,8 @@ int commander_thread_main(int argc, char *argv[])
memset(&current_status, 0, sizeof(current_status));
current_status.state_machine = SYSTEM_STATE_PREFLIGHT;
current_status.flag_system_armed = false;
current_status.offboard_control_signal_found_once = false;
current_status.rc_signal_found_once = false;
/* advertise to ORB */
stat_pub = orb_advertise(ORB_ID(vehicle_status), &current_status);
@@ -1146,10 +1054,15 @@ int commander_thread_main(int argc, char *argv[])
int gps_quality_good_counter = 0;
/* Subscribe to RC data */
int rc_sub = orb_subscribe(ORB_ID(rc_channels));
struct rc_channels_s rc;
memset(&rc, 0, sizeof(rc));
/* Subscribe to manual control data */
int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
struct manual_control_setpoint_s sp_man;
memset(&sp_man, 0, sizeof(sp_man));
/* Subscribe to offboard control data */
int sp_offboard_sub = orb_subscribe(ORB_ID(offboard_control_setpoint));
struct offboard_control_setpoint_s sp_offboard;
memset(&sp_offboard, 0, sizeof(sp_offboard));
int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
struct vehicle_gps_position_s gps;
@@ -1168,11 +1081,23 @@ int commander_thread_main(int argc, char *argv[])
commander_initialized = true;
uint64_t start_time = hrt_absolute_time();
uint64_t failsave_ll_start_time = 0;
bool state_changed = true;
while (!thread_should_exit) {
/* Get current values */
orb_copy(ORB_ID(rc_channels), rc_sub, &rc);
bool new_data;
orb_check(sp_man_sub, &new_data);
if (new_data) {
orb_copy(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man);
}
orb_check(sp_offboard_sub, &new_data);
if (new_data) {
orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard);
}
orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps);
orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors);
@@ -1283,10 +1208,10 @@ int commander_thread_main(int argc, char *argv[])
/* End battery voltage check */
/* Check if last transition deserved an audio event */
#warning This code depends on state that is no longer? maintained
#if 0
trigger_audio_alarm(vehicle_mode_previous, vehicle_state_previous, current_status.mode, current_status.state_machine);
#endif
// #warning This code depends on state that is no longer? maintained
// #if 0
// trigger_audio_alarm(vehicle_mode_previous, vehicle_state_previous, current_status.mode, current_status.state_machine);
// #endif
/* only check gps fix if we are outdoor */
// if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) {
@@ -1341,89 +1266,176 @@ int commander_thread_main(int argc, char *argv[])
/* end: check gps */
/* Start RC state check */
bool prev_lost = current_status.rc_signal_lost;
/* ignore RC signals if in offboard control mode */
if (!current_status.offboard_control_signal_found_once && sp_man.timestamp != 0) {
/* Start RC state check */
if (rc.chan_count > 4 && (hrt_absolute_time() - rc.timestamp) < 100000) {
if ((hrt_absolute_time() - sp_man.timestamp) < 100000) {
/* quadrotor specific logic - check against system type in the future */
/* check if left stick is in lower left position --> switch to standby state */
if ((sp_man.yaw < -STICK_ON_OFF_LIMIT) && sp_man.throttle < STICK_THRUST_RANGE*0.2f) { //TODO: remove hardcoded values
if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
update_state_machine_disarm(stat_pub, &current_status, mavlink_fd);
stick_on_counter = 0;
int16_t rc_yaw_scale = rc.chan[rc.function[YAW]].scale;
int16_t rc_throttle_scale = rc.chan[rc.function[THROTTLE]].scale;
int16_t mode_switch_rc_value = rc.chan[rc.function[OVERRIDE]].scale;
/* Check the value of the rc channel of the mode switch */
mode_switch_rc_value = rc.chan[rc.function[OVERRIDE]].scale;
} else {
stick_off_counter++;
stick_on_counter = 0;
}
}
/* check if left stick is in lower left position --> switch to standby state */
if (rc_yaw_scale < -STICK_ON_OFF_LIMIT && rc_throttle_scale < STICK_THRUST_RANGE*0.2f) { //TODO: remove hardcoded values
if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
update_state_machine_disarm(stat_pub, &current_status, mavlink_fd);
stick_on_counter = 0;
/* check if left stick is in lower right position --> arm */
if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE*0.2f) { //TODO: remove hardcoded values
if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
update_state_machine_arm(stat_pub, &current_status, mavlink_fd);
stick_on_counter = 0;
} else {
stick_on_counter++;
stick_off_counter = 0;
}
}
//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 {
stick_off_counter++;
stick_on_counter = 0;
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);
}
}
/* check if left stick is in lower right position --> arm */
if (rc_yaw_scale > STICK_ON_OFF_LIMIT && rc_throttle_scale < STICK_THRUST_RANGE*0.2f) { //TODO: remove hardcoded values
if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
update_state_machine_arm(stat_pub, &current_status, mavlink_fd);
stick_on_counter = 0;
/* handle the case where RC signal was regained */
if (!current_status.rc_signal_found_once) {
current_status.rc_signal_found_once = true;
mavlink_log_critical(mavlink_fd, "[commander] DETECTED RC SIGNAL FIRST TIME.");
} else {
stick_on_counter++;
stick_off_counter = 0;
if (current_status.rc_signal_lost) mavlink_log_critical(mavlink_fd, "[commander] RECOVERY - RC SIGNAL GAINED!");
}
}
//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 (mode_switch_rc_value > STICK_ON_OFF_LIMIT) {
update_state_machine_mode_manual(stat_pub, &current_status, mavlink_fd);
} else if (mode_switch_rc_value < -STICK_ON_OFF_LIMIT) {
update_state_machine_mode_auto(stat_pub, &current_status, mavlink_fd);
current_status.rc_signal_cutting_off = false;
current_status.rc_signal_lost = false;
current_status.rc_signal_lost_interval = 0;
} else {
update_state_machine_mode_stabilized(stat_pub, &current_status, mavlink_fd);
static uint64_t last_print_time = 0;
/* print error message for first RC glitch and then every 5 s / 5000 ms) */
if (!current_status.rc_signal_cutting_off || ((hrt_absolute_time() - last_print_time) > 5000000)) {
/* only complain if the offboard control is NOT active */
current_status.rc_signal_cutting_off = true;
mavlink_log_critical(mavlink_fd, "[commander] CRITICAL - NO REMOTE SIGNAL!");
last_print_time = hrt_absolute_time();
}
/* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */
current_status.rc_signal_lost_interval = hrt_absolute_time() - sp_man.timestamp;
/* if the RC signal is gone for a full second, consider it lost */
if (current_status.rc_signal_lost_interval > 1000000) {
current_status.rc_signal_lost = true;
current_status.failsave_lowlevel = true;
state_changed = true;
}
// if (hrt_absolute_time() - current_status.failsave_ll_start_time > failsafe_lowlevel_timeout_ms*1000) {
// publish_armed_status(&current_status);
// }
}
/* Publish RC signal */
/* handle the case where RC signal was regained */
if (current_status.rc_signal_lost) mavlink_log_critical(mavlink_fd, "[commander] RECOVERY - RC SIGNAL GAINED!");
current_status.rc_signal_lost = false;
current_status.rc_signal_lost_interval = 0;
} else {
static uint64_t last_print_time = 0;
/* print error message for first RC glitch and then every 5 s / 5000 ms) */
if (!current_status.rc_signal_lost || ((hrt_absolute_time() - last_print_time) > 5000000)) {
mavlink_log_critical(mavlink_fd, "[commander] CRITICAL - NO REMOTE SIGNAL!");
last_print_time = hrt_absolute_time();
}
/* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */
current_status.rc_signal_cutting_off = true;
current_status.rc_signal_lost_interval = hrt_absolute_time() - rc.timestamp;
/* if the RC signal is gone for a full second, consider it lost */
if (current_status.rc_signal_lost_interval > 1000000) current_status.rc_signal_lost = true;
}
/* Check if this is the first loss or first gain*/
if ((!prev_lost && current_status.rc_signal_lost) ||
prev_lost && !current_status.rc_signal_lost) {
/* publish rc lost */
publish_armed_status(&current_status);
}
/* End mode switch */
/* END RC state check */
/* State machine update for offboard control */
if (!current_status.rc_signal_found_once && sp_offboard.timestamp != 0) {
if ((hrt_absolute_time() - sp_offboard.timestamp) < 5000000) {
/* decide about attitude control flag, enable in att/pos/vel */
bool attitude_ctrl_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE ||
sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY ||
sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION);
/* decide about rate control flag, enable it always XXX (for now) */
bool rates_ctrl_enabled = true;
/* set up control mode */
if (current_status.flag_control_attitude_enabled != attitude_ctrl_enabled) {
current_status.flag_control_attitude_enabled = attitude_ctrl_enabled;
state_changed = true;
}
if (current_status.flag_control_rates_enabled != rates_ctrl_enabled) {
current_status.flag_control_rates_enabled = rates_ctrl_enabled;
state_changed = true;
}
/* handle the case where offboard control signal was regained */
if (!current_status.offboard_control_signal_found_once) {
current_status.offboard_control_signal_found_once = true;
/* enable offboard control, disable manual input */
current_status.flag_control_manual_enabled = false;
current_status.flag_control_offboard_enabled = true;
state_changed = true;
mavlink_log_critical(mavlink_fd, "[commander] DETECTED OFFBOARD CONTROL SIGNAL FIRST TIME.");
} else {
if (current_status.offboard_control_signal_lost) {
mavlink_log_critical(mavlink_fd, "[commander] RECOVERY - OFFBOARD CONTROL SIGNAL GAINED!");
state_changed = true;
}
}
current_status.offboard_control_signal_weak = false;
current_status.offboard_control_signal_lost = false;
current_status.offboard_control_signal_lost_interval = 0;
/* arm / disarm on request */
if (sp_offboard.armed && !current_status.flag_system_armed) {
update_state_machine_arm(stat_pub, &current_status, mavlink_fd);
/* switch to stabilized mode = takeoff */
update_state_machine_mode_stabilized(stat_pub, &current_status, mavlink_fd);
} else if (!sp_offboard.armed && current_status.flag_system_armed) {
update_state_machine_disarm(stat_pub, &current_status, mavlink_fd);
}
} else {
static uint64_t last_print_time = 0;
/* print error message for first RC glitch and then every 5 s / 5000 ms) */
if (!current_status.offboard_control_signal_weak || ((hrt_absolute_time() - last_print_time) > 5000000)) {
current_status.offboard_control_signal_weak = true;
mavlink_log_critical(mavlink_fd, "[commander] CRITICAL - NO OFFBOARD CONTROL SIGNAL!");
last_print_time = hrt_absolute_time();
}
/* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */
current_status.offboard_control_signal_lost_interval = hrt_absolute_time() - sp_offboard.timestamp;
/* if the signal is gone for 0.1 seconds, consider it lost */
if (current_status.offboard_control_signal_lost_interval > 100000) {
current_status.offboard_control_signal_lost = true;
current_status.failsave_lowlevel_start_time = hrt_absolute_time();
current_status.failsave_lowlevel = true;
/* kill motors after timeout */
if (hrt_absolute_time() - current_status.failsave_lowlevel_start_time > failsafe_lowlevel_timeout_ms*1000) {
state_changed = true;
}
}
}
}
current_status.counter++;
current_status.timestamp = hrt_absolute_time();
@@ -1438,8 +1450,10 @@ int commander_thread_main(int argc, char *argv[])
}
/* publish at least with 1 Hz */
if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 || state_changed) {
publish_armed_status(&current_status);
orb_publish(ORB_ID(vehicle_status), stat_pub, &current_status);
state_changed = false;
}
/* Store old modes to detect and act on state transitions */
@@ -1457,7 +1471,8 @@ int commander_thread_main(int argc, char *argv[])
/* close fds */
led_deinit();
buzzer_deinit();
close(rc_sub);
close(sp_man_sub);
close(sp_offboard_sub);
close(gps_sub);
close(sensor_sub);
apps/commander/state_machine_helper.c
+2 -1
View File
@@ -223,7 +223,8 @@ void publish_armed_status(const struct vehicle_status_s *current_status) {
/* lock down actuators if required */
// XXX FIXME Currently any loss of RC will completely disable all actuators
// needs proper failsafe
armed.lockdown = (current_status->rc_signal_lost || current_status->flag_hil_enabled) ? true : false;
armed.lockdown = ((current_status->rc_signal_lost && current_status->offboard_control_signal_lost)
|| current_status->flag_hil_enabled) ? true : false;
orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
}
apps/fixedwing_control/fixedwing_control.c
+301 -772
View File
File diff suppressed because it is too large Load Diff
apps/mavlink/mavlink.c
+178 -80
View File
@@ -65,7 +65,7 @@
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/ardrone_motors_setpoint.h>
#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_global_position_setpoint.h>
@@ -126,14 +126,13 @@ static struct vehicle_attitude_s hil_attitude;
static struct vehicle_global_position_s hil_global_pos;
static struct ardrone_motors_setpoint_s ardrone_motors;
static struct offboard_control_setpoint_s offboard_control_sp;
static struct vehicle_command_s vcmd;
static struct actuator_armed_s armed;
static orb_advert_t pub_hil_global_pos = -1;
static orb_advert_t ardrone_motors_pub = -1;
static orb_advert_t cmd_pub = -1;
static orb_advert_t flow_pub = -1;
static orb_advert_t global_position_setpoint_pub = -1;
@@ -188,6 +187,12 @@ static struct mavlink_subscriptions {
.initialized = false
};
static struct mavlink_publications {
orb_advert_t offboard_control_sp_pub;
} mavlink_pubs = {
.offboard_control_sp_pub = -1
};
/* 3: Define waypoint helper functions */
void mavlink_wpm_send_message(mavlink_message_t *msg);
@@ -198,6 +203,7 @@ int mavlink_missionlib_send_gcs_string(const char *string);
uint64_t mavlink_missionlib_get_system_timestamp(void);
void handleMessage(mavlink_message_t *msg);
static void mavlink_update_system();
/**
* Enable / disable Hardware in the Loop simulation mode.
@@ -931,29 +937,6 @@ static void *uorb_receiveloop(void *arg)
orb_copy(ORB_ID(vehicle_attitude_setpoint), subs->spa_sub, &buf.att_sp);
mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(MAVLINK_COMM_0, buf.att_sp.timestamp/1000,
buf.att_sp.roll_body, buf.att_sp.pitch_body, buf.att_sp.yaw_body, buf.att_sp.thrust);
/* Only send in HIL mode */
if (mavlink_hil_enabled) {
/* translate the current syste state to mavlink state and mode */
uint8_t mavlink_state = 0;
uint8_t mavlink_mode = 0;
get_mavlink_mode_and_state(&v_status, &armed, &mavlink_state, &mavlink_mode);
/* HIL message as per MAVLink spec */
mavlink_msg_hil_controls_send(chan,
hrt_absolute_time(),
buf.att_sp.roll_body, /* this may be replaced by ctrl0 later */
buf.att_sp.pitch_body,
buf.att_sp.yaw_body,
buf.att_sp.thrust,
0,
0,
0,
0,
mavlink_mode,
0);
}
}
/* --- ACTUATOR OUTPUTS 0 --- */
@@ -970,6 +953,7 @@ static void *uorb_receiveloop(void *arg)
buf.act_outputs.output[5],
buf.act_outputs.output[6],
buf.act_outputs.output[7]);
// if (NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) {
// mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, hrt_absolute_time(),
// 1 /* port 1 */,
@@ -1072,8 +1056,8 @@ static void *uorb_receiveloop(void *arg)
if (fds[ifds++].revents & POLLIN) {
/* copy local position data into local buffer */
orb_copy(ORB_ID(manual_control_setpoint), subs->man_control_sp_sub, &buf.man_control);
mavlink_msg_manual_control_send(MAVLINK_COMM_0, mavlink_system.sysid, buf.man_control.roll,
buf.man_control.pitch, buf.man_control.yaw, buf.man_control.throttle, 0);
mavlink_msg_manual_control_send(MAVLINK_COMM_0, mavlink_system.sysid, buf.man_control.roll*1000,
buf.man_control.pitch*1000, buf.man_control.yaw*1000, buf.man_control.throttle*1000, 0);
}
/* --- ACTUATOR ARMED --- */
@@ -1088,6 +1072,29 @@ static void *uorb_receiveloop(void *arg)
mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "ctrl1 ", buf.actuators.control[1]);
mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "ctrl2 ", buf.actuators.control[2]);
mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "ctrl3 ", buf.actuators.control[3]);
/* Only send in HIL mode */
if (mavlink_hil_enabled) {
/* translate the current syste state to mavlink state and mode */
uint8_t mavlink_state = 0;
uint8_t mavlink_mode = 0;
get_mavlink_mode_and_state(&v_status, &armed, &mavlink_state, &mavlink_mode);
/* HIL message as per MAVLink spec */
mavlink_msg_hil_controls_send(chan,
hrt_absolute_time(),
buf.actuators.control[0],
buf.actuators.control[1],
buf.actuators.control[2],
buf.actuators.control[3],
0,
0,
0,
0,
mavlink_mode,
0);
}
}
/* --- DEBUG KEY/VALUE --- */
@@ -1133,6 +1140,8 @@ mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg)
}
}
#define MAVLINK_OFFBOARD_CONTROL_FLAG_ARMED 0x10
/****************************************************************************
* Public Functions
****************************************************************************/
@@ -1242,30 +1251,105 @@ void handleMessage(mavlink_message_t *msg)
// printf("got MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT target_system=%u, sysid = %u\n", quad_motors_setpoint.target_system, mavlink_system.sysid);
if (mavlink_system.sysid < 4) {
ardrone_motors.p1 = quad_motors_setpoint.roll[mavlink_system.sysid];
ardrone_motors.p2 = quad_motors_setpoint.pitch[mavlink_system.sysid];
ardrone_motors.p3 = quad_motors_setpoint.yaw[mavlink_system.sysid];
ardrone_motors.p4 = quad_motors_setpoint.thrust[mavlink_system.sysid];
/*
* rate control mode - defined by MAVLink
*/
ardrone_motors.timestamp = hrt_absolute_time();
uint8_t ml_mode = 0;
bool ml_armed = false;
/* only send if RC is off */
if (v_status.rc_signal_lost) {
/* check if input has to be enabled */
if (!v_status.flag_control_offboard_enabled) {
/* XXX Enable offboard control */
}
/* XXX decode mode and set flags */
// if (mode == abc) xxx flag_control_rates_enabled;
/* check if topic has to be advertised */
if (ardrone_motors_pub <= 0) {
ardrone_motors_pub = orb_advertise(ORB_ID(ardrone_motors_setpoint), &ardrone_motors);
}
/* Publish */
orb_publish(ORB_ID(ardrone_motors_setpoint), ardrone_motors_pub, &ardrone_motors);
if (quad_motors_setpoint.mode & MAVLINK_OFFBOARD_CONTROL_FLAG_ARMED) {
ml_armed = true;
}
switch (quad_motors_setpoint.mode) {
case 0:
break;
case 1:
ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_RATES;
break;
case 2:
ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE;
break;
case 3:
ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY;
break;
case 4:
ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_POSITION;
break;
}
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.armed = ml_armed;
offboard_control_sp.mode = ml_mode;
offboard_control_sp.timestamp = hrt_absolute_time();
/* check if topic has to be advertised */
if (mavlink_pubs.offboard_control_sp_pub <= 0) {
mavlink_pubs.offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp);
} else {
/* Publish */
orb_publish(ORB_ID(offboard_control_setpoint), mavlink_pubs.offboard_control_sp_pub, &offboard_control_sp);
}
// /* change armed status if required */
// bool cmd_armed = (quad_motors_setpoint.mode & MAVLINK_OFFBOARD_CONTROL_FLAG_ARMED);
// bool cmd_generated = false;
// if (v_status.flag_control_offboard_enabled != cmd_armed) {
// vcmd.param1 = cmd_armed;
// vcmd.param2 = 0;
// vcmd.param3 = 0;
// vcmd.param4 = 0;
// vcmd.param5 = 0;
// vcmd.param6 = 0;
// vcmd.param7 = 0;
// vcmd.command = MAV_CMD_COMPONENT_ARM_DISARM;
// vcmd.target_system = mavlink_system.sysid;
// vcmd.target_component = MAV_COMP_ID_ALL;
// vcmd.source_system = msg->sysid;
// vcmd.source_component = msg->compid;
// vcmd.confirmation = 1;
// cmd_generated = true;
// }
// /* check if input has to be enabled */
// if ((v_status.flag_control_rates_enabled != (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_RATES)) ||
// (v_status.flag_control_attitude_enabled != (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_ATTITUDE)) ||
// (v_status.flag_control_velocity_enabled != (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_VELOCITY)) ||
// (v_status.flag_control_position_enabled != (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_POSITION))) {
// vcmd.param1 = (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_RATES);
// vcmd.param2 = (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_ATTITUDE);
// vcmd.param3 = (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_VELOCITY);
// vcmd.param4 = (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_POSITION);
// vcmd.param5 = 0;
// vcmd.param6 = 0;
// vcmd.param7 = 0;
// vcmd.command = PX4_CMD_CONTROLLER_SELECTION;
// vcmd.target_system = mavlink_system.sysid;
// vcmd.target_component = MAV_COMP_ID_ALL;
// vcmd.source_system = msg->sysid;
// vcmd.source_component = msg->compid;
// vcmd.confirmation = 1;
// cmd_generated = true;
// }
// if (cmd_generated) {
// /* check if topic is advertised */
// if (cmd_pub <= 0) {
// cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
// } else {
// /* create command */
// orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd);
// }
// }
}
}
@@ -1332,10 +1416,10 @@ void handleMessage(mavlink_message_t *msg)
memset(&rc_hil, 0, sizeof(rc_hil));
static orb_advert_t rc_pub = 0;
rc_hil.chan[0].raw = 1510 + man.x * 500;
rc_hil.chan[1].raw = 1520 + man.y * 500;
rc_hil.chan[2].raw = 1590 + man.r * 500;
rc_hil.chan[3].raw = 1420 + man.z * 500;
rc_hil.chan[0].raw = 1510 + man.x / 2;
rc_hil.chan[1].raw = 1520 + man.y / 2;
rc_hil.chan[2].raw = 1590 + man.r / 2;
rc_hil.chan[3].raw = 1420 + man.z / 2;
rc_hil.chan[0].scaled = man.x;
rc_hil.chan[1].scaled = man.y;
@@ -1345,10 +1429,10 @@ void handleMessage(mavlink_message_t *msg)
struct manual_control_setpoint_s mc;
static orb_advert_t mc_pub = 0;
mc.roll = man.x;
mc.pitch = man.y;
mc.yaw = man.r;
mc.roll = man.z;
mc.roll = man.x / 1000.0f;
mc.pitch = man.y / 1000.0f;
mc.yaw = man.r / 1000.0f;
mc.roll = man.z / 1000.0f;
/* fake RC channels with manual control input from simulator */
@@ -1465,6 +1549,39 @@ int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_conf
return uart;
}
void mavlink_update_system()
{
static initialized = false;
param_t param_system_id;
param_t param_component_id;
param_t param_system_type;
if (!initialized) {
param_system_id = param_find("MAV_SYS_ID");
param_component_id = param_find("MAV_COMP_ID");
param_system_type = param_find("MAV_TYPE");
}
/* update system and component id */
int32_t system_id;
param_get(param_system_id, &system_id);
if (system_id > 0 && system_id < 255) {
mavlink_system.sysid = system_id;
}
int32_t component_id;
param_get(param_component_id, &component_id);
if (component_id > 0 && component_id < 255) {
mavlink_system.compid = component_id;
}
int32_t system_type;
param_get(param_system_type, &system_type);
if (system_type >= 0 && system_type < MAV_TYPE_ENUM_END) {
mavlink_system.type = system_type;
}
}
/**
* MAVLink Protocol main function.
*/
@@ -1479,7 +1596,7 @@ int mavlink_thread_main(int argc, char *argv[])
memset(&rc, 0, sizeof(rc));
memset(&hil_attitude, 0, sizeof(hil_attitude));
memset(&hil_global_pos, 0, sizeof(hil_global_pos));
memset(&ardrone_motors, 0, sizeof(ardrone_motors));
memset(&offboard_control_sp, 0, sizeof(offboard_control_sp));
memset(&vcmd, 0, sizeof(vcmd));
/* print welcome text */
@@ -1541,9 +1658,7 @@ int mavlink_thread_main(int argc, char *argv[])
fflush(stdout);
/* Initialize system properties */
param_t param_system_id = param_find("MAV_SYS_ID");
param_t param_component_id = param_find("MAV_COMP_ID");
param_t param_system_type = param_find("MAV_TYPE");
mavlink_update_system();
/* topics to subscribe globally */
/* subscribe to ORB for global position */
@@ -1582,7 +1697,7 @@ int mavlink_thread_main(int argc, char *argv[])
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, 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);
@@ -1647,24 +1762,7 @@ int mavlink_thread_main(int argc, char *argv[])
/* 1 Hz */
if (lowspeed_counter == 10) {
/* update system and component id */
int32_t system_id;
param_get(param_system_id, &system_id);
if (system_id > 0 && system_id < 255) {
mavlink_system.sysid = system_id;
}
int32_t component_id;
param_get(param_component_id, &component_id);
if (component_id > 0 && component_id < 255) {
mavlink_system.compid = component_id;
}
int32_t system_type;
param_get(param_system_type, &system_type);
if (system_type >= 0 && system_type < MAV_TYPE_ENUM_END) {
mavlink_system.type = system_type;
}
mavlink_update_system();
/* translate the current syste state to mavlink state and mode */
uint8_t mavlink_state = 0;
apps/multirotor_att_control/multirotor_att_control_main.c
+129 -45
View File
@@ -1,7 +1,7 @@
/****************************************************************************
*
* Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
* Author: @author Lorenz Meier <lm@inf.ethz.ch>
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: Lorenz Meier <lm@inf.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -36,6 +36,8 @@
* @file multirotor_att_control_main.c
*
* Implementation of multirotor attitude control main loop.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
*/
#include <nuttx/config.h>
@@ -54,36 +56,92 @@
#include <sys/prctl.h>
#include <arch/board/up_hrt.h>
#include <uORB/uORB.h>
#include <drivers/drv_gyro.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/ardrone_motors_setpoint.h>
#include <uORB/topics/offboard_control_setpoint.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/actuator_controls.h>
#include <systemlib/perf_counter.h>
#include "multirotor_attitude_control.h"
#include "multirotor_rate_control.h"
__EXPORT int multirotor_att_control_main(int argc, char *argv[]);
static enum {
CONTROL_MODE_RATES = 0,
CONTROL_MODE_ATTITUDE = 1,
} control_mode;
static bool thread_should_exit;
static int mc_task;
static bool motor_test_mode = false;
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[])
{
/* declare and safely initialize all structs */
struct vehicle_status_s state;
memset(&state, 0, sizeof(state));
struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att));
@@ -93,18 +151,20 @@ mc_thread_main(int argc, char *argv[])
memset(&manual, 0, sizeof(manual));
struct sensor_combined_s raw;
memset(&raw, 0, sizeof(raw));
struct ardrone_motors_setpoint_s setpoint;
memset(&setpoint, 0, sizeof(setpoint));
struct offboard_control_setpoint_s offboard_sp;
memset(&offboard_sp, 0, sizeof(offboard_sp));
struct vehicle_rates_setpoint_s rates_sp;
memset(&rates_sp, 0, sizeof(rates_sp));
struct actuator_controls_s actuators;
/* subscribe to attitude, motor setpoints and system state */
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
int setpoint_sub = orb_subscribe(ORB_ID(offboard_control_setpoint));
int state_sub = orb_subscribe(ORB_ID(vehicle_status));
int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
int sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
int setpoint_sub = orb_subscribe(ORB_ID(ardrone_motors_setpoint));
/*
* Do not rate-limit the loop to prevent aliasing
@@ -121,8 +181,9 @@ mc_thread_main(int argc, char *argv[])
actuators.control[i] = 0.0f;
}
orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators);
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);
/* register the perf counter */
perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "multirotor_att_control");
@@ -130,6 +191,13 @@ 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 */
@@ -138,13 +206,22 @@ mc_thread_main(int argc, char *argv[])
perf_begin(mc_loop_perf);
/* get a local copy of system state */
orb_copy(ORB_ID(vehicle_status), state_sub, &state);
bool updated;
orb_check(state_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_status), state_sub, &state);
}
/* get a local copy of manual setpoint */
orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual);
/* get a local copy of attitude */
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
/* get a local copy of attitude setpoint */
orb_copy(ORB_ID(vehicle_attitude_setpoint), att_setpoint_sub, &att_sp);
/* get a local copy of rates setpoint */
orb_check(setpoint_sub, &updated);
if (updated) {
orb_copy(ORB_ID(offboard_control_setpoint), setpoint_sub, &offboard_sp);
}
/* get a local copy of the current sensor values */
orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw);
@@ -155,41 +232,57 @@ mc_thread_main(int argc, char *argv[])
/* manual inputs, from RC control or joystick */
att_sp.roll_body = manual.roll;
att_sp.pitch_body = manual.pitch;
att_sp.yaw_rate_body = manual.yaw;
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;
att_sp.yaw_body = 0.0f;
att_sp.roll_rate_body = 0.0f;
att_sp.pitch_rate_body = 0.0f;
att_sp.yaw_rate_body = 0.0f;
att_sp.thrust = 0.1f;
} else {
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);
}
} 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 2: Identify the controller setup to run and set up the inputs correctly */
/** STEP 3: Identify the controller setup to run and set up the inputs correctly */
/* run attitude controller */
if (state.flag_control_attitude_enabled) {
multirotor_control_attitude(&att_sp, &att, &actuators);
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);
}
/* XXX could be also run in an independent loop */
if (state.flag_control_rates_enabled) {
multirotor_control_rates(&att_sp, &raw.gyro_rad_s, &actuators);
}
/* STEP 3: publish the result to the vehicle actuators */
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
perf_end(mc_loop_perf);
}
@@ -211,6 +304,8 @@ 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);
}
@@ -229,22 +324,10 @@ usage(const char *reason)
int multirotor_att_control_main(int argc, char *argv[])
{
int ch;
control_mode = CONTROL_MODE_RATES;
unsigned int optioncount = 0;
while ((ch = getopt(argc, argv, "tm:")) != EOF) {
switch (ch) {
case 'm':
if (!strcmp(optarg, "rates")) {
control_mode = CONTROL_MODE_RATES;
} else if (!strcmp(optarg, "attitude")) {
control_mode = CONTROL_MODE_RATES;
} else {
usage("unrecognized -m value");
}
optioncount += 2;
break;
case 't':
motor_test_mode = true;
optioncount += 1;
@@ -255,6 +338,7 @@ int multirotor_att_control_main(int argc, char *argv[])
default:
fprintf(stderr, "option: -%c\n", ch);
usage("unrecognized option");
break;
}
}
argc -= optioncount;
apps/multirotor_att_control/multirotor_attitude_control.c
+19 -9
View File
@@ -188,7 +188,7 @@ static int parameters_update(const struct mc_att_control_param_handles *h, struc
}
void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
const struct vehicle_attitude_s *att, struct actuator_controls_s *actuators)
const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, struct actuator_controls_s *actuators)
{
static uint64_t last_run = 0;
const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
@@ -214,11 +214,11 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
// pid_init(&yaw_pos_controller, p.yaw_p, p.yaw_i, p.yaw_d, p.yaw_awu,
// PID_MODE_DERIVATIV_SET, 154);
pid_init(&yaw_speed_controller, p.yawrate_p, p.yawrate_i, p.yawrate_d, p.yawrate_awu,
PID_MODE_DERIVATIV_SET, 155);
PID_MODE_DERIVATIV_SET);
pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, p.att_awu,
PID_MODE_DERIVATIV_SET, 156);
PID_MODE_DERIVATIV_SET);
pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, p.att_awu,
PID_MODE_DERIVATIV_SET, 157);
PID_MODE_DERIVATIV_SET);
initialized = true;
}
@@ -243,7 +243,7 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
float roll_control = pid_calculate(&roll_controller, att_sp->roll_body + p.att_yoff,
att->roll, att->rollspeed, deltaT);
/* control yaw rate */
float yaw_rate_control = pid_calculate(&yaw_speed_controller, att_sp->yaw_rate_body, att->yawspeed, 0.0f, deltaT);
float yaw_rate_control = pid_calculate(&yaw_speed_controller, att_sp->yaw_body, att->yawspeed, 0.0f, deltaT);
/*
* compensate the vertical loss of thrust
@@ -305,10 +305,20 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
roll_controller.saturated = 1;
}
actuators->control[0] = roll_control;
actuators->control[1] = pitch_control;
actuators->control[2] = yaw_rate_control;
actuators->control[3] = motor_thrust;
if (actuators) {
actuators->control[0] = roll_control;
actuators->control[1] = pitch_control;
actuators->control[2] = yaw_rate_control;
actuators->control[3] = motor_thrust;
}
// XXX change yaw rate to yaw pos controller
if (rates_sp) {
rates_sp->roll = roll_control;
rates_sp->pitch = pitch_control;
rates_sp->yaw = yaw_rate_control;
rates_sp->thrust = motor_thrust;
}
motor_skip_counter++;
}
apps/multirotor_att_control/multirotor_attitude_control.h
+3 -2
View File
@@ -48,9 +48,10 @@
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/actuator_controls.h>
void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att,
struct actuator_controls_s *actuators);
void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, struct actuator_controls_s *actuators);
#endif /* MULTIROTOR_ATTITUDE_CONTROL_H_ */
apps/multirotor_att_control/multirotor_rate_control.c
+7 -7
View File
@@ -132,7 +132,7 @@ static int parameters_update(const struct mc_rate_control_param_handles *h, stru
return OK;
}
void multirotor_control_rates(const struct vehicle_attitude_setpoint_s *rate_sp,
void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
const float rates[], struct actuator_controls_s *actuators)
{
static uint64_t last_run = 0;
@@ -157,11 +157,11 @@ void multirotor_control_rates(const struct vehicle_attitude_setpoint_s *rate_sp,
parameters_update(&h, &p);
pid_init(&yaw_speed_controller, p.yawrate_p, 0, p.yawrate_i, p.yawrate_awu,
PID_MODE_DERIVATIV_SET, 155);
PID_MODE_DERIVATIV_SET);
pid_init(&pitch_controller, p.attrate_p, p.attrate_i, 0, p.attrate_awu,
PID_MODE_DERIVATIV_SET, 156);
PID_MODE_DERIVATIV_SET);
pid_init(&roll_controller, p.attrate_p, p.attrate_i, 0, p.attrate_awu,
PID_MODE_DERIVATIV_SET, 157);
PID_MODE_DERIVATIV_SET);
initialized = true;
}
@@ -179,13 +179,13 @@ void multirotor_control_rates(const struct vehicle_attitude_setpoint_s *rate_sp,
/* calculate current control outputs */
/* control pitch (forward) output */
float pitch_control = pid_calculate(&pitch_controller, rate_sp->pitch_rate_body,
float pitch_control = pid_calculate(&pitch_controller, rate_sp->pitch,
rates[1], 0.0f, deltaT);
/* control roll (left/right) output */
float roll_control = pid_calculate(&roll_controller, rate_sp->roll_rate_body,
float roll_control = pid_calculate(&roll_controller, rate_sp->roll,
rates[0], 0.0f, deltaT);
/* control yaw rate */
float yaw_rate_control = pid_calculate(&yaw_speed_controller, rate_sp->yaw_rate_body, rates[2], 0.0f, deltaT);
float yaw_rate_control = pid_calculate(&yaw_speed_controller, rate_sp->yaw, rates[2], 0.0f, deltaT);
/*
* compensate the vertical loss of thrust
apps/multirotor_att_control/multirotor_rate_control.h
+2 -2
View File
@@ -47,10 +47,10 @@
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/actuator_controls.h>
void multirotor_control_rates(const struct vehicle_attitude_setpoint_s *rate_sp,
void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
const float rates[], struct actuator_controls_s *actuators);
#endif /* MULTIROTOR_RATE_CONTROL_H_ */
apps/multirotor_pos_control/position_control.c
-40
View File
@@ -51,46 +51,6 @@
#include "multirotor_position_control.h"
float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next)
{
double lat_now_rad = lat_now / 180.0 * M_PI;
double lon_now_rad = lon_now / 180.0 * M_PI;
double lat_next_rad = lat_next / 180.0 * M_PI;
double lon_next_rad = lon_next / 180.0 * M_PI;
double d_lat = lat_next_rad - lat_now_rad;
double d_lon = lon_next_rad - lon_now_rad;
double a = sin(d_lat / 2.0) * sin(d_lat / 2.0) + sin(d_lon / 2.0) * sin(d_lon / 2.0) * cos(lat_now_rad) * cos(lat_next_rad);
double c = 2 * atan2(sqrt(a), sqrt(1 - a));
const double radius_earth = 6371000.0;
return radius_earth * c;
}
float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next)
{
double lat_now_rad = lat_now / 180.0 * M_PI;
double lon_now_rad = lon_now / 180.0 * M_PI;
double lat_next_rad = lat_next / 180.0 * M_PI;
double lon_next_rad = lon_next / 180.0 * M_PI;
double d_lat = lat_next_rad - lat_now_rad;
double d_lon = lon_next_rad - lon_now_rad;
/* conscious mix of double and float trig function to maximize speed and efficiency */
float theta = atan2f(sin(d_lon) * cos(lat_next_rad) , cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon));
// XXX wrapping check is incomplete
if (theta < 0.0f) {
theta = theta + 2.0f * M_PI_F;
}
return theta;
}
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)
apps/px4/ground_estimator/ground_estimator.c
+2 -2
View File
@@ -75,7 +75,7 @@ int ground_estimator_thread_main(int argc, char *argv[]) {
/* subscribe to raw data */
int sub_raw = orb_subscribe(ORB_ID(sensor_combined));
/* advertise attitude */
/* advertise debug value */
struct debug_key_value_s dbg = { .key = "posx", .value = 0.0f };
orb_advert_t pub_dbg = orb_advertise(ORB_ID(debug_key_value), &dbg);
@@ -97,7 +97,7 @@ int ground_estimator_thread_main(int argc, char *argv[]) {
/* XXX this is seriously bad - should be an emergency */
} else if (ret == 0) {
/* XXX this means no sensor data - should be critical or emergency */
printf("[attitude estimator bm] WARNING: Not getting sensor data - sensor app running?\n");
printf("[ground estimator bm] WARNING: Not getting sensor data - sensor app running?\n");
} else {
struct sensor_combined_s s;
orb_copy(ORB_ID(sensor_combined), sub_raw, &s);
apps/sdlog/sdlog.c
+3 -1
View File
@@ -289,6 +289,8 @@ int sdlog_thread_main(int argc, char *argv[]) {
/* subscribe to ORB for sensors raw */
subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
fds[fdsc_count].fd = subs.sensor_sub;
/* rate-limit raw data updates to 200Hz */
orb_set_interval(subs.sensor_sub, 5);
fds[fdsc_count].events = POLLIN;
fdsc_count++;
@@ -459,7 +461,7 @@ int sdlog_thread_main(int argc, char *argv[]) {
sysvector_bytes += write(sysvector_file, (const char*)&sysvector, sizeof(sysvector));
usleep(4900);
usleep(10000);
}
unsigned bytes = sensor_combined_bytes + actuator_outputs_bytes + blackbox_file_bytes + actuator_controls_bytes;
apps/sensors/sensors.cpp
+7 -4
View File
@@ -567,7 +567,7 @@ Sensors::accel_init()
_fd_bma180 = open("/dev/bma180", O_RDONLY);
if (_fd_bma180 < 0) {
warn("/dev/bma180");
errx(1, "FATAL: no accelerometer found");
warn("FATAL: no accelerometer found");
}
/* discard first (junk) reading */
@@ -600,7 +600,7 @@ Sensors::gyro_init()
_fd_gyro_l3gd20 = open("/dev/l3gd20", O_RDONLY);
if (_fd_gyro_l3gd20 < 0) {
warn("/dev/l3gd20");
errx(1, "FATAL: no gyro found");
warn("FATAL: no gyro found");
}
/* discard first (junk) reading */
@@ -968,7 +968,7 @@ Sensors::ppm_poll()
_rc.chan[i].scaled = (ppm_buffer[i] - _parameters.trim[i]) / (float)(_parameters.max[i] - _parameters.trim[i]);
} else if (ppm_buffer[i] < (_parameters.trim[i] - _parameters.dz[i])) {
/* division by zero impossible for trim == min (as for throttle), as this falls in the above if clause */
_rc.chan[i].scaled = -1.0f + ((ppm_buffer[i] - _parameters.min[i]) / (float)(_parameters.trim[i] - _parameters.min[i]));
_rc.chan[i].scaled = -((_parameters.trim[i] - ppm_buffer[i]) / (float)(_parameters.trim[i] - _parameters.min[i]));
} else {
/* in the configured dead zone, output zero */
@@ -988,6 +988,8 @@ Sensors::ppm_poll()
_rc.chan_count = ppm_decoded_channels;
_rc.timestamp = ppm_last_valid_decode;
manual_control.timestamp = ppm_last_valid_decode;
/* roll input - rolling right is stick-wise and rotation-wise positive */
manual_control.roll = _rc.chan[_rc.function[ROLL]].scaled;
if (manual_control.roll < -1.0f) manual_control.roll = -1.0f;
@@ -1027,6 +1029,7 @@ Sensors::ppm_poll()
orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc);
orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual_control);
}
#endif
@@ -1090,7 +1093,7 @@ Sensors::task_main()
/* advertise the manual_control topic */
{
struct manual_control_setpoint_s manual_control;
manual_control.mode = ROLLPOS_PITCHPOS_YAWRATE_THROTTLE;
manual_control.mode = MANUAL_CONTROL_MODE_ATT_YAW_RATE;
manual_control.roll = 0.0f;
manual_control.pitch = 0.0f;
manual_control.yaw = 0.0f;
apps/systemlib/Makefile
+2 -1
View File
@@ -48,7 +48,8 @@ CSRCS = err.c \
#
ifeq ($(TARGET),px4fmu)
CSRCS += systemlib.c \
pid/pid.c
pid/pid.c \
geo/geo.c
endif
include $(APPDIR)/mk/app.mk
apps/systemlib/geo/geo.c
+88
View File
@@ -0,0 +1,88 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
* Julian Oes <joes@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 geo.c
*
* Geo / math functions to perform geodesic calculations
*
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
*/
#include <systemlib/geo/geo.h>
#include <math.h>
__EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next)
{
double lat_now_rad = lat_now / 180.0 * M_PI;
double lon_now_rad = lon_now / 180.0 * M_PI;
double lat_next_rad = lat_next / 180.0 * M_PI;
double lon_next_rad = lon_next / 180.0 * M_PI;
double d_lat = lat_next_rad - lat_now_rad;
double d_lon = lon_next_rad - lon_now_rad;
double a = sin(d_lat / 2.0) * sin(d_lat / 2.0) + sin(d_lon / 2.0) * sin(d_lon / 2.0) * cos(lat_now_rad) * cos(lat_next_rad);
double c = 2 * atan2(sqrt(a), sqrt(1 - a));
const double radius_earth = 6371000.0;
return radius_earth * c;
}
__EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next)
{
double lat_now_rad = lat_now / 180.0 * M_PI;
double lon_now_rad = lon_now / 180.0 * M_PI;
double lat_next_rad = lat_next / 180.0 * M_PI;
double lon_next_rad = lon_next / 180.0 * M_PI;
double d_lat = lat_next_rad - lat_now_rad;
double d_lon = lon_next_rad - lon_now_rad;
/* conscious mix of double and float trig function to maximize speed and efficiency */
float theta = atan2f(sin(d_lon) * cos(lat_next_rad) , cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon));
// XXX wrapping check is incomplete
if (theta < 0.0f) {
theta = theta + 2.0f * M_PI_F;
}
return theta;
}
apps/systemlib/geo/geo.h
+49
View File
@@ -0,0 +1,49 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
* Julian Oes <joes@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 geo.h
*
* Definition of geo / math functions to perform geodesic calculations
*
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
*/
__EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next);
__EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next);
apps/systemlib/param/param.c
+2 -2
View File
@@ -484,7 +484,7 @@ param_export(int fd, bool only_unsaved)
s->unsaved = false;
/* append the appripriate BSON type object */
/* append the appropriate BSON type object */
switch (param_type(s->param)) {
case PARAM_TYPE_INT32:
param_get(s->param, &i);
@@ -688,4 +688,4 @@ param_foreach(void (*func)(void *arg, param_t param), void *arg, bool only_chang
func(arg, param);
}
}
}
apps/systemlib/pid/pid.c
+68 -23
View File
@@ -40,34 +40,54 @@
*/
#include "pid.h"
#include <math.h>
__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax,
uint8_t mode, uint8_t plot_i)
uint8_t mode)
{
pid->kp = kp;
pid->ki = ki;
pid->kd = kd;
pid->intmax = intmax;
pid->mode = mode;
pid->plot_i = plot_i;
pid->count = 0;
pid->saturated = 0;
pid->last_output = 0;
pid->sp = 0;
pid->error_previous = 0;
pid->integral = 0;
}
__EXPORT void pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax)
__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax)
{
pid->kp = kp;
pid->ki = ki;
pid->kd = kd;
pid->intmax = intmax;
// pid->mode = mode;
int ret = 0;
// pid->sp = 0;
// pid->error_previous = 0;
// pid->integral = 0;
if (isfinite(kp)) {
pid->kp = kp;
} else {
ret = 1;
}
if (isfinite(ki)) {
pid->ki = ki;
} else {
ret = 1;
}
if (isfinite(kd)) {
pid->kd = kd;
} else {
ret = 1;
}
if (isfinite(intmax)) {
pid->intmax = intmax;
} else {
ret = 1;
}
// pid->limit = limit;
return ret;
}
//void pid_set(PID_t *pid, float sp)
@@ -95,6 +115,11 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
goto start
*/
if (!isfinite(sp) || !isfinite(val) || !isfinite(val_dot) || !isfinite(dt))
{
return pid->last_output;
}
float i, d;
pid->sp = sp;
float error = pid->sp - val;
@@ -111,7 +136,7 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
}
// Anti-Windup. Needed if we don't use the saturation above.
if (pid->intmax != 0.0) {
if (pid->intmax != 0.0f) {
if (i > pid->intmax) {
pid->integral = pid->intmax;
@@ -122,14 +147,6 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
} else {
pid->integral = i;
}
//Send Controller integrals
// Disabled because of new possibilities with debug_vect.
// Now sent in Main Loop at 5 Hz. 26.06.2010 Laurens
// if (pid->plot_i && (pid->count++ % 16 == 0)&&(global_data.param[PARAM_SEND_SLOT_DEBUG_2] == 1))
// {
// mavlink_msg_debug_send(MAVLINK_COMM_1, pid->plot_i, pid->integral);
// }
}
if (pid->mode == PID_MODE_DERIVATIV_CALC) {
@@ -139,10 +156,38 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
d = -val_dot;
} else {
d = 0;
d = 0.0f;
}
pid->error_previous = error;
if (pid->kd == 0.0f) {
d = 0.0f;
}
return (error * pid->kp) + (i * pid->ki) + (d * pid->kd);
if (pid->ki == 0.0f) {
i = 0;
}
float p;
if (pid->kp == 0.0f) {
p = 0.0f;
} else {
p = error;
}
if (isfinite(error)) {
pid->error_previous = error;
}
float output = (error * pid->kp) + (i * pid->ki) + (d * pid->kd);
if (isfinite(output)) {
pid->last_output = output;
}
if (!isfinite(pid->integral)) {
pid->integral = 0;
}
return pid->last_output;
}
apps/systemlib/pid/pid.h
+4 -3
View File
@@ -58,14 +58,15 @@ typedef struct {
float sp;
float integral;
float error_previous;
float last_output;
float limit;
uint8_t mode;
uint8_t plot_i;
uint8_t count;
uint8_t saturated;
} PID_t;
__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, uint8_t mode, uint8_t plot_i);
__EXPORT void pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax);
__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, uint8_t mode);
__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax);
//void pid_set(PID_t *pid, float sp);
__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt);
apps/uORB/objects_common.cpp
+5 -3
View File
@@ -59,7 +59,6 @@ ORB_DEFINE(output_pwm, struct pwm_output_values);
#include <drivers/drv_rc_input.h>
ORB_DEFINE(input_rc, struct rc_input_values);
// XXX need to check wether these should be here
#include "topics/vehicle_attitude.h"
ORB_DEFINE(vehicle_attitude, struct vehicle_attitude_s);
@@ -78,8 +77,8 @@ ORB_DEFINE(vehicle_global_position, struct vehicle_global_position_s);
#include "topics/vehicle_local_position.h"
ORB_DEFINE(vehicle_local_position, struct vehicle_local_position_s);
#include "topics/ardrone_motors_setpoint.h"
ORB_DEFINE(ardrone_motors_setpoint, struct ardrone_motors_setpoint_s);
#include "topics/vehicle_rates_setpoint.h"
ORB_DEFINE(vehicle_rates_setpoint, struct vehicle_rates_setpoint_s);
#include "topics/rc_channels.h"
ORB_DEFINE(rc_channels, struct rc_channels_s);
@@ -99,6 +98,9 @@ ORB_DEFINE(vehicle_attitude_setpoint, struct vehicle_attitude_setpoint_s);
#include "topics/manual_control_setpoint.h"
ORB_DEFINE(manual_control_setpoint, struct manual_control_setpoint_s);
#include "topics/offboard_control_setpoint.h"
ORB_DEFINE(offboard_control_setpoint, struct offboard_control_setpoint_s);
#include "topics/optical_flow.h"
ORB_DEFINE(optical_flow, struct optical_flow_s);
apps/uORB/topics/manual_control_setpoint.h
+23 -27
View File
@@ -1,21 +1,21 @@
/****************************************************************************
*
* Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
* Author: @author Lorenz Meier <lm@inf.ethz.ch>
* Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
* Author: @author Lorenz Meier <lm@inf.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 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.
* 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.
* 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
@@ -57,31 +57,27 @@
*/
enum MANUAL_CONTROL_MODE
{
DIRECT = 0,
ROLLPOS_PITCHPOS_YAWRATE_THROTTLE = 1,
ROLLRATE_PITCHRATE_YAWRATE_THROTTLE = 2,
ROLLPOS_PITCHPOS_YAWPOS_THROTTLE = 3
MANUAL_CONTROL_MODE_DIRECT = 0,
MANUAL_CONTROL_MODE_ATT_YAW_RATE = 1,
MANUAL_CONTROL_MODE_ATT_YAW_POS = 2,
MANUAL_CONTROL_MODE_MULTIROTOR_SIMPLE = 3 /**< roll / pitch rotated aligned to the takeoff orientation, throttle stabilized, yaw pos */
};
struct manual_control_setpoint_s {
uint64_t timestamp;
enum MANUAL_CONTROL_MODE mode; /**< The current control inputs mode */
float roll; /**< roll / roll rate input */
float pitch;
float yaw;
float throttle;
enum MANUAL_CONTROL_MODE mode; /**< The current control inputs mode */
float roll; /**< ailerons roll / roll rate input */
float pitch; /**< elevator / pitch / pitch rate */
float yaw; /**< rudder / yaw rate / yaw */
float throttle; /**< throttle / collective thrust / altitude */
float override_mode_switch;
float override_mode_switch;
float ailerons;
float elevator;
float rudder;
float flaps;
float aux1_cam_pan;
float aux2_cam_tilt;
float aux3_cam_zoom;
float aux4_cam_roll;
float aux1_cam_pan_flaps;
float aux2_cam_tilt;
float aux3_cam_zoom;
float aux4_cam_roll;
}; /**< manual control inputs */
apps/uORB/topics/offboard_control_setpoint.h
+94
View File
@@ -0,0 +1,94 @@
/****************************************************************************
*
* Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
* Author: @author Lorenz Meier <lm@inf.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file offboard_control_setpoint.h
* Definition of the manual_control_setpoint uORB topic.
*/
#ifndef TOPIC_OFFBOARD_CONTROL_SETPOINT_H_
#define TOPIC_OFFBOARD_CONTROL_SETPOINT_H_
#include <stdint.h>
#include "../uORB.h"
/**
* @addtogroup topics
* @{
*/
/**
* Off-board control inputs.
*
* Typically sent by a ground control station / joystick or by
* some off-board controller via C or SIMULINK.
*/
enum OFFBOARD_CONTROL_MODE
{
OFFBOARD_CONTROL_MODE_DIRECT = 0,
OFFBOARD_CONTROL_MODE_DIRECT_RATES = 1,
OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE = 2,
OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY = 3,
OFFBOARD_CONTROL_MODE_DIRECT_POSITION = 4,
OFFBOARD_CONTROL_MODE_ATT_YAW_RATE = 5,
OFFBOARD_CONTROL_MODE_ATT_YAW_POS = 6,
OFFBOARD_CONTROL_MODE_MULTIROTOR_SIMPLE = 7, /**< roll / pitch rotated aligned to the takeoff orientation, throttle stabilized, yaw pos */
};
struct offboard_control_setpoint_s {
uint64_t timestamp;
enum OFFBOARD_CONTROL_MODE mode; /**< The current control inputs mode */
bool armed; /**< Armed flag set, yes / no */
float p1; /**< ailerons roll / roll rate input */
float p2; /**< elevator / pitch / pitch rate */
float p3; /**< rudder / yaw rate / yaw */
float p4; /**< throttle / collective thrust / altitude */
float override_mode_switch;
float aux1_cam_pan_flaps;
float aux2_cam_tilt;
float aux3_cam_zoom;
float aux4_cam_roll;
}; /**< offboard control inputs */
/**
* @}
*/
/* register this as object request broker structure */
ORB_DECLARE(offboard_control_setpoint);
#endif
apps/uORB/topics/vehicle_attitude_setpoint.h
+7 -10
View File
@@ -56,21 +56,18 @@ struct vehicle_attitude_setpoint_s
{
uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */
float roll_tait_bryan; /**< Tait-Bryan angle in NED frame */
float pitch_tait_bryan; /**< Tait-Bryan angle in NED frame */
float yaw_tait_bryan; /**< Tait-Bryan angle in NED frame */
float tait_bryan_valid; /**< Set to true if Tait-Bryan angles are valid */
//float roll_tait_bryan; /**< Tait-Bryan angle in NED frame */
//float pitch_tait_bryan; /**< Tait-Bryan angle in NED frame */
//float yaw_tait_bryan; /**< Tait-Bryan angle in NED frame */
//float tait_bryan_valid; /**< Set to true if Tait-Bryan angles are valid */
float roll_body; /**< body angle in NED frame */
float pitch_body; /**< body angle in NED frame */
float yaw_body; /**< body angle in NED frame */
float roll_rate_body; /**< body angle in NED frame */
float pitch_rate_body; /**< body angle in NED frame */
float yaw_rate_body; /**< body angle in NED frame */
float body_valid; /**< Set to true if Tait-Bryan angles are valid */
//float body_valid; /**< Set to true if body angles are valid */
float R_body[9]; /**< Rotation matrix describing the setpoint as rotation from the current body frame */
bool R_valid; /**< Set to true if rotation matrix is valid */
//float R_body[9]; /**< Rotation matrix describing the setpoint as rotation from the current body frame */
//bool R_valid; /**< Set to true if rotation matrix is valid */
float thrust; /**< Thrust in Newton the power system should generate */
apps/uORB/topics/vehicle_command.h
+4
View File
@@ -50,6 +50,10 @@
* @{
*/
enum PX4_CMD {
PX4_CMD_CONTROLLER_SELECTION = 1000,
};
struct vehicle_command_s
{
float param1; /**< Parameter 1, as defined by MAVLink MAV_CMD enum. */
apps/uORB/topics/ardrone_motors_setpoint.h → apps/uORB/topics/vehicle_rates_setpoint.h
+13 -15
View File
@@ -33,12 +33,12 @@
****************************************************************************/
/**
* @file ardrone_motors_setpoint.h
* Definition of the ardrone_motors_setpoint uORB topic.
* @file vehicle_rates_setpoint.h
* Definition of the vehicle rates setpoint topic
*/
#ifndef TOPIC_ARDRONE_MOTORS_SETPOINT_H_
#define TOPIC_ARDRONE_MOTORS_SETPOINT_H_
#ifndef TOPIC_VEHICLE_RATES_SETPOINT_H_
#define TOPIC_VEHICLE_RATES_SETPOINT_H_
#include <stdint.h>
#include "../uORB.h"
@@ -47,24 +47,22 @@
* @addtogroup topics
* @{
*/
struct ardrone_motors_setpoint_s
struct vehicle_rates_setpoint_s
{
uint64_t timestamp; //in microseconds since system start, is set whenever the writing thread stores new data
uint64_t timestamp; /**< in microseconds since system start */
uint8_t group; /**< quadrotor group */
uint8_t mode; /**< requested flight mode XXX define */
float p1; /**< parameter 1: rate control: roll rate, att control: roll angle (in radians, NED) */
float p2; /**< parameter 2: rate control: pitch rate, att control: pitch angle (in radians, NED) */
float p3; /**< parameter 3: rate control: yaw rate, att control: yaw angle (in radians, NED) */
float p4; /**< parameter 4: thrust, [0..1] */
}; /**< AR.Drone low level motors */
float roll; /**< body angular rates in NED frame */
float pitch; /**< body angular rates in NED frame */
float yaw; /**< body angular rates in NED frame */
float thrust; /**< thrust normalized to 0..1 */
}; /**< vehicle_rates_setpoint */
/**
* @}
*/
/* register this as object request broker structure */
ORB_DECLARE(ardrone_motors_setpoint);
ORB_DECLARE(vehicle_rates_setpoint);
#endif
apps/uORB/topics/vehicle_status.h
+12 -1
View File
@@ -110,6 +110,8 @@ struct vehicle_status_s
uint16_t counter; /**< incremented by the writing thread everytime new data is stored */
uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */
uint64_t failsave_lowlevel_start_time; /**< time when the lowlevel failsafe flag was set */
//uint64_t failsave_highlevel_begin; TO BE COMPLETED
commander_state_machine_t state_machine; /**< current flight state, main state machine */
enum VEHICLE_FLIGHT_MODE flight_mode; /**< current flight mode, as defined by mode switch */
@@ -127,17 +129,26 @@ struct vehicle_status_s
bool flag_control_rates_enabled; /**< true if rates are stabilized */
bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */
bool flag_control_speed_enabled; /**< true if speed (implies direction) is controlled */
bool flag_control_velocity_enabled; /**< true if speed (implies direction) is controlled */
bool flag_control_position_enabled; /**< true if position is controlled */
bool flag_preflight_gyro_calibration; /**< true if gyro calibration is requested */
bool flag_preflight_mag_calibration; /**< true if mag calibration is requested */
bool flag_preflight_accel_calibration;
bool rc_signal_found_once;
bool rc_signal_lost; /**< true if RC reception is terminally lost */
bool rc_signal_cutting_off; /**< true if RC reception is weak / cutting off */
uint64_t rc_signal_lost_interval; /**< interval in microseconds since when no RC signal is available */
bool offboard_control_signal_found_once;
bool offboard_control_signal_lost;
bool offboard_control_signal_weak;
uint64_t offboard_control_signal_lost_interval; /**< interval in microseconds without an offboard control message */
bool failsave_lowlevel; /**< Set to true if low-level failsafe mode is enabled */
//bool failsave_highlevel;
/* see SYS_STATUS mavlink message for the following */
uint32_t onboard_control_sensors_present;
uint32_t onboard_control_sensors_enabled;
nuttx/ChangeLog
+43
View File
@@ -3377,3 +3377,46 @@
commands used in the build (Contributed by Richard Cochran).
* drivers/net/enc28j60.c: The ENC28J60 Ethernet driver is
now functional.
* configs/fire-stm32v2: Add support or the fire-stm32v3 board as
well (untested because I do not have a v3 board).
* lib/stdio/lib_sscanf.c: Add %n psuedo-format (from Kate).
* lib/stdio/lib_sscanf.c: There is an issue of handling input
when (1) no fieldwidth is provided and (2) there is no space
seperating the input values. No solutions is in place for this
case now (either space or a fieldwidth must be provided). But
at least some of the bad logic that attempted to handle this
case has been removed (noted by Kate).
* arch/arm/src/stm32/stm32_eth.c: DMA buffer sizes must be an
even multiple of 4, 8, or 16 bytes.
* arch/arm/src/stm32/stm32_idle.c: Fixes STM32F107 DMA issues:
We cannot go into sleep mode while Ethernet is actively DMAing.
* configs/shenzhou/src/up_ssd1289.c: Add infrastructure to support
SSD1289 LCD. Initial checkin is just a clone of the
STM32F4Discovery's FSMC-based LCD interface. The Shenzhou
will need a completely need bit-banging interface; this
initial check-in is only for the framework.
* configs/shenzhou/src/up_ssd1289.c: Bit-banging driver is
code complete.
* configs/shenzhou/src/up_lcd.c: Oops. Shenzhou LCD does not
have an SSD1289 controller. Its an ILI93xx. Ported the
STM3240G-EVAL ILI93xx driver to work on the Shenzhou board.
* configs/shenzhou/nxwm: Added an NxWM configuratino for the
Shenzhou board. This is untested on initial check-in. It will
be used to verify the Shenzhou LCD driver (and eventually the
touchscreen driver).
* configs/shenzhou/src/up_touchscreen.c: Add ADS7843E touchscreen
support for the Shenzhou board. The initial check-in is untested
and basically a clone of the the touchscreen support fro the SAM-3U.
* tools/cfgparser.c: There are some NxWidget configuration
settings that must be de-quoted.
* arch/arm/src/stm32/Kconfig: There is no SPI4. Some platforms
SPI3 and some do not (still not clear).
* nuttx/configs/shenzhou: Various fixes to build new NxWM
configuration.
* configs/shenzhou: Oops. The Shenzhou LCD is and SSD1289,
not an ILI93xx.
* configs/shenzhou/src/up_ssd1289.c: The LCD is basically functional
on the Shenzhou board.
* graphics/nxmu: Correct some bad parameter checking that caused
failures when DEBUG was enabled.
nuttx/Kconfig
+49 -17
View File
@@ -216,7 +216,7 @@ endmenu
menu "Debug Options"
config DEBUG
bool "Enable debug features"
bool "Enable Debug Features"
default n
---help---
Enables built-in debug features. Selecting this option will (1) Enable
@@ -227,87 +227,115 @@ config DEBUG
if DEBUG
config DEBUG_VERBOSE
bool "Enable debug verbose output"
bool "Enable Debug Verbose Output"
default n
---help---
Enables verbose debug output (assuming debug output is enabled)
Enables verbose debug output (assuming debug output is enabled). As a
general rule, when DEBUG is enabled only errors will be reported in the debug
output. But if you also enable DEBUG_VERBOSE, then very chatty (and
often annoying) output will be generated. This means there are two levels
of debug output: errors-only and everything.
config DEBUG_ENABLE
bool "Enable debug controls"
bool "Enable Debug Controls"
default n
---help---
Support an interface to dynamically enable or disable debug output.
comment "Subsystem Debug Options"
config DEBUG_SCHED
bool "Enable scheduler debug output"
bool "Enable Scheduler Debug Output"
default n
---help---
Enable OS debug output (disabled by default)
config DEBUG_MM
bool "Enable memory manager debug output"
bool "Enable Memory Manager Debug Output"
default n
---help---
Enable memory management debug output (disabled by default)
config DEBUG_NET
bool "Enable network debug output"
bool "Enable Network Debug Output"
default n
depends on NET
---help---
Enable network debug output (disabled by default)
config DEBUG_USB
bool "Enable USB debug output"
bool "Enable USB Debug Output"
default n
depends on USBDEV || USBHOST
---help---
Enable usb debug output (disabled by default)
config DEBUG_FS
bool "Enable file system debug output"
bool "Enable File System Debug Output"
default n
---help---
Enable file system debug output (disabled by default)
config DEBUG_LIB
bool "Enable C library debug output"
bool "Enable C Library Debug Output"
default n
---help---
Enable C library debug output (disabled by default)
config DEBUG_BINFMT
bool "Enable binary loader debug output"
bool "Enable Binary Loader Debug Output"
default n
---help---
Enable binary loader debug output (disabled by default)
config DEBUG_GRAPHICS
bool "Enable graphics debug output"
bool "Enable Graphics Debug Output"
default n
---help---
Enable NX graphics debug output (disabled by default)
config DEBUG_I2C
bool "Enable I2C debug output"
comment "Driver Debug Options"
config DEBUG_LCD
bool "Enable Low-level LCD Debug Output"
default n
depends on LCD
---help---
Enable low level debug output from the LCD driver (disabled by default)
config DEBUG_INPUT
bool "Enable Input Device Debug Output"
default n
depends on INPUT
---help---
Enable low level debug output from the input device drivers such as
mice and touchscreens (disabled by default)
config DEBUG_I2C
bool "Enable I2C Debug Output"
default n
depends on I2C
---help---
Enable I2C driver debug output (disabled by default)
config DEBUG_SPI
bool "Enable SPI debug output"
bool "Enable SPI Debug Output"
default n
depends on SPI
---help---
Enable I2C driver debug output (disabled by default)
config DEBUG_WATCHDOG
bool "Enable watchdog timer debug output"
bool "Enable Watchdog Timer Debug Output"
default n
depends on WATCHDOG
---help---
Enable watchdog timer debug output (disabled by default)
endif
config DEBUG_SYMBOLS
bool "Enable debug symbols"
bool "Enable Debug Symbols"
default n
---help---
Build without optimization and with debug symbols (needed
@@ -339,6 +367,10 @@ menu "File Systems"
source fs/Kconfig
endmenu
menu "Graphics Support"
source graphics/Kconfig
endmenu
menu "Memory Management"
source mm/Kconfig
endmenu
nuttx/Makefile
+2 -1
View File
@@ -38,7 +38,8 @@ TOPDIR := ${shell pwd | sed -e 's/ /\\ /g'}
-include ${TOPDIR}/tools/Config.mk
-include ${TOPDIR}/Make.defs
# Control build verbosity.
# Control build verbosity
ifeq ($(V),1)
export Q :=
else
nuttx/README.txt
+2
View File
@@ -848,6 +848,8 @@ apps
| `- README.txt
|- nshlib/
| `- README.txt
|- NxWidgets/
| `- README.txt
|- system/
| |- i2c/README.txt
| |- free/README.txt
nuttx/arch/arm/src/stm32/Kconfig
+76 -31
View File
@@ -340,14 +340,7 @@ config STM32_SPI2
config STM32_SPI3
bool "SPI3"
default n
depends on STM32_STM32F20XX || STM32_STM32F40XX
select SPI
select STM32_SPI
config STM32_SPI4
bool "SPI4"
default n
depends on STM32_STM32F10XX
depends on STM32_CONNECTIVITYLINE || STM32_STM32F20XX || STM32_STM32F40XX
select SPI
select STM32_SPI
@@ -1623,12 +1616,12 @@ config STM32_I2C_DYNTIMEO
config STM32_I2C_DYNTIMEO_USECPERBYTE
int "Timeout Microseconds per Byte"
default 0
default 500
depends on STM32_I2C_DYNTIMEO
config STM32_I2C_DYNTIMEO_STARTSTOP
int "Timeout for Start/Stop (Milliseconds)"
default 5000
default 1000
depends on STM32_I2C_DYNTIMEO
config STM32_I2CTIMEOSEC
@@ -1684,6 +1677,7 @@ config SDIO_WIDTH_D1_ONLY
endmenu
if STM32_ETHMAC
menu "Ethernet MAC configuration"
config STM32_PHYADDR
@@ -1695,7 +1689,6 @@ config STM32_PHYADDR
config STM32_MII
bool "Use MII interface"
default n
depends on STM32_ETHMAC
---help---
Support Ethernet MII interface.
@@ -1732,16 +1725,15 @@ config STM32_MII_EXTCLK
endchoice
config STM32_AUTONEG
bool "Use autonegtiation"
bool "Use autonegotiation"
default y
depends on STM32_ETHMAC
---help---
Use PHY autonegotion to determine speed and mode
Use PHY autonegotiation to determine speed and mode
config STM32_ETHFD
bool "Full duplex"
default n
depends on STM32_ETHMAC && !STM32_AUTONEG
depends on !STM32_AUTONEG
---help---
If STM32_AUTONEG is not defined, then this may be defined to select full duplex
mode. Default: half-duplex
@@ -1749,61 +1741,104 @@ config STM32_ETHFD
config STM32_ETH100MBPS
bool "100 Mbps"
default n
depends on STM32_ETHMAC && !STM32_AUTONEG
depends on !STM32_AUTONEG
---help---
If STM32_AUTONEG is not defined, then this may be defined to select 100 MBps
speed. Default: 10 Mbps
config STM32_PHYSR
hex "PHY status register address"
int "PHY Status Register Address (decimal)"
depends on STM32_AUTONEG
---help---
This must be provided if STM32_AUTONEG is defined. The PHY status register
address may diff from PHY to PHY. This configuration sets the address of
the PHY status register.
config STM32_PHYSR_SPEED
hex "PHY speed mask"
config STM32_PHYSR_ALTCONFIG
bool "PHY Status Alternate Bit Layout"
default n
depends on STM32_AUTONEG
---help---
Different PHYs present speed and mode information in different ways. Some
will present separate information for speed and mode (this is the default).
Those PHYs, for example, may provide a 10/100 Mbps indication and a separate
full/half duplex indication. This options selects an alternative representation
where speed and mode information are combined. This might mean, for example,
separate bits for 10HD, 100HD, 10FD and 100FD.
config STM32_PHYSR_SPEED
hex "PHY Speed Mask"
depends on STM32_AUTONEG && !STM32_PHYSR_ALTCONFIG
---help---
This must be provided if STM32_AUTONEG is defined. This provides bit mask
indicating 10 or 100MBps speed.
for isolating the 10 or 100MBps speed indication.
config STM32_PHYSR_100MBPS
hex "PHY 100Mbps speed value"
depends on STM32_AUTONEG
hex "PHY 100Mbps Speed Value"
depends on STM32_AUTONEG && !STM32_PHYSR_ALTCONFIG
---help---
This must be provided if STM32_AUTONEG is defined. This provides the value
of the speed bit(s) indicating 100MBps speed.
config STM32_PHYSR_MODE
hex "PHY mode mask"
depends on STM32_AUTONEG
hex "PHY Mode Mask"
depends on STM32_AUTONEG && !STM32_PHYSR_ALTCONFIG
---help---
This must be provided if STM32_AUTONEG is defined. This provide bit mask
indicating full or half duplex modes.
for isolating the full or half duplex mode bits.
config STM32_PHYSR_FULLDUPLEX
hex "PHY full duplex mode value"
depends on STM32_AUTONEG
hex "PHY Full Duplex Mode Value"
depends on STM32_AUTONEG && !STM32_PHYSR_ALTCONFIG
---help---
This must be provided if STM32_AUTONEG is defined. This provides the
value of the mode bits indicating full duplex mode.
config STM32_PHYSR_ALTMODE
hex "PHY Mode Mask"
depends on STM32_AUTONEG && STM32_PHYSR_ALTCONFIG
---help---
This must be provided if STM32_AUTONEG is defined. This provide bit mask
for isolating the speed and full/half duplex mode bits.
config STM32_PHYSR_10HD
hex "10MHz/Half Duplex Value"
depends on STM32_AUTONEG && STM32_PHYSR_ALTCONFIG
---help---
This must be provided if STM32_AUTONEG is defined. This is the value
under the bit mask that represents the 10Mbps, half duplex setting.
config STM32_PHYSR_100HD
hex "100MHz/Half Duplex Value"
depends on STM32_AUTONEG && STM32_PHYSR_ALTCONFIG
---help---
This must be provided if STM32_AUTONEG is defined. This is the value
under the bit mask that represents the 100Mbps, half duplex setting.
config STM32_PHYSR_10FD
hex "10MHz/Full Duplex Value"
depends on STM32_AUTONEG && STM32_PHYSR_ALTCONFIG
---help---
This must be provided if STM32_AUTONEG is defined. This is the value
under the bit mask that represents the 10Mbps, full duplex setting.
config STM32_PHYSR_100FD
hex "100MHz/Full Duplex Value"
depends on STM32_AUTONEG && STM32_PHYSR_ALTCONFIG
---help---
This must be provided if STM32_AUTONEG is defined. This is the value
under the bit mask that represents the 100Mbps, full duplex setting.
config STM32_ETH_PTP
bool "Precision Time Protocol (PTP)"
default n
depends on STM32_ETHMAC
---help---
Precision Time Protocol (PTP). Not supported but some hooks are indicated
with this condition.
endmenu
config STM32_RMII
bool
default y if !STM32_MII
depends on STM32_ETHMAC
choice
prompt "RMII clock configuration"
@@ -1837,6 +1872,16 @@ config STM32_RMII_EXTCLK
endchoice
config STM32_ETHMAC_REGDEBUG
bool "Register-Level Debug"
default n
depends on DEBUG
---help---
Enable very low-level register access debug. Depends on DEBUG.
endmenu
endif
menu "USB Host Configuration"
config STM32_OTGFS_RXFIFO_SIZE
nuttx/arch/arm/src/stm32/chip/stm32_eth.h
+27 -7
View File
@@ -62,7 +62,9 @@
#define STM32_ETH_MACVLANTR_OFFSET 0x001c /* Ethernet MAC VLAN tag register */
#define STM32_ETH_MACRWUFFR_OFFSET 0x0028 /* Ethernet MAC remote wakeup frame filter reg */
#define STM32_ETH_MACPMTCSR_OFFSET 0x002c /* Ethernet MAC PMT control and status register */
#define STM32_ETH_MACDBGR_OFFSET 0x0034 /* Ethernet MAC debug register */
#if defined(CONFIG_STM32_STM32F20XX) || defined(CONFIG_STM32_STM32F40XX)
# define STM32_ETH_MACDBGR_OFFSET 0x0034 /* Ethernet MAC debug register */
#endif
#define STM32_ETH_MACSR_OFFSET 0x0038 /* Ethernet MAC interrupt status register */
#define STM32_ETH_MACIMR_OFFSET 0x003c /* Ethernet MAC interrupt mask register */
#define STM32_ETH_MACA0HR_OFFSET 0x0040 /* Ethernet MAC address 0 high register */
@@ -132,7 +134,9 @@
#define STM32_ETH_MACVLANTR (STM32_ETHERNET_BASE+STM32_ETH_MACVLANTR_OFFSET)
#define STM32_ETH_MACRWUFFR (STM32_ETHERNET_BASE+STM32_ETH_MACRWUFFR_OFFSET)
#define STM32_ETH_MACPMTCSR (STM32_ETHERNET_BASE+STM32_ETH_MACPMTCSR_OFFSET)
#define STM32_ETH_MACDBGR (STM32_ETHERNET_BASE+STM32_ETH_MACDBGR_OFFSET)
#if defined(CONFIG_STM32_STM32F20XX) || defined(CONFIG_STM32_STM32F40XX)
# define STM32_ETH_MACDBGR (STM32_ETHERNET_BASE+STM32_ETH_MACDBGR_OFFSET)
#endif
#define STM32_ETH_MACSR (STM32_ETHERNET_BASE+STM32_ETH_MACSR_OFFSET)
#define STM32_ETH_MACIMR (STM32_ETHERNET_BASE+STM32_ETH_MACIMR_OFFSET)
#define STM32_ETH_MACA0HR (STM32_ETHERNET_BASE+STM32_ETH_MACA0HR_OFFSET)
@@ -216,7 +220,9 @@
# define ETH_MACCR_IFG(n) ((12-((n) >> 3)) << ETH_MACCR_IFG_SHIFT) /* n bit times, n=40,48,..96 */
#define ETH_MACCR_JD (1 << 22) /* Bit 22: Jabber disable */
#define ETH_MACCR_WD (1 << 23) /* Bit 23: Watchdog disable */
#define ETH_MACCR_CSTF (1 << 25) /* Bits 25: CRC stripping for Type frames */
#if defined(CONFIG_STM32_STM32F20XX) || defined(CONFIG_STM32_STM32F40XX)
# define ETH_MACCR_CSTF (1 << 25) /* Bits 25: CRC stripping for Type frames */
#endif
/* Ethernet MAC frame filter register */
@@ -303,6 +309,8 @@
/* Ethernet MAC debug register */
#if defined(CONFIG_STM32_STM32F20XX) || defined(CONFIG_STM32_STM32F40XX)
#define ETH_MACDBGR_MMRPEA (1 << 0) /* Bit 0: MAC MII receive protocol engine active */
#define ETH_MACDBGR_MSFRWCS_SHIFT (1) /* Bits 1-2: MAC small FIFO read / write controllers status */
#define ETH_MACDBGR_MSFRWCS_MASK (3 << ETH_MACDBGR_MSFRWCS_SHIFT)
@@ -337,6 +345,8 @@
#define ETH_MACDBGR_TFNE (1 << 24) /* Bit 24: Tx FIFO not empty */
#define ETH_MACDBGR_TFF (1 << 25) /* Bit 25: Tx FIFO full */
#endif
/* Ethernet MAC interrupt status register */
#define ETH_MACSR_PMTS (1 << 3) /* Bit 3: PMT status */
@@ -419,7 +429,9 @@
#define ETH_MMCCR_ROR (1 << 2) /* Bit 2: Reset on read */
#define ETH_MMCCR_MCF (1 << 3) /* Bit 3: MMC counter freeze */
#define ETH_MMCCR_MCP (1 << 4) /* Bit 4: MMC counter preset */
#define ETH_MMCCR_MCFHP (1 << 5) /* Bit 5: MMC counter Full-Half preset */
#if defined(CONFIG_STM32_STM32F20XX) || defined(CONFIG_STM32_STM32F40XX)
# define ETH_MMCCR_MCFHP (1 << 5) /* Bit 5: MMC counter Full-Half preset */
#endif
/* Ethernet MMC receive interrupt and interrupt mask registers */
@@ -453,6 +465,8 @@
#define ETH_PTPTSCR_TSSTU (1 << 3) /* Bit 3: Time stamp system time update */
#define ETH_PTPTSCR_TSITE (1 << 4) /* Bit 4: Time stamp interrupt trigger enable */
#define ETH_PTPTSCR_TSARU (1 << 5) /* Bit 5: Time stamp addend register update */
#if defined(CONFIG_STM32_STM32F20XX) || defined(CONFIG_STM32_STM32F40XX)
#define ETH_PTPTSCR_TSSARFE (1 << 8) /* Bit 8: Time stamp snapshot for all received frames enable */
#define ETH_PTPTSCR_TSSSR (1 << 9) /* Bit 9: Time stamp subsecond rollover: digital or binary rollover control */
#define ETH_PTPTSCR_TSPTPPSV2E (1 << 10) /* Bit 10: Time stamp PTP packet snooping for version2 format enable */
@@ -468,6 +482,7 @@
# define ETH_PTPTSCR_TSCNT_E2E (2 << ETH_PTPTSCR_TSCNT_SHIFT) /* 10: End-to-end transparent clock */
# define ETH_PTPTSCR_TSCNT_P2P (3 << ETH_PTPTSCR_TSCNT_SHIFT) /* 11: Peer-to-peer transparent clock */
#define ETH_PTPTSCR_TSPFFMAE (1 << 18) /* Bit 18: Time stamp PTP frame filtering MAC address enable */
#endif
/* Ethernet PTP subsecond increment register */
@@ -543,7 +558,9 @@
#define ETH_DMABMR_USP (1 << 23) /* Bit 23: Use separate PBL */
#define ETH_DMABMR_FPM (1 << 24) /* Bit 24: 4xPBL mode */
#define ETH_DMABMR_AAB (1 << 25) /* Bit 25: Address-aligned beats */
#define ETH_DMABMR_MB (1 << 26) /* Bit 26: Mixed burst */
#if defined(CONFIG_STM32_STM32F20XX) || defined(CONFIG_STM32_STM32F40XX)
# define ETH_DMABMR_MB (1 << 26) /* Bit 26: Mixed burst */
#endif
/* Ethernet DMA transmit poll demand register (32-bit) */
/* Ethernet DMA receive poll demand register (32-bit) */
@@ -694,7 +711,9 @@
/* RDES0: Receive descriptor Word0 */
#define ETH_RDES0_PCE (1 << 0) /* Bit 0: Payload checksum error */
#define ETH_RDES0_ESA (1 << 0) /* Bit 0: Extended status available */
#if defined(CONFIG_STM32_STM32F20XX) || defined(CONFIG_STM32_STM32F40XX)
# define ETH_RDES0_ESA (1 << 0) /* Bit 0: Extended status available */
#endif
#define ETH_RDES0_CE (1 << 1) /* Bit 1: CRC error */
#define ETH_RDES0_DBE (1 << 2) /* Bit 2: Dribble bit error */
#define ETH_RDES0_RE (1 << 3) /* Bit 3: Receive error */
@@ -718,8 +737,9 @@
/* RDES1: Receive descriptor Word1 */
#define ETH_RDES1_RBS1_SHIFT (0) /* Bits 0-12: Receive buffer 1 size */
#define ETH_RDES1_RBS1_SHIFT (0) /* Bits 0-12: Receive buffer 1 size */
#define ETH_RDES1_RBS1_MASK (0x1fff << ETH_RDES1_RBS1_SHIFT)
/* Bit 13: Reserved */
#define ETH_RDES1_RCH (1 << 14) /* Bit 14: Second address chained */
#define ETH_RDES1_RER (1 << 15) /* Bit 15: Receive end of ring */
#define ETH_RDES1_RBS2_SHIFT (16) /* Bits 16-28: Receive buffer 2 size */
nuttx/arch/arm/src/stm32/chip/stm32f10xxx_gpio.h
+45 -35
View File
@@ -265,48 +265,58 @@
/* AF remap and debug I/O configuration register */
#define AFIO_MAPR_SWJ_CFG_SHIFT (24) /* Bits 26-24: Serial Wire JTAG configuration*/
#define AFIO_MAPR_SPI1_REMAP (1 << 0) /* Bit 0: SPI1 remapping */
#define AFIO_MAPR_I2C1_REMAP (1 << 1) /* Bit 1: I2C1 remapping */
#define AFIO_MAPR_USART1_REMAP (1 << 2) /* Bit 2: USART1 remapping */
#define AFIO_MAPR_USART2_REMAP (1 << 3) /* Bit 3: USART2 remapping */
#define AFIO_MAPR_USART3_REMAP_SHIFT (4) /* Bits 5-4: USART3 remapping */
#define AFIO_MAPR_USART3_REMAP_MASK (3 << AFIO_MAPR_USART3_REMAP_SHIFT)
# define AFIO_MAPR_USART3_NOREMAP (0 << AFIO_MAPR_USART3_REMAP_SHIFT) /* 00: No remap */
# define AFIO_MAPR_USART3_PARTREMAP (1 << AFIO_MAPR_USART3_REMAP_SHIFT) /* 01: Partial remap */
# define AFIO_MAPR_USART3_FULLREMAP (3 << AFIO_MAPR_USART3_REMAP_SHIFT) /* 11: Full remap */
#define AFIO_MAPR_TIM1_REMAP_SHIFT (6) /* Bits 7-6: TIM1 remapping */
#define AFIO_MAPR_TIM1_REMAP_MASK (3 << AFIO_MAPR_TIM1_REMAP_SHIFT)
# define AFIO_MAPR_TIM1_NOREMAP (0 << AFIO_MAPR_TIM1_REMAP_SHIFT) /* 00: No remap */
# define AFIO_MAPR_TIM1_PARTREMAP (1 << AFIO_MAPR_TIM1_REMAP_SHIFT) /* 01: Partial remap */
# define AFIO_MAPR_TIM1_FULLREMAP (3 << AFIO_MAPR_TIM1_REMAP_SHIFT) /* 11: Full remap */
#define AFIO_MAPR_TIM2_REMAP_SHIFT (8) /* Bits 9-8: TIM2 remapping */
#define AFIO_MAPR_TIM2_REMAP_MASK (3 << AFIO_MAPR_TIM2_REMAP_SHIFT)
# define AFIO_MAPR_TIM2_NOREMAP (0 << AFIO_MAPR_TIM2_REMAP_SHIFT) /* 00: No remap */
# define AFIO_MAPR_TIM2_PARTREMAP1 (1 << AFIO_MAPR_TIM2_REMAP_SHIFT) /* 01: Partial remap */
# define AFIO_MAPR_TIM2_PARTREMAP2 (2 << AFIO_MAPR_TIM2_REMAP_SHIFT) /* 10: Partial remap */
# define AFIO_MAPR_TIM2_FULLREMAP (3 << AFIO_MAPR_TIM2_REMAP_SHIFT) /* 11: Full remap */
#define AFIO_MAPR_TIM3_REMAP_SHIFT (10) /* Bits 11-10: TIM3 remapping */
#define AFIO_MAPR_TIM3_REMAP_MASK (3 << AFIO_MAPR_TIM3_REMAP_SHIFT)
# define AFIO_MAPR_TIM3_NOREMAP (0 << AFIO_MAPR_TIM3_REMAP_SHIFT) /* 00: No remap */
# define AFIO_MAPR_TIM3_PARTREMAP (2 << AFIO_MAPR_TIM3_REMAP_SHIFT) /* 10: Partial remap */
# define AFIO_MAPR_TIM3_FULLREMAP (3 << AFIO_MAPR_TIM3_REMAP_SHIFT) /* 11: Full remap */
#define AFIO_MAPR_TIM4_REMAP (1 << 12) /* Bit 12: TIM4 remapping */
#define AFIO_MAPR_CAN1_REMAP_SHIFT (13) /* Bits 14-13: CAN Alternate function remapping */
#define AFIO_MAPR_CAN1_REMAP_MASK (3 << AFIO_MAPR_CAN1_REMAP_SHIFT)
# define AFIO_MAPR_PA1112 (0 << AFIO_MAPR_CAN1_REMAP_SHIFT) /* 00: CANRX mapped to PA11, CANTX mapped to PA12 */
# define AFIO_MAPR_PB89 (2 << AFIO_MAPR_CAN1_REMAP_SHIFT) /* 10: CANRX mapped to PB8, CANTX mapped to PB9 */
# define AFIO_MAPR_PD01 (3 << AFIO_MAPR_CAN1_REMAP_SHIFT) /* 11: CANRX mapped to PD0, CANTX mapped to PD1 */
#define AFIO_MAPR_PD01_REMAP (1 << 15) /* Bit 15 : Port D0/Port D1 mapping on OSC_IN/OSC_OUT */
#define AFIO_MAPR_TIM5CH4_IREMAP (1 << 16) /* Bit 16: TIM5 channel4 internal remap */
/* Bits 17-20: Reserved */
#ifdef CONFIG_STM32_CONNECTIVITYLINE
# define AFIO_MAPR_ETH_REMAP (1 << 21) /* Bit 21: Ethernet MAC I/O remapping */
# define AFIO_MAPR_CAN2_REMAP (1 << 22) /* Bit 22: CAN2 I/O remapping */
# define AFIO_MAPR_MII_RMII_SEL (1 << 23) /* Bit 23: MII or RMII selection */
#endif
#define AFIO_MAPR_SWJ_CFG_SHIFT (24) /* Bits 26-24: Serial Wire JTAG configuration */
#define AFIO_MAPR_SWJ_CFG_MASK (7 << AFIO_MAPR_SWJ_CFG_SHIFT)
# define AFIO_MAPR_SWJRST (0 << AFIO_MAPR_SWJ_CFG_SHIFT) /* 000: Full SWJ (JTAG-DP + SW-DP): Reset State */
# define AFIO_MAPR_SWJ (1 << AFIO_MAPR_SWJ_CFG_SHIFT) /* 001: Full SWJ (JTAG-DP + SW-DP) but without JNTRST */
# define AFIO_MAPR_SWDP (2 << AFIO_MAPR_SWJ_CFG_SHIFT) /* 010: JTAG-DP Disabled and SW-DP Enabled */
# define AFIO_MAPR_DISAB (4 << AFIO_MAPR_SWJ_CFG_SHIFT) /* 100: JTAG-DP Disabled and SW-DP Disabled */
/* Bit 27: Reserved */
#ifdef CONFIG_STM32_CONNECTIVITYLINE
# define AFIO_MAPR_MII_RMII_SEL (1 << 23) /* MII or RMII selection */
# define AFIO_MAPR_SPI3_REMAP (1 << 28) /* Bit 28: SPI3 remapping */
# define AFIO_MAPR_TIM2ITR1_IREMAP (1 << 29) /* Bit 29: TIM2 internal trigger 1 remapping */
# define AFIO_MAPR_PTP_PPS_REMAP (1 << 30) /* Bit 30: Ethernet PTP PPS remapping */
#endif
#define AFIO_MAPR_PD01_REMAP (1 << 15) /* Bit 15 : Port D0/Port D1 mapping on OSC_IN/OSC_OUT */
#define AFIO_MAPR_CAN_REMAP_SHIFT (13) /* Bits 14-13: CAN Alternate function remapping */
#define AFIO_MAPR_CAN_REMAP_MASK (3 << AFIO_MAPR_CAN_REMAP_SHIFT)
# define AFIO_MAPR_PA1112 (0 << AFIO_MAPR_CAN_REMAP_SHIFT) /* 00: CANRX mapped to PA11, CANTX mapped to PA12 */
# define AFIO_MAPR_PB89 (2 << AFIO_MAPR_CAN_REMAP_SHIFT) /* 10: CANRX mapped to PB8, CANTX mapped to PB9 */
# define AFIO_MAPR_PD01 (3 << AFIO_MAPR_CAN_REMAP_SHIFT) /* 11: CANRX mapped to PD0, CANTX mapped to PD1 */
#define AFIO_MAPR_TIM4_REMAP (1 << 12) /* Bit 12: TIM4 remapping */
#define AFIO_MAPR_TIM3_REMAP_SHIFT (10) /* Bits 11-10: TIM3 remapping */
#define AFIO_MAPR_TIM3_REMAP_MASK (3 << AFIO_MAPR_TIM3_REMAP_SHIFT)
# define AFIO_MAPR_TIM3_NOREMAP (0 << AFIO_MAPR_TIM3_REMAP_SHIFT) /* 00: No remap (CH1/PA6, CH2/PA7, CH3/PB0, CH4/PB1) */
# define AFIO_MAPR_TIM3_PARTREMAP (2 << AFIO_MAPR_TIM3_REMAP_SHIFT) /* 10: Partial remap (CH1/PB4, CH2/PB5, CH3/PB0, CH4/PB1) */
# define AFIO_MAPR_TIM3_FULLREMAP (3 << AFIO_MAPR_TIM3_REMAP_SHIFT) /* 11: Full remap (CH1/PC6, CH2/PC7, CH3/PC8, CH4/PC9) */
#define AFIO_MAPR_TIM2_REMAP_SHIFT (8) /* Bits 9-8: TIM2 remapping */
#define AFIO_MAPR_TIM2_REMAP_MASK (3 << AFIO_MAPR_TIM2_REMAP_SHIFT)
# define AFIO_MAPR_TIM2_NOREMAP (0 << AFIO_MAPR_TIM2_REMAP_SHIFT) /* 00: No remap (CH1/ETR/PA0, CH2/PA1, CH3/PA2, CH4/PA3) */
# define AFIO_MAPR_TIM2_PARTREMAP1 (1 << AFIO_MAPR_TIM2_REMAP_SHIFT) /* 01: Partial remap (CH1/ETR/PA15, CH2/PB3, CH3/PA2, CH4/PA3) */
# define AFIO_MAPR_TIM2_PARTREMAP2 (2 << AFIO_MAPR_TIM2_REMAP_SHIFT) /* 10: Partial remap (CH1/ETR/PA0, CH2/PA1, CH3/PB10, CH4/PB11) */
# define AFIO_MAPR_TIM2_FULLREMAP (3 << AFIO_MAPR_TIM2_REMAP_SHIFT) /* 11: Full remap (CH1/ETR/PA15, CH2/PB3, CH3/PB10, CH4/PB11) */
#define AFIO_MAPR_TIM1_REMAP_SHIFT (6) /* Bits 7-6: TIM1 remapping */
#define AFIO_MAPR_TIM1_REMAP_MASK (3 << AFIO_MAPR_TIM1_REMAP_SHIFT)
# define AFIO_MAPR_TIM1_NOREMAP (0 << AFIO_MAPR_TIM1_REMAP_SHIFT) /* 00: No remap (ETR/PA12, CH1/PA8, CH2/PA9, CH3/PA10, CH4/PA11, BKIN/PB12, CH1N/PB13, CH2N/PB14, CH3N/PB15) */
# define AFIO_MAPR_TIM1_PARTREMAP (1 << AFIO_MAPR_TIM1_REMAP_SHIFT) /* 01: Partial remap (ETR/PA12, CH1/PA8, CH2/PA9, CH3/PA10, CH4/PA11, BKIN/PA6, CH1N/PA7, CH2N/PB0, CH3N/PB1) */
# define AFIO_MAPR_TIM1_FULLREMAP (3 << AFIO_MAPR_TIM1_REMAP_SHIFT) /* 11: Full remap (ETR/PE7, CH1/PE9, CH2/PE11, CH3/PE13, CH4/PE14, BKIN/PE15, CH1N/PE8, CH2N/PE10, CH3N/PE12) */
#define AFIO_MAPR_USART3_REMAP_SHIFT (6) /* Bits 5-4: USART3 remapping */
#define AFIO_MAPR_USART3_REMAP_MASK (3 << AFIO_MAPR_USART3_REMAP_SHIFT)
# define AFIO_MAPR_USART3_NOREMAP (0 << AFIO_MAPR_USART3_REMAP_SHIFT) /* 00: No remap (TX/PB10, RX/PB11, CK/PB12, CTS/PB13, RTS/PB14) */
# define AFIO_MAPR_USART3_PARTREMAP (1 << AFIO_MAPR_USART3_REMAP_SHIFT) /* 01: Partial remap (TX/PC10, RX/PC11, CK/PC12, CTS/PB13, RTS/PB14) */
# define AFIO_MAPR_USART3_FULLREMAP (3 << AFIO_MAPR_USART3_REMAP_SHIFT) /* 11: Full remap (TX/PD8, RX/PD9, CK/PD10, CTS/PD11, RTS/PD12) */
#define AFIO_MAPR_USART2_REMAP (1 << 3) /* Bit 3: USART2 remapping */
#define AFIO_MAPR_USART1_REMAP (1 << 2) /* Bit 2: USART1 remapping */
#define AFIO_MAPR_I2C1_REMAP (1 << 1) /* Bit 1: I2C1 remapping */
#define AFIO_MAPR_SPI1_REMAP (1 << 0) /* Bit 0: SPI1 remapping */
/* Bit 31: Reserved */
/* External interrupt configuration register 1 */
#define AFIO_EXTICR_PORT_MASK (0x0f)
nuttx/arch/arm/src/stm32/stm32_eth.c
+107 -27
View File
@@ -135,17 +135,35 @@
# ifndef CONFIG_STM32_PHYSR
# error "CONFIG_STM32_PHYSR must be defined in the NuttX configuration"
# endif
# ifndef CONFIG_STM32_PHYSR_SPEED
# error "CONFIG_STM32_PHYSR_SPEED must be defined in the NuttX configuration"
# endif
# ifndef CONFIG_STM32_PHYSR_100MBPS
# error "CONFIG_STM32_PHYSR_100MBPS must be defined in the NuttX configuration"
# endif
# ifndef CONFIG_STM32_PHYSR_MODE
# error "CONFIG_STM32_PHYSR_MODE must be defined in the NuttX configuration"
# endif
# ifndef CONFIG_STM32_PHYSR_FULLDUPLEX
# error "CONFIG_STM32_PHYSR_FULLDUPLEX must be defined in the NuttX configuration"
# ifdef CONFIG_STM32_PHYSR_ALTCONFIG
# ifndef CONFIG_STM32_PHYSR_ALTMODE
# error "CONFIG_STM32_PHYSR_ALTMODE must be defined in the NuttX configuration"
# endif
# ifndef CONFIG_STM32_PHYSR_10HD
# error "CONFIG_STM32_PHYSR_10HD must be defined in the NuttX configuration"
# endif
# ifndef CONFIG_STM32_PHYSR_100HD
# error "CONFIG_STM32_PHYSR_100HD must be defined in the NuttX configuration"
# endif
# ifndef CONFIG_STM32_PHYSR_10FD
# error "CONFIG_STM32_PHYSR_10FD must be defined in the NuttX configuration"
# endif
# ifndef CONFIG_STM32_PHYSR_100FD
# error "CONFIG_STM32_PHYSR_100FD must be defined in the NuttX configuration"
# endif
# else
# ifndef CONFIG_STM32_PHYSR_SPEED
# error "CONFIG_STM32_PHYSR_SPEED must be defined in the NuttX configuration"
# endif
# ifndef CONFIG_STM32_PHYSR_100MBPS
# error "CONFIG_STM32_PHYSR_100MBPS must be defined in the NuttX configuration"
# endif
# ifndef CONFIG_STM32_PHYSR_MODE
# error "CONFIG_STM32_PHYSR_MODE must be defined in the NuttX configuration"
# endif
# ifndef CONFIG_STM32_PHYSR_FULLDUPLEX
# error "CONFIG_STM32_PHYSR_FULLDUPLEX must be defined in the NuttX configuration"
# endif
# endif
#endif
@@ -168,10 +186,12 @@
#endif
/* Add 4 to the configured buffer size to account for the 2 byte checksum
* memory needed at the end of the maximum size packet.
* memory needed at the end of the maximum size packet. Buffer sizes must
* be an even multiple of 4, 8, or 16 bytes (depending on buswidth). We
* will use the 16-byte alignment in all cases.
*/
#define OPTIMAL_ETH_BUFSIZE (CONFIG_NET_BUFSIZE+4)
#define OPTIMAL_ETH_BUFSIZE ((CONFIG_NET_BUFSIZE + 4 + 15) & ~15)
#ifndef CONFIG_STM32_ETH_BUFSIZE
# define CONFIG_STM32_ETH_BUFSIZE OPTIMAL_ETH_BUFSIZE
@@ -181,6 +201,10 @@
# error "CONFIG_STM32_ETH_BUFSIZE is too large"
#endif
#if (CONFIG_STM32_ETH_BUFSIZE & 15) != 0
# error "CONFIG_STM32_ETH_BUFSIZE must be aligned"
#endif
#if CONFIG_STM32_ETH_BUFSIZE != OPTIMAL_ETH_BUFSIZE
# warning "You using an incomplete/untested configuration"
#endif
@@ -265,14 +289,22 @@
* ETH_MACCR_IFG Bits 17-19: Interframe gap
* ETH_MACCR_JD Bit 22: Jabber disable
* ETH_MACCR_WD Bit 23: Watchdog disable
* ETH_MACCR_CSTF Bits 25: CRC stripping for Type frames
* ETH_MACCR_CSTF Bits 25: CRC stripping for Type frames (F2/F4 only)
*/
#if defined(CONFIG_STM32_STM32F20XX) || defined(CONFIG_STM32_STM32F40XX)
#define MACCR_CLEAR_BITS \
( ETH_MACCR_RE | ETH_MACCR_TE | ETH_MACCR_DC | ETH_MACCR_BL_MASK | \
(ETH_MACCR_RE | ETH_MACCR_TE | ETH_MACCR_DC | ETH_MACCR_BL_MASK | \
ETH_MACCR_APCS | ETH_MACCR_RD | ETH_MACCR_IPCO | ETH_MACCR_DM | \
ETH_MACCR_LM | ETH_MACCR_ROD | ETH_MACCR_FES | ETH_MACCR_CSD | \
ETH_MACCR_IFG_MASK | ETH_MACCR_JD | ETH_MACCR_WD | ETH_MACCR_CSTF )
ETH_MACCR_IFG_MASK | ETH_MACCR_JD | ETH_MACCR_WD | ETH_MACCR_CSTF)
#else
#define MACCR_CLEAR_BITS \
(ETH_MACCR_RE | ETH_MACCR_TE | ETH_MACCR_DC | ETH_MACCR_BL_MASK | \
ETH_MACCR_APCS | ETH_MACCR_RD | ETH_MACCR_IPCO | ETH_MACCR_DM | \
ETH_MACCR_LM | ETH_MACCR_ROD | ETH_MACCR_FES | ETH_MACCR_CSD | \
ETH_MACCR_IFG_MASK | ETH_MACCR_JD | ETH_MACCR_WD)
#endif
/* The following bits are set or left zero unconditionally in all modes.
*
@@ -289,7 +321,7 @@
* ETH_MACCR_IFG Interframe gap 0 (96 bits)
* ETH_MACCR_JD Jabber disable 0 (enabled)
* ETH_MACCR_WD Watchdog disable 0 (enabled)
* ETH_MACCR_CSTF CRC stripping for Type frames 0 (disabled)
* ETH_MACCR_CSTF CRC stripping for Type frames 0 (disabled, F2/F4 only)
*
* The following are set conditioinally based on mode and speed.
*
@@ -444,13 +476,20 @@
* ETH_DMABMR_USP Bit 23: Use separate PBL
* ETH_DMABMR_FPM Bit 24: 4xPBL mode
* ETH_DMABMR_AAB Bit 25: Address-aligned beats
* ETH_DMABMR_MB Bit 26: Mixed burst
* ETH_DMABMR_MB Bit 26: Mixed burst (F2/F4 only)
*/
#if defined(CONFIG_STM32_STM32F20XX) || defined(CONFIG_STM32_STM32F40XX)
#define DMABMR_CLEAR_MASK \
(ETH_DMABMR_SR | ETH_DMABMR_DA | ETH_DMABMR_DSL_MASK | ETH_DMABMR_EDFE | \
ETH_DMABMR_PBL_MASK | ETH_DMABMR_RTPR_MASK | ETH_DMABMR_FB | ETH_DMABMR_RDP_MASK | \
ETH_DMABMR_USP | ETH_DMABMR_FPM | ETH_DMABMR_AAB | ETH_DMABMR_MB)
#else
#define DMABMR_CLEAR_MASK \
(ETH_DMABMR_SR | ETH_DMABMR_DA | ETH_DMABMR_DSL_MASK | ETH_DMABMR_EDFE | \
ETH_DMABMR_PBL_MASK | ETH_DMABMR_RTPR_MASK | ETH_DMABMR_FB | ETH_DMABMR_RDP_MASK | \
ETH_DMABMR_USP | ETH_DMABMR_FPM | ETH_DMABMR_AAB)
#endif
/* The following bits are set or left zero unconditionally in all modes.
*
@@ -466,7 +505,7 @@
* ETH_DMABMR_USP Use separate PBL 1 (enabled)
* ETH_DMABMR_FPM 4xPBL mode 0 (disabled)
* ETH_DMABMR_AAB Address-aligned beats 1 (enabled)
* ETH_DMABMR_MB Mixed burst 0 (disabled)
* ETH_DMABMR_MB Mixed burst 0 (disabled, F2/F4 only)
*/
#ifdef CONFIG_STM32_ETH_ENHANCEDDESC
@@ -1437,9 +1476,6 @@ static int stm32_recvframe(FAR struct stm32_ethmac_s *priv)
{
priv->segments++;
nllvdbg("rxhead: %p rxcurr: %p segments: %d\n",
priv->rxhead, priv->rxcurr, priv->segments);
/* Check if the there is only one segment in the frame */
if (priv->segments == 1)
@@ -1451,6 +1487,9 @@ static int stm32_recvframe(FAR struct stm32_ethmac_s *priv)
rxcurr = priv->rxcurr;
}
nllvdbg("rxhead: %p rxcurr: %p segments: %d\n",
priv->rxhead, priv->rxcurr, priv->segments);
/* Check if any errors are reported in the frame */
if ((rxdesc->rdes0 & ETH_RDES0_ES) == 0)
@@ -1983,7 +2022,7 @@ static int stm32_ifup(struct uip_driver_s *dev)
ndbg("Bringing up: %d.%d.%d.%d\n",
dev->d_ipaddr & 0xff, (dev->d_ipaddr >> 8) & 0xff,
(dev->d_ipaddr >> 16) & 0xff, dev->d_ipaddr >> 24 );
(dev->d_ipaddr >> 16) & 0xff, dev->d_ipaddr >> 24);
/* Configure the Ethernet interface for DMA operation. */
@@ -2224,7 +2263,7 @@ static void stm32_txdescinit(FAR struct stm32_ethmac_s *priv)
/* Initialize the next descriptor with the Next Descriptor Polling Enable */
if( i < (CONFIG_STM32_ETH_NTXDESC-1))
if (i < (CONFIG_STM32_ETH_NTXDESC-1))
{
/* Set next descriptor address register with next descriptor base
* address
@@ -2303,7 +2342,7 @@ static void stm32_rxdescinit(FAR struct stm32_ethmac_s *priv)
/* Initialize the next descriptor with the Next Descriptor Polling Enable */
if( i < (CONFIG_STM32_ETH_NRXDESC-1))
if (i < (CONFIG_STM32_ETH_NRXDESC-1))
{
/* Set next descriptor address register with next descriptor base
* address
@@ -2506,7 +2545,7 @@ static int stm32_phyinit(FAR struct stm32_ethmac_s *priv)
if (timeout >= PHY_RETRY_TIMEOUT)
{
ndbg("Timed out waiting for link status\n");
ndbg("Timed out waiting for link status: %04x\n", phyval);
return -ETIMEDOUT;
}
@@ -2552,6 +2591,46 @@ static int stm32_phyinit(FAR struct stm32_ethmac_s *priv)
/* Remember the selected speed and duplex modes */
nvdbg("PHYSR[%d]: %04x\n", CONFIG_STM32_PHYSR, phyval);
/* Different PHYs present speed and mode information in different ways. IF
* This CONFIG_STM32_PHYSR_ALTCONFIG is selected, this indicates that the PHY
* represents speed and mode information are combined, for example, with
* separate bits for 10HD, 100HD, 10FD and 100FD.
*/
#ifdef CONFIG_STM32_PHYSR_ALTCONFIG
switch (phyval & CONFIG_STM32_PHYSR_ALTMODE)
{
default:
case CONFIG_STM32_PHYSR_10HD:
priv->fduplex = 0;
priv->mbps100 = 0;
break;
case CONFIG_STM32_PHYSR_100HD:
priv->fduplex = 0;
priv->mbps100 = 1;
break;
case CONFIG_STM32_PHYSR_10FD:
priv->fduplex = 1;
priv->mbps100 = 0;
break;
case CONFIG_STM32_PHYSR_100FD:
priv->fduplex = 1;
priv->mbps100 = 1;
break;
}
/* Different PHYs present speed and mode information in different ways. Some
* will present separate information for speed and mode (this is the default).
* Those PHYs, for example, may provide a 10/100 Mbps indication and a separate
* full/half duplex indication.
*/
#else
if ((phyval & CONFIG_STM32_PHYSR_MODE) == CONFIG_STM32_PHYSR_FULLDUPLEX)
{
priv->fduplex = 1;
@@ -2561,6 +2640,7 @@ static int stm32_phyinit(FAR struct stm32_ethmac_s *priv)
{
priv->mbps100 = 1;
}
#endif
#else /* Auto-negotion not selected */
@@ -2854,7 +2934,7 @@ static void stm32_ethreset(FAR struct stm32_ethmac_s *priv)
* reset all the registers holds their reset values.
*/
regval = stm32_getreg(STM32_ETH_DMABMR);
regval = stm32_getreg(STM32_ETH_DMABMR);
regval |= ETH_DMABMR_SR;
stm32_putreg(regval, STM32_ETH_DMABMR);
nuttx/arch/arm/src/stm32/stm32_gpio.c
+38 -31
View File
@@ -124,14 +124,27 @@ static inline void stm32_gpioremap(void)
uint32_t val = 0;
#ifdef CONFIG_STM32_JTAG_FULL_ENABLE
/* The reset default */
#elif CONFIG_STM32_JTAG_NOJNTRST_ENABLE
val |= AFIO_MAPR_SWJ; /* enabled but without JNTRST */
#elif CONFIG_STM32_JTAG_SW_ENABLE
val |= AFIO_MAPR_SWDP; /* set JTAG-DP disabled and SW-DP enabled */
#else
val |= AFIO_MAPR_DISAB; /* set JTAG-DP and SW-DP Disabled */
#ifdef CONFIG_STM32_SPI1_REMAP
val |= AFIO_MAPR_SPI1_REMAP;
#endif
#ifdef CONFIG_STM32_SPI3_REMAP
#endif
#ifdef CONFIG_STM32_I2C1_REMAP
val |= AFIO_MAPR_I2C1_REMAP;
#endif
#ifdef CONFIG_STM32_USART1_REMAP
val |= AFIO_MAPR_USART1_REMAP;
#endif
#ifdef CONFIG_STM32_USART2_REMAP
val |= AFIO_MAPR_USART2_REMAP;
#endif
#ifdef CONFIG_STM32_USART3_FULL_REMAP
val |= AFIO_MAPR_USART3_FULLREMAP;
#endif
#ifdef CONFIG_STM32_USART3_PARTIAL_REMAP
val |= AFIO_MAPR_USART3_PARTREMAP;
#endif
#ifdef CONFIG_STM32_TIM1_FULL_REMAP
@@ -159,35 +172,29 @@ static inline void stm32_gpioremap(void)
val |= AFIO_MAPR_TIM4_REMAP;
#endif
#ifdef CONFIG_STM32_USART1_REMAP
val |= AFIO_MAPR_USART1_REMAP;
#endif
#ifdef CONFIG_STM32_USART2_REMAP
val |= AFIO_MAPR_USART2_REMAP;
#endif
#ifdef CONFIG_STM32_USART3_FULL_REMAP
val |= AFIO_MAPR_USART3_FULLREMAP;
#endif
#ifdef CONFIG_STM32_USART3_PARTIAL_REMAP
val |= AFIO_MAPR_USART3_PARTREMAP;
#endif
#ifdef CONFIG_STM32_SPI1_REMAP
val |= AFIO_MAPR_SPI1_REMAP;
#endif
#ifdef CONFIG_STM32_SPI3_REMAP
#endif
#ifdef CONFIG_STM32_I2C1_REMAP
val |= AFIO_MAPR_I2C1_REMAP;
#endif
#ifdef CONFIG_STM32_CAN1_REMAP1
val |= AFIO_MAPR_PB89;
#endif
#ifdef CONFIG_STM32_CAN1_REMAP2
val |= AFIO_MAPR_PD01;
#endif
#ifdef CONFIG_STM32_CAN2_REMAP /* Connectivity line only */
val |= AFIO_MAPR_CAN2_REMAP;
#endif
#ifdef CONFIG_STM32_ETH_REMAP /* Connectivity line only */
val |= AFIO_MAPR_ETH_REMAP;
#endif
#ifdef CONFIG_STM32_JTAG_FULL_ENABLE
/* The reset default */
#elif CONFIG_STM32_JTAG_NOJNTRST_ENABLE
val |= AFIO_MAPR_SWJ; /* enabled but without JNTRST */
#elif CONFIG_STM32_JTAG_SW_ENABLE
val |= AFIO_MAPR_SWDP; /* set JTAG-DP disabled and SW-DP enabled */
#else
val |= AFIO_MAPR_DISAB; /* set JTAG-DP and SW-DP Disabled */
#endif
putreg32(val, STM32_AFIO_MAPR);
#endif
nuttx/arch/arm/src/stm32/stm32_idle.c
+24 -1
View File
@@ -45,6 +45,7 @@
#include <arch/irq.h>
#include "chip.h"
#include "stm32_pm.h"
#include "up_internal.h"
@@ -178,11 +179,33 @@ void up_idle(void)
up_idlepm();
/* Sleep until an interrupt occurs to save power */
/* Sleep until an interrupt occurs to save power.
*
* NOTE: There is an STM32F107 errata that is fixed by the following
* workaround:
*
* "2.17.11 Ethernet DMA not working after WFI/WFE instruction
* Description
* If a WFI/WFE instruction is executed to put the system in sleep mode
* while the Ethernet MAC master clock on the AHB bus matrix is ON and all
* remaining masters clocks are OFF, the Ethernet DMA will be not able to
* perform any AHB master accesses during sleep mode."
*
* Workaround
* Enable DMA1 or DMA2 clocks in the RCC_AHBENR register before
* executing the WFI/WFE instruction."
*
* Here the workaround is just to avoid SLEEP mode for the connectivity
* line parts if Ethernet is enabled. The errate recommends a more
* general solution: Enabling DMA1/2 clocking in stm32f10xx_rcc.c if the
* STM32107 Ethernet peripheral is enabled.
*/
#if !defined(CONFIG_STM32_CONNECTIVITYLINE) || !defined(CONFIG_STM32_ETHMAC)
BEGIN_IDLE();
asm("WFI");
END_IDLE();
#endif
#endif
}
nuttx/arch/arm/src/stm32/stm32f10xxx_rcc.c
+121 -6
View File
@@ -90,7 +90,7 @@ static inline void rcc_reset(void)
regval = getreg32(STM32_RCC_CR); /* Reset HSEBYP bit */
regval &= ~RCC_CR_HSEBYP;
putreg32(regval, STM32_RCC_CR);
regval = getreg32(STM32_RCC_CFGR); /* Reset PLLSRC, PLLXTPRE, PLLMUL and USBPRE bits */
regval &= ~(RCC_CFGR_PLLSRC|RCC_CFGR_PLLXTPRE|RCC_CFGR_PLLMUL_MASK|RCC_CFGR_USBPRE);
putreg32(regval, STM32_RCC_CFGR);
@@ -235,7 +235,7 @@ static inline void rcc_enableapb1(void)
regval |= RCC_APB1ENR_SPI2EN;
#endif
#ifdef CONFIG_STM32_SPI3
/* SPI 3 clock enable */
@@ -411,13 +411,128 @@ static inline void rcc_enableapb2(void)
* Name: stm32_stdclockconfig
*
* Description:
* Called to change to new clock based on settings in board.h
*
* Called to change to new clock based on settings in board.h. This
* version is for the Connectivity Line parts.
*
* NOTE: This logic would need to be extended if you need to select low-
* power clocking modes!
****************************************************************************/
#ifndef CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG
#if !defined(CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG) && defined(CONFIG_STM32_CONNECTIVITYLINE)
static void stm32_stdclockconfig(void)
{
uint32_t regval;
/* Enable HSE */
regval = getreg32(STM32_RCC_CR);
regval &= ~RCC_CR_HSEBYP; /* Disable HSE clock bypass */
regval |= RCC_CR_HSEON; /* Enable HSE */
putreg32(regval, STM32_RCC_CR);
/* Set flash wait states
* Sysclk runs with 72MHz -> 2 waitstates.
* 0WS from 0-24MHz
* 1WS from 24-48MHz
* 2WS from 48-72MHz
*/
regval = getreg32(STM32_FLASH_ACR);
regval &= ~FLASH_ACR_LATENCY_MASK;
regval |= (FLASH_ACR_LATENCY_2|FLASH_ACR_PRTFBE);
putreg32(regval, STM32_FLASH_ACR);
/* Set up PLL input scaling (with source = PLL2) */
regval = getreg32(STM32_RCC_CFGR2);
regval &= ~(RCC_CFGR2_PREDIV2_MASK | RCC_CFGR2_PLL2MUL_MASK |
RCC_CFGR2_PREDIV1SRC_MASK | RCC_CFGR2_PREDIV1_MASK);
regval |= (STM32_PLL_PREDIV2 | STM32_PLL_PLL2MUL |
RCC_CFGR2_PREDIV1SRC_PLL2 | STM32_PLL_PREDIV1);
putreg32(regval, STM32_RCC_CFGR2);
/* Set the PCLK2 divider */
regval = getreg32(STM32_RCC_CFGR);
regval &= ~(RCC_CFGR_PPRE2_MASK | RCC_CFGR_HPRE_MASK);
regval |= STM32_RCC_CFGR_PPRE2;
regval |= RCC_CFGR_HPRE_SYSCLK;
putreg32(regval, STM32_RCC_CFGR);
/* Set the PCLK1 divider */
regval = getreg32(STM32_RCC_CFGR);
regval &= ~RCC_CFGR_PPRE1_MASK;
regval |= STM32_RCC_CFGR_PPRE1;
putreg32(regval, STM32_RCC_CFGR);
/* Enable PLL2 */
regval = getreg32(STM32_RCC_CR);
regval |= RCC_CR_PLL2ON;
putreg32(regval, STM32_RCC_CR);
/* Wait for PLL2 ready */
while((getreg32(STM32_RCC_CR) & RCC_CR_PLL2RDY) == 0);
/* Setup PLL3 for MII/RMII clock on MCO */
#if defined(CONFIG_STM32_MII_MCO) || defined(CONFIG_STM32_RMII_MCO)
regval = getreg32(STM32_RCC_CFGR2);
regval &= ~(RCC_CFGR2_PLL3MUL_MASK);
regval |= STM32_PLL_PLL3MUL;
putreg32(regval, STM32_RCC_CFGR2);
/* Switch PLL3 on */
regval = getreg32(STM32_RCC_CR);
regval |= RCC_CR_PLL3ON;
putreg32(regval, STM32_RCC_CR);
while ((getreg32(STM32_RCC_CR) & RCC_CR_PLL3RDY) == 0);
#endif
/* Set main PLL source and multiplier */
regval = getreg32(STM32_RCC_CFGR);
regval &= ~(RCC_CFGR_PLLSRC | RCC_CFGR_PLLMUL_MASK);
regval |= (RCC_CFGR_PLLSRC | STM32_PLL_PLLMUL);
putreg32(regval, STM32_RCC_CFGR);
/* Switch main PLL on */
regval = getreg32(STM32_RCC_CR);
regval |= RCC_CR_PLLON;
putreg32(regval, STM32_RCC_CR);
while ((getreg32(STM32_RCC_CR) & RCC_CR_PLLRDY) == 0);
/* Select PLL as system clock source */
regval = getreg32(STM32_RCC_CFGR);
regval &= ~RCC_CFGR_SW_MASK;
regval |= RCC_CFGR_SW_PLL;
putreg32(regval, STM32_RCC_CFGR);
/* Wait until PLL is used as the system clock source */
while ((getreg32(STM32_RCC_CFGR) & RCC_CFGR_SWS_PLL) == 0);
}
#endif
/****************************************************************************
* Name: stm32_stdclockconfig
*
* Description:
* Called to change to new clock based on settings in board.h. This
* version is for the non-Connectivity Line parts.
*
* NOTE: This logic would need to be extended if you need to select low-
* power clocking modes!
****************************************************************************/
#if !defined(CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG) && !defined(CONFIG_STM32_CONNECTIVITYLINE)
static void stm32_stdclockconfig(void)
{
uint32_t regval;
@@ -430,7 +545,7 @@ static void stm32_stdclockconfig(void)
volatile int32_t timeout;
/* Enable External High-Speed Clock (HSE) */
regval = getreg32(STM32_RCC_CR);
regval &= ~RCC_CR_HSEBYP; /* Disable HSE clock bypass */
regval |= RCC_CR_HSEON; /* Enable HSE */
nuttx/configs/README.txt
+2 -1
View File
@@ -1570,7 +1570,8 @@ configs/ez80f0910200zco
configs/fire-stm32v2
A configuration for the M3 Wildfire STM32 board. This board is based on the
STM32F103VET6 chip. See http://firestm32.taobao.com
STM32F103VET6 chip. See http://firestm32.taobao.com . Version 2 and 3 of
the boards are supported but only version 2 has been tested.
configs/hymini-stm32v
A configuration for the HY-Mini STM32v board. This board is based on the
nuttx/drivers/Kconfig
+1 -1
View File
@@ -151,7 +151,7 @@ config SPI_EXCHANGE
config SPI_CMDDATA
bool "SPI CMD/DATA"
default y
default n
---help---
Devices on the SPI bus require out-of-band support to distinguish command
transfers from data transfers. Such devices will often support either 9-bit
nuttx/drivers/input/Kconfig
+8 -1
View File
@@ -6,8 +6,15 @@ config INPUT_TSC2007
bool "TI TSC2007 touchscreen controller"
default n
select I2C
---help---
Enable support for the TI TSC2007 touchscreen controller
config INPUT_ADS7843E
bool "TI ADS7843E touchscreen controller"
bool "TI ADS7843/TSC2046 touchscreen controller"
default n
select SPI
---help---
Enable support for the TI/Burr-Brown ADS7842 touchscreen controller. I believe
that driver should be compatibile with the TI/Burr-Brown TSC2046 and XPT2046
touchscreen controllers as well.
nuttx/drivers/input/ads7843e.c
+6
View File
@@ -9,6 +9,12 @@
* "Touch Screen Controller, ADS7843," Burr-Brown Products from Texas
* Instruments, SBAS090B, September 2000, Revised May 2002"
*
* See also:
* "Low Voltage I/O Touch Screen Controller, TSC2046," Burr-Brown Products
* from Texas Instruments, SBAS265F, October 2002, Revised August 2007.
*
* "XPT2046 Data Sheet," Shenzhen XPTek Technology Co., Ltd, 2007
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
nuttx/drivers/input/ads7843e.h
+6
View File
@@ -8,6 +8,12 @@
* "Touch Screen Controller, ADS7843," Burr-Brown Products from Texas
* Instruments, SBAS090B, September 2000, Revised May 2002"
*
* See also:
* "Low Voltage I/O Touch Screen Controller, TSC2046," Burr-Brown Products
* from Texas Instruments, SBAS265F, October 2002, Revised August 2007."
*
* "XPT2046 Data Sheet," Shenzhen XPTek Technology Co., Ltd, 2007
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
nuttx/drivers/lcd/Kconfig
+43
View File
@@ -2,6 +2,21 @@
# For a description of the syntax of this configuration file,
# see misc/tools/kconfig-language.txt.
#
config LCD_NOGETRUN
bool "Write-only LCD"
default n
---help---
Many LCD hardware interfaces provide only minimal graphics capability. In
particulary, many simple LCD interfaces are write only. That is we, can
write graphics data to the LCD device memory, but we cannot read it back.
If the LCD hardware does not support reading the graphics memory, then
this option should be defined so that the NX layer can taking alternative
measures when the LCD is not readable. For example, if the LCD is not
readable, then NX will not attempt to support transparency.
See also NX_WRITEONLY in the graphics support menu.
config LCD_MAXCONTRAST
int "LCD maximum contrast"
default 63 if NOKIA6100_S1D15G10
@@ -28,6 +43,7 @@ config LCD_P14201
p14201.c. Driver for RiT P14201 series display with SD1329 IC
controller. This OLED is used with older versions of the
TI/Luminary LM3S8962 Evaluation Kit.
if LCD_P14201
config P14201_NINTERFACES
int "Number of physical P14201 devices"
@@ -174,6 +190,33 @@ config LCD_UG9664HSWAG01
Technology Inc. Used with the LPC Xpresso and Embedded Artists
base board.
config LCD_SSD1289
bool "LCD Based on SSD1289 Controller"
default n
---help---
Enables generic support for any LCD based on the Solomon Systech,
Ltd, SSD1289 Controller. Use of this driver will usually require so
detailed customization of the LCD initialization code as necessary
for the specific LCD driven by the SSD1289 controller.
if LCD_SSD1289
choice
prompt "SSD1289 Initialization Profile"
default SSD1289_PROFILE1
config SSD1289_PROFILE1
bool "Profile 1"
config SSD1289_PROFILE2
bool "Profile 2"
config SSD1289_PROFILE3
bool "Profile 3"
endchoice
endif
choice
prompt "LCD Orientation"
default LCD_LANDSCAPE
nuttx/drivers/lcd/ssd1289.c
+5 -3
View File
@@ -766,7 +766,7 @@ static int ssd1289_getvideoinfo(FAR struct lcd_dev_s *dev,
{
DEBUGASSERT(dev && vinfo);
lcdvdbg("fmt: %d xres: %d yres: %d nplanes: 1\n",
SSD1289_COLORFMT, SSD1289_XRES, SSD1289_XRES);
SSD1289_COLORFMT, SSD1289_XRES, SSD1289_YRES);
vinfo->fmt = SSD1289_COLORFMT; /* Color format: RGB16-565: RRRR RGGG GGGB BBBB */
vinfo->xres = SSD1289_XRES; /* Horizontal resolution in pixel columns */
@@ -925,6 +925,7 @@ static inline int ssd1289_hwinitialize(FAR struct ssd1289_dev_s *priv)
#ifndef CONFIG_LCD_NOGETRUN
uint16_t id;
#endif
int ret;
/* Select the LCD */
@@ -1168,19 +1169,20 @@ static inline int ssd1289_hwinitialize(FAR struct ssd1289_dev_s *priv)
/* One driver has a 50 msec delay here */
/* up_mdelay(50); */
return OK;
ret = OK;
}
#ifndef CONFIG_LCD_NOGETRUN
else
{
lcddbg("Unsupported LCD type\n");
return -ENODEV;
ret = -ENODEV;
}
#endif
/* De-select the LCD */
lcd->deselect(lcd);
return ret;
}
/*************************************************************************************
nuttx/drivers/net/enc28j60.h
+1 -1
View File
@@ -78,7 +78,7 @@
*
* The last five locations (0x1b to 0x1f) of all banks point to a common set
* of registers: EIE, EIR, ESTAT, ECON2 and ECON1. These are key registers
* usedin controlling and monitoring the operation of the device. Their
* used in controlling and monitoring the operation of the device. Their
* common mapping allows easy access without switching the bank.
*
* Control registers for the ENC28J60 are generically grouped as ETH, MAC and
nuttx/graphics/Kconfig
+433
View File
@@ -2,3 +2,436 @@
# For a description of the syntax of this configuration file,
# see misc/tools/kconfig-language.txt.
#
config NX
bool "NX Graphics"
default n
---help---
Enables overall support for graphics library and NX
if NX
config NX_LCDDRIVER
bool "LCD driver"
default y
depends on LCD
---help---
By default, the NX graphics system uses the frame buffer driver interface
defined in include/nuttx/fb.h. However, if LCD is support is enabled,
this this option is provide to select, instead, the LCD driver interface
defined in include/nuttx/lcd/lcd.h.
config NX_NPLANES
int "Number of Color Planes"
default 1
---help---
Some YUV color formats requires support for multiple planes, one for each
color component. Unless you have such special hardware, this value should be
undefined or set to 1.
config NX_WRITEONLY
bool "Write-only Graphics Device"
default y if NX_LCDDRIVER && LCD_NOGETRUN
default n if !NX_LCDDRIVER || !LCD_NOGETRUN
---help---
Define if the underlying graphics device does not support read operations.
Automatically defined if NX_LCDDRIVER and LCD_NOGETRUN are
defined.
menu "Supported Pixel Depths"
config NX_DISABLE_1BPP
bool "1 BPP"
default y
---help---
NX supports a variety of pixel depths. You can save some memory by disabling
support for unused color depths. The selection disables support for 1BPP
pixel depth.
config NX_DISABLE_2BPP
bool "2 BPP"
default y
---help---
NX supports a variety of pixel depths. You can save some memory by disabling
support for unused color depths. The selection disables support for 2BPP
pixel depth.
config NX_DISABLE_4BPP
bool "4 BPP"
default y
---help---
NX supports a variety of pixel depths. You can save some memory by disabling
support for unused color depths. The selection disables support for 4BPP
pixel depth.
config NX_DISABLE_8BPP
bool "8 BPP"
default y
---help---
NX supports a variety of pixel depths. You can save some memory by disabling
support for unused color depths. The selection disables support for 8BPP
pixel depth.
config NX_DISABLE_16BPP
bool "16 BPP"
default y
---help---
NX supports a variety of pixel depths. You can save some memory by disabling
support for unused color depths. The selection disables support for 16BPP
pixel depth.
config NX_DISABLE_24BPP
bool "24 BPP"
default y
---help---
NX supports a variety of pixel depths. You can save some memory by disabling
support for unused color depths. The selection disables support for 24BPP
pixel depth.
config NX_DISABLE_32BPP
bool "32 BPP"
default y
---help---
NX supports a variety of pixel depths. You can save some memory by disabling
support for unused color depths. The selection disables support for 32BPP
pixel depth.
endmenu
config NX_PACKEDMSFIRST
bool "Packed MS First"
default y
depends on NX_DISABLE_1BPP || NX_DISABLE_2BPP || NX_DISABLE_4BPP
---help---
If a pixel depth of less than 8-bits is used, then NX needs to know if the
pixels pack from the MS to LS or from LS to MS
menu "Input Devices"
config NX_MOUSE
bool "Mouse/Touchscreen Support"
default n
---help---
Build in support for mouse or touchscreeninput.
config NX_KBD
bool "Keyboard Support"
default n
---help---
Build in support of keypad/keyboard input.
endmenu
menu "Framed Window Borders"
config NXTK_BORDERWIDTH
int "Border Width"
default 4
---help---
Specifies with with of the border (in pixels) used with framed windows.
The default is 4.
config NXTK_BORDERCOLOR1
hex "Border Color"
default 0
---help---
Specify the colors of the border used with framed windows.
NXTL_BODERCOLOR is the "normal" color of the border.
NXTK_BORDERCOLOR2 is the shadow side color and so is normally darker.
NXTK_BORDERCOLOR3 is the shiny side color and so is normally brighter.
config NXTK_BORDERCOLOR2
hex "Darker Border Color"
default 0
---help---
Specify the colors of the border used with framed windows.
NXTL_BODERCOLOR is the "normal" color of the border.
NXTK_BORDERCOLOR2 is the shadow side color and so is normally darker.
NXTK_BORDERCOLOR3 is the shiny side color and so is normally brighter.
config NXTK_BORDERCOLOR3
hex "Brighter Border Color"
default 0
---help---
Specify the colors of the border used with framed windows.
NXTL_BODERCOLOR is the "normal" color of the border.
NXTK_BORDERCOLOR2 is the shadow side color and so is normally darker.
NXTK_BORDERCOLOR3 is the shiny side color and so is normally brighter.
endmenu
config NXTK_AUTORAISE
bool "Autoraise"
default n
---help---
If set, a window will be raised to the top if the mouse position is over a
visible portion of the window. Default: A mouse button must be clicked over
a visible portion of the window.
menu "Font Selections"
config NXFONTS_CHARBITS
int "Bits in Character Set"
default 7
range 7 8
---help---
The number of bits in the character set. Current options are only 7 and 8.
The default is 7.
config NXFONT_SANS17X22
bool "Sans 17x22"
default n
---help---
This option enables support for a tiny, 17x22 san serif font
(font ID FONTID_SANS17X22 == 14).
config NXFONT_SANS20X26
bool "Sans 20x26"
default n
---help---
This option enables support for a tiny, 20x26 san serif font
(font ID FONTID_SANS20X26 == 15).
config NXFONT_SANS23X27
bool "Sans 23x27"
default n
---help---
This option enables support for a tiny, 23x27 san serif font
(font ID FONTID_SANS23X27 == 1).
config NXFONT_SANS22X29
bool "Sans 22x29"
default n
---help---
This option enables support for a small, 22x29 san serif font
(font ID FONTID_SANS22X29 == 2).
config NXFONT_SANS28X37
bool "Sans 28x37"
default n
---help---
This option enables support for a medium, 28x37 san serif font
(font ID FONTID_SANS28X37 == 3).
config NXFONT_SANS39X48
bool "Sans 39x48"
default n
---help---
This option enables support for a large, 39x48 san serif font
(font ID FONTID_SANS39X48 == 4).
config NXFONT_SANS17X23B
bool "Sans 17x23 Bold"
default n
---help---
This option enables support for a tiny, 17x23 san serif bold font
(font ID FONTID_SANS17X23B == 16).
config NXFONT_SANS20X27B
bool "Sans 20x27 Bold"
default n
---help---
This option enables support for a tiny, 20x27 san serif bold font
(font ID FONTID_SANS20X27B == 17).
config NXFONT_SANS22X29B
bool "Sans 22x29 Bold"
default n
---help---
This option enables support for a small, 22x29 san serif bold font
(font ID FONTID_SANS22X29B == 5).
config NXFONT_SANS28X37B
bool "Sans 28x37 Bold"
default n
---help---
This option enables support for a medium, 28x37 san serif bold font
(font ID FONTID_SANS28X37B == 6).
config NXFONT_SANS40X49B
bool "Sans 40x49 Bold"
default n
---help---
This option enables support for a large, 40x49 san serif bold font
(font ID FONTID_SANS40X49B == 7).
config NXFONT_SERIF22X29
bool "Serif 22x29"
default n
---help---
This option enables support for a small, 22x29 font (with serifs)
(font ID FONTID_SERIF22X29 == 8).
config NXFONT_SERIF29X37
bool "Serif 29x37"
default n
---help---
This option enables support for a medium, 29x37 font (with serifs)
(font ID FONTID_SERIF29X37 == 9).
config NXFONT_SERIF38X48
bool "Serif 38x48"
default n
---help---
This option enables support for a large, 38x48 font (with serifs)
(font ID FONTID_SERIF38X48 == 10).
config NXFONT_SERIF22X28B
bool "Serif 22x28 Bold"
default n
---help---
This option enables support for a small, 27x38 bold font (with serifs)
(font ID FONTID_SERIF22X28B == 11).
config NXFONT_SERIF27X38B
bool "Serif 27x38 Bold"
default n
---help---
This option enables support for a medium, 27x38 bold font (with serifs)
(font ID FONTID_SERIF27X38B == 12).
config NXFONT_SERIF38X49B
bool "Serif 38x49 Bold"
default n
---help---
This option enables support for a large, 38x49 bold font (with serifs)
(font ID FONTID_SERIF38X49B == 13).
endmenu
menuconfig NXCONSOLE
bool "NxConsole"
default n
---help---
Enables building of the NxConsole driver.
if NXCONSOLE
comment "NxConsole Output Text/Graphics Options"
config NXCONSOLE_BPP
int "NxConsole BPP"
default 1 if !NX_DISABLE_1BPP
default 2 if !NX_DISABLE_2BPP
default 4 if !NX_DISABLE_4BPP
default 8 if !NX_DISABLE_8BPP
default 16 if !NX_DISABLE_16BPP
default 24 if !NX_DISABLE_24BPP
default 32 if !NX_DISABLE_32BPP
---help---
Currently, NxConsole supports only a single pixel depth. This
configuration setting must be provided to support that single pixel depth.
Default: The smallest enabled pixel depth. (see NX_DISABLE_*BPP)
config NXCONSOLE_CURSORCHAR
int "Character code to use as the cursor"
default 137
---help---
The bitmap code to use as the cursor. Default '_' (137)
config NXCONSOLE_MXCHARS
int "Max Characters on Display"
default 128
---help---
NxConsole needs to remember every character written to the console so
that it can redraw the window. This setting determines the size of some
internal memory allocations used to hold the character data. Default: 128.
config NXCONSOLE_CACHESIZE
int "Font Cache Size"
default 16
---help---
NxConsole supports caching of rendered fonts. This font caching is required
for two reasons: (1) First, it improves text performance, but more
importantly (2) it preserves the font memory. Since the NX server runs on
a separate server thread, it requires that the rendered font memory persist
until the server has a chance to render the font. Unfortunately, the font
cache would be quite large if all fonts were saved. The NXCONSOLE_CACHESIZE
setting will control the size of the font cache (in number of glyphs). Only that
number of the most recently used glyphs will be retained. Default: 16.
NOTE: There can still be a race condition between the NxConsole driver and the
NX task. If you every see character corruption (especially when printing
a lot of data or scrolling), then increasing the value of NXCONSOLE_CACHESIZE
is something that you should try. Alternatively, you can reduce the size of
MQ_MAXMSGSIZE which will force NxConsole task to pace the server task.
NXCONSOLE_CACHESIZE should be larger than MQ_MAXMSGSIZE in any event.
config NXCONSOLE_LINESEPARATION
int "Line Separation"
default 0
---help---
This the space (in rows) between each row of test. Default: 0
config NXCONSOLE_NOWRAP
bool "No wrap"
default n
---help---
By default, lines will wrap when the test reaches the right hand side
of the window. This setting can be defining to change this behavior so
that the text is simply truncated until a new line is encountered.
comment "NxConsole Input options"
config NXCONSOLE_NXKBDIN
bool "NX KBD input"
default n
---help---
Take input from the NX keyboard input callback. By default, keyboard
input is taken from stdin (/dev/console). If this option is set, then
the interface nxcon_kdbin() is enabled. That interface may be driven
by window callback functions so that keyboard input *only* goes to the
top window.
config NXCONSOLE_KBDBUFSIZE
int "Keyboard Input Buffer Size"
default 16
---help---
If NXCONSOLE_NXKBDIN is enabled, then this value may be used to
define the size of the per-window keyboard input buffer. Default: 16
config NXCONSOLE_NPOLLWAITERS
int "Number of Poll Waiters"
default 4
---help---
The number of threads that can be waiting for read data available.
Default: 4
endif
comment "NX Multi-user only options"
menuconfig NX_MULTIUSER
bool "Multi-user NX Server"
default n
---help---
Configures NX in multi-user mode
if NX_MULTIUSER
config NX_BLOCKING
bool "Blocking"
default n
---help---
Open the client message queues in blocking mode. In this case,
nx_eventhandler() will not return until a message is received and processed.
config NX_MXSERVERMSGS
int "Max Server Messages"
default 32
---help---
Specifies the maximum number of messages that can fit in the message queues.
No additional resources are allocated, but this can be set to prevent
flooding of the client or server with too many messages (PREALLOC_MQ_MSGS
controls how many messages are pre-allocated).
config NX_MXCLIENTMSGS
int "Max Client Messages"
default 16
---help---
Specifies the maximum number of messages that can fit in the message queues.
No additional resources are allocated, but this can be set to prevent
flooding of the client or server with too many messages (PREALLOC_MQ_MSGS
controls how many messages are pre-allocated).
endif
endif
nuttx/graphics/nxmu/nx_block.c
+2 -2
View File
@@ -111,8 +111,8 @@ int nx_block(NXWINDOW hwnd, FAR void *arg)
#ifdef CONFIG_DEBUG
if (!hwnd)
{
errno = EINVAL;
return NULL;
set_errno(EINVAL);
return ERROR;
}
#endif
nuttx/graphics/nxmu/nxmu_sendclient.c
+2 -2
View File
@@ -93,9 +93,9 @@ int nxmu_sendclient(FAR struct nxfe_conn_s *conn, FAR const void *msg,
/* Sanity checking */
#ifdef CONFIG_DEBUG
if (!conn || conn->swrmq)
if (!conn || !conn->swrmq)
{
errno = EINVAL;
set_errno(EINVAL);
return ERROR;
}
#endif
nuttx/graphics/nxmu/nxmu_sendserver.c
+2 -2
View File
@@ -93,9 +93,9 @@ int nxmu_sendserver(FAR struct nxfe_conn_s *conn, FAR const void *msg,
/* Sanity checking */
#ifdef CONFIG_DEBUG
if (!conn || conn->cwrmq)
if (!conn || !conn->cwrmq)
{
errno = EINVAL;
set_errno(EINVAL);
return ERROR;
}
#endif
nuttx/lib/stdio/lib_sscanf.c
+181 -64
View File
@@ -1,7 +1,7 @@
/****************************************************************************
* lib/stdio/lib_sscanf.c
*
* Copyright (C) 2007, 2008, 2011 Gregory Nutt. All rights reserved.
* Copyright (C) 2007, 2008, 2011-2012 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <gnutt@nuttx.org>
*
* Redistribution and use in source and binary forms, with or without
@@ -38,9 +38,11 @@
****************************************************************************/
#include <nuttx/compiler.h>
#include <sys/types.h>
#include <stdarg.h>
#include <stdlib.h>
#include <stdbool.h>
#include <string.h>
#include <ctype.h>
#include <debug.h>
@@ -63,7 +65,7 @@
* Global Function Prototypes
****************************************************************************/
int vsscanf(char *buf, const char *s, va_list ap);
int vsscanf(char *buf, const char *fmt, va_list ap);
/**************************************************************************
* Global Constant Data
@@ -79,6 +81,64 @@ int vsscanf(char *buf, const char *s, va_list ap);
static const char spaces[] = " \t\n\r\f\v";
/****************************************************************************
* Private Functions
****************************************************************************/
/****************************************************************************
* Function: findwidth
*
* Description:
* Try to figure out the width of the input data.
*
****************************************************************************/
static int findwidth(FAR const char *buf, FAR const char *fmt)
{
FAR const char *next = fmt + 1;
/* No... is there a space after the format? Or does the format string end
* here?
*/
if (isspace(*next) || *next == 0)
{
/* Use the input up until the first white space is encountered. */
return strcspn(buf, spaces);
}
/* No.. Another possibility is the the format character is followed by
* some recognizable delimiting value.
*/
if (*next != '%')
{
/* If so we will say that the string ends there if we can find that
* delimiter in the input string.
*/
FAR const char *ptr = strchr(buf, *next);
if (ptr)
{
return (int)(ptr - buf);
}
}
/* No... the format has not delimiter and is back-to-back with the next
* formats (or no is following by a delimiter that does not exist in the
* input string). At this point we just bail and Use the input up until
* the first white space is encountered.
*
* NOTE: This means that values from the following format may be
* concatenated with the first. This is a bug. We have no generic way of
* determining the width of the data if there is no fieldwith, no space
* separating the input, and no usable delimiter character.
*/
return strcspn(buf, spaces);
}
/****************************************************************************
* Private Variables
****************************************************************************/
@@ -109,67 +169,83 @@ int sscanf(FAR const char *buf, FAR const char *fmt, ...)
* ANSI standard vsscanf implementation.
*
****************************************************************************/
int vsscanf(FAR char *buf, FAR const char *s, va_list ap)
int vsscanf(FAR char *buf, FAR const char *fmt, va_list ap)
{
int count;
int noassign;
int width;
int base = 10;
int lflag;
FAR char *bufstart;
FAR char *tv;
FAR const char *tc;
bool lflag;
bool noassign;
int count;
int width;
int base = 10;
char tmp[MAXLN];
lvdbg("vsscanf: buf=\"%s\" fmt=\"%s\"\n", buf, s);
lvdbg("vsscanf: buf=\"%s\" fmt=\"%s\"\n", buf, fmt);
count = noassign = width = lflag = 0;
while (*s && *buf)
/* Remember the start of the input buffer. We will need this for %n
* calculations.
*/
bufstart = buf;
/* Parse the format, extracting values from the input buffer as needed */
count = 0;
width = 0;
noassign = false;
lflag = false;
while (*fmt && *buf)
{
/* Skip over white space */
while (isspace(*s))
while (isspace(*fmt))
{
s++;
fmt++;
}
/* Check for a conversion specifier */
if (*s == '%')
if (*fmt == '%')
{
lvdbg("vsscanf: Specifier found\n");
/* Check for qualifiers on the conversion specifier */
s++;
for (; *s; s++)
fmt++;
for (; *fmt; fmt++)
{
lvdbg("vsscanf: Processing %c\n", *s);
lvdbg("vsscanf: Processing %c\n", *fmt);
if (strchr("dibouxcsefg%", *s))
if (strchr("dibouxcsefgn%", *fmt))
{
break;
}
if (*s == '*')
if (*fmt == '*')
{
noassign = 1;
noassign = true;
}
else if (*s == 'l' || *s == 'L')
else if (*fmt == 'l' || *fmt == 'L')
{
lflag = 1;
/* NOTE: Missing check for long long ('ll') */
lflag = true;
}
else if (*s >= '1' && *s <= '9')
else if (*fmt >= '1' && *fmt <= '9')
{
for (tc = s; isdigit(*s); s++);
strncpy(tmp, tc, s - tc);
tmp[s - tc] = '\0';
for (tc = fmt; isdigit(*fmt); fmt++);
strncpy(tmp, tc, fmt - tc);
tmp[fmt - tc] = '\0';
width = atoi(tmp);
s--;
fmt--;
}
}
/* Process %s: String conversion */
if (*s == 's')
if (*fmt == 's')
{
lvdbg("vsscanf: Performing string conversion\n");
@@ -178,9 +254,13 @@ int vsscanf(FAR char *buf, FAR const char *s, va_list ap)
buf++;
}
/* Was a fieldwidth specified? */
if (!width)
{
width = strcspn(buf, spaces);
/* No... Guess a field width using some heuristics */
width = findwidth(buf, fmt);
}
if (!noassign)
@@ -189,17 +269,22 @@ int vsscanf(FAR char *buf, FAR const char *s, va_list ap)
strncpy(tv, buf, width);
tv[width] = '\0';
}
buf += width;
}
/* Process %c: Character conversion */
else if (*s == 'c')
else if (*fmt == 'c')
{
lvdbg("vsscanf: Performing character conversion\n");
/* Was a fieldwidth specified? */
if (!width)
{
/* No, then width is this one single character */
width = 1;
}
@@ -209,12 +294,13 @@ int vsscanf(FAR char *buf, FAR const char *s, va_list ap)
strncpy(tv, buf, width);
tv[width] = '\0';
}
buf += width;
}
/* Process %d, %o, %b, %x, %u: Various integer conversions */
else if (strchr("dobxu", *s))
else if (strchr("dobxu", *fmt))
{
lvdbg("vsscanf: Performing integer conversion\n");
@@ -229,37 +315,34 @@ int vsscanf(FAR char *buf, FAR const char *s, va_list ap)
* conversion specification.
*/
if (*s == 'd' || *s == 'u')
if (*fmt == 'd' || *fmt == 'u')
{
base = 10;
}
else if (*s == 'x')
else if (*fmt == 'x')
{
base = 16;
}
else if (*s == 'o')
else if (*fmt == 'o')
{
base = 8;
}
else if (*s == 'b')
else if (*fmt == 'b')
{
base = 2;
}
/* Copy the integer string into a temporary working buffer. */
/* Was a fieldwidth specified? */
if (!width)
{
if (isspace(*(s + 1)) || *(s + 1) == 0)
{
width = strcspn(buf, spaces);
}
else
{
width = strchr(buf, *(s + 1)) - buf;
}
/* No... Guess a field width using some heuristics */
width = findwidth(buf, fmt);
}
/* Copy the numeric string into a temporary working buffer. */
strncpy(tmp, buf, width);
tmp[width] = '\0';
@@ -270,21 +353,30 @@ int vsscanf(FAR char *buf, FAR const char *s, va_list ap)
buf += width;
if (!noassign)
{
int *pint = va_arg(ap, int*);
#ifdef SDCC
char *endptr;
int tmpint = strtol(tmp, &endptr, base);
long tmplong = strtol(tmp, &endptr, base);
#else
int tmpint = strtol(tmp, NULL, base);
long tmplong = strtol(tmp, NULL, base);
#endif
lvdbg("vsscanf: Return %d to 0x%p\n", tmpint, pint);
*pint = tmpint;
if (lflag)
{
long *plong = va_arg(ap, long*);
lvdbg("vsscanf: Return %ld to 0x%p\n", tmplong, plong);
*plong = tmplong;
}
else
{
int *pint = va_arg(ap, int*);
lvdbg("vsscanf: Return %ld to 0x%p\n", tmplong, pint);
*pint = (int)tmplong;
}
}
}
/* Process %f: Floating point conversion */
else if (*s == 'f')
else if (*fmt == 'f')
{
#ifndef CONFIG_LIBC_FLOATINGPOINT
/* No floating point conversions */
@@ -303,20 +395,17 @@ int vsscanf(FAR char *buf, FAR const char *s, va_list ap)
buf++;
}
/* Copy the real string into a temporary working buffer. */
/* Was a fieldwidth specified? */
if (!width)
{
if (isspace(*(s + 1)) || *(s + 1) == 0)
{
width = strcspn(buf, spaces);
}
else
{
width = strchr(buf, *(s + 1)) - buf;
}
/* No... Guess a field width using some heuristics */
width = findwidth(buf, fmt);
}
/* Copy the real string into a temporary working buffer. */
strncpy(tmp, buf, width);
tmp[width] = '\0';
buf += width;
@@ -356,13 +445,41 @@ int vsscanf(FAR char *buf, FAR const char *s, va_list ap)
#endif
}
if (!noassign)
/* Process %n: Character count */
else if (*fmt == 'n')
{
lvdbg("vsscanf: Performing character count\n");
if (!noassign)
{
size_t nchars = (size_t)(buf - bufstart);
if (lflag)
{
long *plong = va_arg(ap, long*);
*plong = (long)nchars;
}
else
{
int *pint = va_arg(ap, int*);
*pint = (int)nchars;
}
}
}
/* Note %n does not count as a conversion */
if (!noassign && *fmt != 'n')
{
count++;
}
width = noassign = lflag = 0;
s++;
width = 0;
noassign = false;
lflag = false;
fmt++;
}
/* Its is not a conversion specifier */
@@ -374,13 +491,13 @@ int vsscanf(FAR char *buf, FAR const char *s, va_list ap)
buf++;
}
if (*s != *buf)
if (*fmt != *buf)
{
break;
}
else
{
s++;
fmt++;
buf++;
}
}
nuttx/tools/cfgparser.c
+15 -4
View File
@@ -56,13 +56,24 @@ char line[LINESIZE+1];
****************************************************************************/
/* These are configuration variable name that are quoted by configuration tool
* but which must be unquoated when used in C code.
* but which must be unquoted when used in C code.
*/
static const char *dequote_list[] =
{
"CONFIG_USER_ENTRYPOINT",
NULL
/* NuttX */
"CONFIG_USER_ENTRYPOINT", /* Name of entry point function */
/* NxWidgets/NxWM */
"CONFIG_NXWM_BACKGROUND_IMAGE", /* Name of bitmap image class */
"CONFIG_NXWM_STARTWINDOW_ICON", /* Name of bitmap image class */
"CONFIG_NXWM_NXCONSOLE_ICON", /* Name of bitmap image class */
"CONFIG_NXWM_CALIBRATION_ICON", /* Name of bitmap image class */
"CONFIG_NXWM_HEXCALCULATOR_ICON", /* Name of bitmap image class */
NULL /* Marks the end of the list */
};
/****************************************************************************
@@ -239,7 +250,7 @@ static char *dequote_value(const char *varname, char *varval)
/* Handle the case where nothing is left after dequoting */
if (len < 0)
if (len <= 0)
{
dqval = NULL;
}