Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-04 21:23:57 +08:00
Code Issues Actions 4 Packages Projects Releases Wiki Activity

Merge branch 'master' into fw_control

Browse Source
This commit is contained in:
Thomas Gubler
2012-10-22 18:11:47 +02:00
parent ab447ac713 e6bab270c3
commit 836c55e122
101 changed files with 5447 additions and 2711 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.gitignore
+2 -1
View File
@@ -1,5 +1,5 @@
.built
.context
*.context
.depend
.config
.version
@@ -10,6 +10,7 @@ apps/namedapp/namedapp_proto.h
Make.dep
*.o
*.a
*~
Images/*.bin
Images/*.px4
nuttx/Make.defs
ROMFS/scripts/rcS
+5 -3
View File
@@ -20,6 +20,11 @@
set MODE autostart
set USB autoconnect
#
# Start playing the startup tune
#
tone_alarm start
#
# Try to mount the microSD card.
#
@@ -31,9 +36,6 @@ else
echo "[init] no microSD card found"
fi
usleep 500
#
# Look for an init script on the microSD card.
#
apps/ardrone_interface/ardrone_interface.c
-1
View File
@@ -286,7 +286,6 @@ int ardrone_interface_thread_main(int argc, char *argv[])
/* close uarts */
close(ardrone_write);
//ar_multiplexing_deinit(gpios);
/* enable UART, writes potentially an empty buffer, but multiplexing is disabled */
ardrone_write = ardrone_open_uart(device, &uart_config_original);
apps/attitude_estimator_ekf/.context
View File
apps/attitude_estimator_ekf/Makefile
+4 -2
View File
@@ -40,6 +40,7 @@ CSRCS = attitude_estimator_ekf_main.c \
codegen/eye.c \
codegen/attitudeKalmanfilter.c \
codegen/mrdivide.c \
codegen/rdivide.c \
codegen/attitudeKalmanfilter_initialize.c \
codegen/attitudeKalmanfilter_terminate.c \
codegen/rt_nonfinite.c \
@@ -47,8 +48,9 @@ CSRCS = attitude_estimator_ekf_main.c \
codegen/rtGetNaN.c \
codegen/norm.c \
codegen/diag.c \
codegen/cross.c \
codegen/power.c
codegen/power.c \
codegen/cross.c
# XXX this is *horribly* broken
INCLUDES += $(TOPDIR)/../mavlink/include/mavlink
apps/attitude_estimator_ekf/attitudeKalmanfilter.m
Executable → Regular
View File
apps/attitude_estimator_ekf/attitudeKalmanfilter.prj
Executable → Regular
View File
apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
+198 -182
View File
@@ -71,46 +71,34 @@ __EXPORT int attitude_estimator_ekf_main(int argc, char *argv[]);
static unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
static float dt = 1.0f;
static float dt = 0.005f;
/* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */
static float z_k[9]; /**< Measurement vector */
static float x_aposteriori_k[12]; /**< */
static float x_aposteriori[12];
static float z_k[9] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 9.81f, 0.2f, -0.2f, 0.2f}; /**< Measurement vector */
static float x_aposteriori_k[12]; /**< states */
static float P_aposteriori_k[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.0f, 100.0f, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 100.0f, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 0, 100.0f,
};
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.0f, 100.0f, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 100.0f, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 0, 100.0f,
}; /**< init: diagonal matrix with big values */
static float 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.0f, 100.0f, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 100.0f, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 0, 100.0f,
}; /**< init: diagonal matrix with big values */
static float x_aposteriori[12];
static float P_aposteriori[144];
/* output euler angles */
static float euler[3] = {0.0f, 0.0f, 0.0f};
static float Rot_matrix[9] = {1.f, 0, 0,
0, 1.f, 0,
0, 0, 1.f
}; /**< init: identity matrix */
0, 1.f, 0,
0, 0, 1.f
}; /**< init: identity matrix */
static bool thread_should_exit = false; /**< Deamon exit flag */
@@ -159,11 +147,11 @@ int attitude_estimator_ekf_main(int argc, char *argv[])
thread_should_exit = false;
attitude_estimator_ekf_task = task_spawn("attitude_estimator_ekf",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
20000,
attitude_estimator_ekf_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
20000,
attitude_estimator_ekf_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
exit(0);
}
@@ -236,7 +224,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
/* 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);
orb_advert_t pub_dbg = -1;
/* keep track of sensor updates */
uint32_t sensor_last_count[3] = {0, 0, 0};
@@ -250,12 +238,18 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
/* initialize parameter handles */
parameters_init(&ekf_param_handles);
uint64_t start_time = hrt_absolute_time();
bool initialized = false;
float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f };
unsigned offset_count = 0;
/* Main loop*/
while (!thread_should_exit) {
struct pollfd fds[2] = {
{ .fd = sub_raw, .events = POLLIN },
{ .fd = sub_params, .events = POLLIN }
{ .fd = sub_raw, .events = POLLIN },
{ .fd = sub_params, .events = POLLIN }
};
int ret = poll(fds, 2, 1000);
@@ -265,7 +259,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
/* XXX this means no sensor data - should be critical or emergency */
printf("[attitude estimator ekf] WARNING: Not getting sensor data - sensor app running?\n");
} else {
/* only update parameters if they changed */
if (fds[1].revents & POLLIN) {
/* read from param to clear updated flag */
@@ -282,156 +276,178 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
/* get latest measurements */
orb_copy(ORB_ID(sensor_combined), sub_raw, &raw);
/* Calculate data time difference in seconds */
dt = (raw.timestamp - last_measurement) / 1000000.0f;
last_measurement = raw.timestamp;
uint8_t update_vect[3] = {0, 0, 0};
if (!initialized) {
/* Fill in gyro measurements */
if (sensor_last_count[0] != raw.gyro_counter) {
update_vect[0] = 1;
sensor_last_count[0] = raw.gyro_counter;
sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]);
sensor_last_timestamp[0] = raw.timestamp;
}
gyro_offsets[0] += raw.gyro_rad_s[0];
gyro_offsets[1] += raw.gyro_rad_s[1];
gyro_offsets[2] += raw.gyro_rad_s[2];
offset_count++;
z_k[0] = raw.gyro_rad_s[0];
z_k[1] = raw.gyro_rad_s[1];
z_k[2] = raw.gyro_rad_s[2];
if (hrt_absolute_time() - start_time > 3000000LL) {
initialized = true;
gyro_offsets[0] /= offset_count;
gyro_offsets[1] /= offset_count;
gyro_offsets[2] /= offset_count;
}
} else {
/* update accelerometer measurements */
if (sensor_last_count[1] != raw.accelerometer_counter) {
update_vect[1] = 1;
sensor_last_count[1] = raw.accelerometer_counter;
sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]);
sensor_last_timestamp[1] = raw.timestamp;
}
z_k[3] = raw.accelerometer_m_s2[0];
z_k[4] = raw.accelerometer_m_s2[1];
z_k[5] = raw.accelerometer_m_s2[2];
/* Calculate data time difference in seconds */
dt = (raw.timestamp - last_measurement) / 1000000.0f;
last_measurement = raw.timestamp;
uint8_t update_vect[3] = {0, 0, 0};
/* update magnetometer measurements */
if (sensor_last_count[2] != raw.magnetometer_counter) {
update_vect[2] = 1;
sensor_last_count[2] = raw.magnetometer_counter;
sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]);
sensor_last_timestamp[2] = raw.timestamp;
}
z_k[6] = raw.magnetometer_ga[0];
z_k[7] = raw.magnetometer_ga[1];
z_k[8] = raw.magnetometer_ga[2];
/* Fill in gyro measurements */
if (sensor_last_count[0] != raw.gyro_counter) {
update_vect[0] = 1;
sensor_last_count[0] = raw.gyro_counter;
sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]);
sensor_last_timestamp[0] = raw.timestamp;
}
uint64_t now = hrt_absolute_time();
unsigned int time_elapsed = now - last_run;
last_run = now;
z_k[0] = raw.gyro_rad_s[0] - gyro_offsets[0];
z_k[1] = raw.gyro_rad_s[1] - gyro_offsets[1];
z_k[2] = raw.gyro_rad_s[2] - gyro_offsets[2];
if (time_elapsed > loop_interval_alarm) {
//TODO: add warning, cpu overload here
// if (overloadcounter == 20) {
// printf("CPU OVERLOAD DETECTED IN ATTITUDE ESTIMATOR EKF (%lu > %lu)\n", time_elapsed, loop_interval_alarm);
// overloadcounter = 0;
/* update accelerometer measurements */
if (sensor_last_count[1] != raw.accelerometer_counter) {
update_vect[1] = 1;
sensor_last_count[1] = raw.accelerometer_counter;
sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]);
sensor_last_timestamp[1] = raw.timestamp;
}
z_k[3] = raw.accelerometer_m_s2[0];
z_k[4] = raw.accelerometer_m_s2[1];
z_k[5] = raw.accelerometer_m_s2[2];
/* update magnetometer measurements */
if (sensor_last_count[2] != raw.magnetometer_counter) {
update_vect[2] = 1;
sensor_last_count[2] = raw.magnetometer_counter;
sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]);
sensor_last_timestamp[2] = raw.timestamp;
}
z_k[6] = raw.magnetometer_ga[0];
z_k[7] = raw.magnetometer_ga[1];
z_k[8] = raw.magnetometer_ga[2];
uint64_t now = hrt_absolute_time();
unsigned int time_elapsed = now - last_run;
last_run = now;
if (time_elapsed > loop_interval_alarm) {
//TODO: add warning, cpu overload here
// if (overloadcounter == 20) {
// printf("CPU OVERLOAD DETECTED IN ATTITUDE ESTIMATOR EKF (%lu > %lu)\n", time_elapsed, loop_interval_alarm);
// overloadcounter = 0;
// }
overloadcounter++;
}
int32_t z_k_sizes = 9;
// float u[4] = {0.0f, 0.0f, 0.0f, 0.0f};
static bool const_initialized = false;
/* initialize with good values once we have a reasonable dt estimate */
if (!const_initialized && dt < 0.05 && dt > 0.005)
{
dt = 0.005f;
parameters_update(&ekf_param_handles, &ekf_params);
x_aposteriori_k[0] = z_k[0];
x_aposteriori_k[1] = z_k[1];
x_aposteriori_k[2] = z_k[2];
x_aposteriori_k[3] = 0.0f;
x_aposteriori_k[4] = 0.0f;
x_aposteriori_k[5] = 0.0f;
x_aposteriori_k[6] = z_k[3];
x_aposteriori_k[7] = z_k[4];
x_aposteriori_k[8] = z_k[5];
x_aposteriori_k[9] = z_k[6];
x_aposteriori_k[10] = z_k[7];
x_aposteriori_k[11] = z_k[8];
const_initialized = true;
}
/* do not execute the filter if not initialized */
if (!const_initialized) {
continue;
}
uint64_t timing_start = hrt_absolute_time();
attitudeKalmanfilter(update_vect, dt, z_k, x_aposteriori_k, P_aposteriori_k, ekf_params.q, ekf_params.r,
euler, Rot_matrix, x_aposteriori, P_aposteriori);
/* swap values for next iteration, check for fatal inputs */
if (isfinite(euler[0]) && isfinite(euler[1]) && isfinite(euler[2])) {
memcpy(P_aposteriori_k, P_aposteriori, sizeof(P_aposteriori_k));
memcpy(x_aposteriori_k, x_aposteriori, sizeof(x_aposteriori_k));
} else {
/* due to inputs or numerical failure the output is invalid, skip it */
continue;
}
uint64_t timing_diff = hrt_absolute_time() - timing_start;
// /* print rotation matrix every 200th time */
if (printcounter % 200 == 0) {
// printf("x apo:\n%8.4f\t%8.4f\t%8.4f\n%8.4f\t%8.4f\t%8.4f\n%8.4f\t%8.4f\t%8.4f\n",
// x_aposteriori[0], x_aposteriori[1], x_aposteriori[2],
// x_aposteriori[3], x_aposteriori[4], x_aposteriori[5],
// x_aposteriori[6], x_aposteriori[7], x_aposteriori[8]);
// }
//printf("EKF attitude iteration: %d, runtime: %d us, dt: %d us (%d Hz)\n", loopcounter, (int)timing_diff, (int)(dt * 1000000.0f), (int)(1.0f / dt));
//printf("roll: %8.4f\tpitch: %8.4f\tyaw:%8.4f\n", (double)euler[0], (double)euler[1], (double)euler[2]);
//printf("update rates gyro: %8.4f\taccel: %8.4f\tmag:%8.4f\n", (double)sensor_update_hz[0], (double)sensor_update_hz[1], (double)sensor_update_hz[2]);
// printf("\n%d\t%d\t%d\n%d\t%d\t%d\n%d\t%d\t%d\n", (int)(Rot_matrix[0] * 100), (int)(Rot_matrix[1] * 100), (int)(Rot_matrix[2] * 100),
// (int)(Rot_matrix[3] * 100), (int)(Rot_matrix[4] * 100), (int)(Rot_matrix[5] * 100),
// (int)(Rot_matrix[6] * 100), (int)(Rot_matrix[7] * 100), (int)(Rot_matrix[8] * 100));
}
// int i = printcounter % 9;
// // for (int i = 0; i < 9; i++) {
// char name[10];
// sprintf(name, "xapo #%d", i);
// memcpy(dbg.key, name, sizeof(dbg.key));
// dbg.value = x_aposteriori[i];
// if (pub_dbg < 0) {
// pub_dbg = orb_advertise(ORB_ID(debug_key_value), &dbg);
// } else {
// orb_publish(ORB_ID(debug_key_value), pub_dbg, &dbg);
// }
overloadcounter++;
printcounter++;
if (last_data > 0 && raw.timestamp - last_data > 12000) printf("[attitude estimator ekf] sensor data missed! (%llu)\n", raw.timestamp - last_data);
last_data = raw.timestamp;
/* send out */
att.timestamp = raw.timestamp;
att.roll = euler[0];
att.pitch = euler[1];
att.yaw = euler[2];
att.rollspeed = x_aposteriori[0];
att.pitchspeed = x_aposteriori[1];
att.yawspeed = x_aposteriori[2];
//att.yawspeed =z_k[2] ;
/* copy offsets */
memcpy(&att.rate_offsets, &(x_aposteriori[3]), sizeof(att.rate_offsets));
/* copy rotation matrix */
memcpy(&att.R, Rot_matrix, sizeof(Rot_matrix));
att.R_valid = true;
// Broadcast
orb_publish(ORB_ID(vehicle_attitude), pub_att, &att);
}
int32_t z_k_sizes = 9;
// float u[4] = {0.0f, 0.0f, 0.0f, 0.0f};
static bool const_initialized = false;
/* initialize with good values once we have a reasonable dt estimate */
if (!const_initialized /*&& dt < 0.05 && dt > 0.005*/)
{
dt = 0.005f;
parameters_update(&ekf_param_handles, &ekf_params);
x_aposteriori_k[0] = z_k[0];
x_aposteriori_k[1] = z_k[1];
x_aposteriori_k[2] = z_k[2];
x_aposteriori_k[3] = 0.0f;
x_aposteriori_k[4] = 0.0f;
x_aposteriori_k[5] = 0.0f;
x_aposteriori_k[6] = z_k[3];
x_aposteriori_k[7] = z_k[4];
x_aposteriori_k[8] = z_k[5];
x_aposteriori_k[9] = z_k[6];
x_aposteriori_k[10] = z_k[7];
x_aposteriori_k[11] = z_k[8];
const_initialized = true;
}
/* do not execute the filter if not initialized */
if (!const_initialized) {
continue;
}
dt = 0.004f;
uint64_t timing_start = hrt_absolute_time();
// attitudeKalmanfilter(dt, update_vect, z_k, &z_k_sizes, u, x_aposteriori_k, P_aposteriori_k, knownConst, euler,
// Rot_matrix, x_aposteriori, P_aposteriori);
attitudeKalmanfilter(update_vect, dt, z_k, x_aposteriori_k, P_aposteriori_k, ekf_params.q, ekf_params.r,
euler, Rot_matrix, x_aposteriori, P_aposteriori);
/* swap values for next iteration */
memcpy(P_aposteriori_k, P_aposteriori, sizeof(P_aposteriori_k));
memcpy(x_aposteriori_k, x_aposteriori, sizeof(x_aposteriori_k));
uint64_t timing_diff = hrt_absolute_time() - timing_start;
// /* print rotation matrix every 200th time */
if (printcounter % 200 == 0) {
// printf("x apo:\n%8.4f\t%8.4f\t%8.4f\n%8.4f\t%8.4f\t%8.4f\n%8.4f\t%8.4f\t%8.4f\n",
// x_aposteriori[0], x_aposteriori[1], x_aposteriori[2],
// x_aposteriori[3], x_aposteriori[4], x_aposteriori[5],
// x_aposteriori[6], x_aposteriori[7], x_aposteriori[8]);
// }
//printf("EKF attitude iteration: %d, runtime: %d us, dt: %d us (%d Hz)\n", loopcounter, (int)timing_diff, (int)(dt * 1000000.0f), (int)(1.0f / dt));
//printf("roll: %8.4f\tpitch: %8.4f\tyaw:%8.4f\n", (double)euler[0], (double)euler[1], (double)euler[2]);
//printf("update rates gyro: %8.4f\taccel: %8.4f\tmag:%8.4f\n", (double)sensor_update_hz[0], (double)sensor_update_hz[1], (double)sensor_update_hz[2]);
// printf("\n%d\t%d\t%d\n%d\t%d\t%d\n%d\t%d\t%d\n", (int)(Rot_matrix[0] * 100), (int)(Rot_matrix[1] * 100), (int)(Rot_matrix[2] * 100),
// (int)(Rot_matrix[3] * 100), (int)(Rot_matrix[4] * 100), (int)(Rot_matrix[5] * 100),
// (int)(Rot_matrix[6] * 100), (int)(Rot_matrix[7] * 100), (int)(Rot_matrix[8] * 100));
}
// int i = printcounter % 9;
// // for (int i = 0; i < 9; i++) {
// char name[10];
// sprintf(name, "xapo #%d", i);
// memcpy(dbg.key, name, sizeof(dbg.key));
// dbg.value = x_aposteriori[i];
// orb_publish(ORB_ID(debug_key_value), pub_dbg, &dbg);
printcounter++;
if (last_data > 0 && raw.timestamp - last_data > 12000) printf("[attitude estimator ekf] sensor data missed! (%llu)\n", raw.timestamp - last_data);
last_data = raw.timestamp;
/* send out */
att.timestamp = raw.timestamp;
att.roll = euler[0];
att.pitch = euler[1];
att.yaw = euler[2];
// XXX replace with x_apo after fix to filter
att.rollspeed = raw.gyro_rad_s[0]; //x_aposteriori[0];
att.pitchspeed = raw.gyro_rad_s[1]; //x_aposteriori[1];
att.yawspeed = raw.gyro_rad_s[2]; //x_aposteriori[2];
/* copy offsets */
memcpy(&att.rate_offsets, &(x_aposteriori[3]), sizeof(att.rate_offsets));
/* copy rotation matrix */
memcpy(&att.R, Rot_matrix, sizeof(Rot_matrix));
att.R_valid = true;
// Broadcast
orb_publish(ORB_ID(vehicle_attitude), pub_att, &att);
}
}
apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c
+6 -6
View File
@@ -61,13 +61,13 @@ PARAM_DEFINE_FLOAT(EKF_ATT_Q10, 1e-1f);
PARAM_DEFINE_FLOAT(EKF_ATT_Q11, 1e-1f);
/* gyro measurement noise */
PARAM_DEFINE_FLOAT(EKF_ATT_R0, 1e-2f);
PARAM_DEFINE_FLOAT(EKF_ATT_R1, 1e-2f);
PARAM_DEFINE_FLOAT(EKF_ATT_R2, 1e-2f);
PARAM_DEFINE_FLOAT(EKF_ATT_R0, 0.01f);
PARAM_DEFINE_FLOAT(EKF_ATT_R1, 0.01f);
PARAM_DEFINE_FLOAT(EKF_ATT_R2, 0.01f);
/* accelerometer measurement noise */
PARAM_DEFINE_FLOAT(EKF_ATT_R3, 1e-1f);
PARAM_DEFINE_FLOAT(EKF_ATT_R4, 1e-1f);
PARAM_DEFINE_FLOAT(EKF_ATT_R5, 1e-1f);
PARAM_DEFINE_FLOAT(EKF_ATT_R3, 1e1f);
PARAM_DEFINE_FLOAT(EKF_ATT_R4, 1e1f);
PARAM_DEFINE_FLOAT(EKF_ATT_R5, 1e1f);
/* magnetometer measurement noise */
PARAM_DEFINE_FLOAT(EKF_ATT_R6, 1e-1f);
PARAM_DEFINE_FLOAT(EKF_ATT_R7, 1e-1f);
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c
Executable → Regular
+219 -181
View File
File diff suppressed because it is too large Load Diff
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h
Executable → Regular
+2 -2
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Mon Oct 01 19:38:49 2012
* C source code generated on: Tue Oct 16 15:27:58 2012
*
*/
@@ -11,7 +11,7 @@
#define __ATTITUDEKALMANFILTER_H__
/* Include files */
#include <math.h>
#include <stdio.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include "rt_defines.h"
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c
Executable → Regular
+1 -1
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter_initialize'
*
* C source code generated on: Mon Oct 01 19:38:49 2012
* C source code generated on: Tue Oct 16 15:27:58 2012
*
*/
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h
Executable → Regular
+2 -2
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter_initialize'
*
* C source code generated on: Mon Oct 01 19:38:49 2012
* C source code generated on: Tue Oct 16 15:27:58 2012
*
*/
@@ -11,7 +11,7 @@
#define __ATTITUDEKALMANFILTER_INITIALIZE_H__
/* Include files */
#include <math.h>
#include <stdio.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include "rt_defines.h"
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c
Executable → Regular
+1 -1
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter_terminate'
*
* C source code generated on: Mon Oct 01 19:38:49 2012
* C source code generated on: Tue Oct 16 15:27:58 2012
*
*/
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h
Executable → Regular
+2 -2
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter_terminate'
*
* C source code generated on: Mon Oct 01 19:38:49 2012
* C source code generated on: Tue Oct 16 15:27:58 2012
*
*/
@@ -11,7 +11,7 @@
#define __ATTITUDEKALMANFILTER_TERMINATE_H__
/* Include files */
#include <math.h>
#include <stdio.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include "rt_defines.h"
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h
Executable → Regular
+1 -1
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Mon Oct 01 19:38:49 2012
* C source code generated on: Tue Oct 16 15:27:58 2012
*
*/
apps/attitude_estimator_ekf/codegen/cross.c
Executable → Regular
+1 -1
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'cross'
*
* C source code generated on: Mon Oct 01 19:38:49 2012
* C source code generated on: Tue Oct 16 15:27:58 2012
*
*/
apps/attitude_estimator_ekf/codegen/cross.h
Executable → Regular
+2 -2
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'cross'
*
* C source code generated on: Mon Oct 01 19:38:49 2012
* C source code generated on: Tue Oct 16 15:27:58 2012
*
*/
@@ -11,7 +11,7 @@
#define __CROSS_H__
/* Include files */
#include <math.h>
#include <stdio.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include "rt_defines.h"
apps/attitude_estimator_ekf/codegen/diag.c
Executable → Regular
+4 -4
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'diag'
*
* C source code generated on: Mon Oct 01 19:38:49 2012
* C source code generated on: Tue Oct 16 15:27:58 2012
*
*/
@@ -30,7 +30,7 @@
void b_diag(const real32_T v[9], real32_T d[81])
{
int32_T j;
memset((void *)&d[0], 0, 81U * sizeof(real32_T));
memset(&d[0], 0, 81U * sizeof(real32_T));
for (j = 0; j < 9; j++) {
d[j + 9 * j] = v[j];
}
@@ -57,7 +57,7 @@ void c_diag(const real32_T v[3], real32_T d[9])
void d_diag(const real32_T v[6], real32_T d[36])
{
int32_T j;
memset((void *)&d[0], 0, 36U * sizeof(real32_T));
memset(&d[0], 0, 36U * sizeof(real32_T));
for (j = 0; j < 6; j++) {
d[j + 6 * j] = v[j];
}
@@ -69,7 +69,7 @@ void d_diag(const real32_T v[6], real32_T d[36])
void diag(const real32_T v[12], real32_T d[144])
{
int32_T j;
memset((void *)&d[0], 0, 144U * sizeof(real32_T));
memset(&d[0], 0, 144U * sizeof(real32_T));
for (j = 0; j < 12; j++) {
d[j + 12 * j] = v[j];
}
apps/attitude_estimator_ekf/codegen/diag.h
Executable → Regular
+2 -2
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'diag'
*
* C source code generated on: Mon Oct 01 19:38:49 2012
* C source code generated on: Tue Oct 16 15:27:58 2012
*
*/
@@ -11,7 +11,7 @@
#define __DIAG_H__
/* Include files */
#include <math.h>
#include <stdio.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include "rt_defines.h"
apps/attitude_estimator_ekf/codegen/eye.c
Executable → Regular
+3 -3
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'eye'
*
* C source code generated on: Mon Oct 01 19:38:49 2012
* C source code generated on: Tue Oct 16 15:27:58 2012
*
*/
@@ -30,7 +30,7 @@
void b_eye(real_T I[144])
{
int32_T i;
memset((void *)&I[0], 0, 144U * sizeof(real_T));
memset(&I[0], 0, 144U * sizeof(real_T));
for (i = 0; i < 12; i++) {
I[i + 12 * i] = 1.0;
}
@@ -42,7 +42,7 @@ void b_eye(real_T I[144])
void eye(real_T I[9])
{
int32_T i;
memset((void *)&I[0], 0, 9U * sizeof(real_T));
memset(&I[0], 0, 9U * sizeof(real_T));
for (i = 0; i < 3; i++) {
I[i + 3 * i] = 1.0;
}
apps/attitude_estimator_ekf/codegen/eye.h
Executable → Regular
+2 -2
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'eye'
*
* C source code generated on: Mon Oct 01 19:38:49 2012
* C source code generated on: Tue Oct 16 15:27:58 2012
*
*/
@@ -11,7 +11,7 @@
#define __EYE_H__
/* Include files */
#include <math.h>
#include <stdio.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include "rt_defines.h"
apps/attitude_estimator_ekf/codegen/mrdivide.c
Executable → Regular
+104 -114
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'mrdivide'
*
* C source code generated on: Mon Oct 01 19:38:49 2012
* C source code generated on: Tue Oct 16 15:27:58 2012
*
*/
@@ -29,9 +29,9 @@
*/
void b_mrdivide(const real32_T A[36], const real32_T B[9], real32_T y[36])
{
real32_T b_A[9];
int32_T rtemp;
int32_T k;
real32_T b_A[9];
real32_T b_B[36];
int32_T r1;
int32_T r2;
@@ -54,15 +54,15 @@ void b_mrdivide(const real32_T A[36], const real32_T B[9], real32_T y[36])
r1 = 0;
r2 = 1;
r3 = 2;
maxval = (real32_T)fabsf(b_A[0]);
a21 = (real32_T)fabsf(b_A[1]);
maxval = (real32_T)fabs(b_A[0]);
a21 = (real32_T)fabs(b_A[1]);
if (a21 > maxval) {
maxval = a21;
r1 = 1;
r2 = 0;
}
if ((real32_T)fabsf(b_A[2]) > maxval) {
if ((real32_T)fabs(b_A[2]) > maxval) {
r1 = 2;
r2 = 1;
r3 = 0;
@@ -74,7 +74,7 @@ void b_mrdivide(const real32_T A[36], const real32_T B[9], real32_T y[36])
b_A[3 + r3] -= b_A[r3] * b_A[3 + r1];
b_A[6 + r2] -= b_A[r2] * b_A[6 + r1];
b_A[6 + r3] -= b_A[r3] * b_A[6 + r1];
if ((real32_T)fabsf(b_A[3 + r3]) > (real32_T)fabsf(b_A[3 + r2])) {
if ((real32_T)fabs(b_A[3 + r3]) > (real32_T)fabs(b_A[3 + r2])) {
rtemp = r2;
r2 = r3;
r3 = rtemp;
@@ -107,51 +107,46 @@ void b_mrdivide(const real32_T A[36], const real32_T B[9], real32_T y[36])
*/
void c_mrdivide(const real32_T A[72], const real32_T B[36], real32_T y[72])
{
int32_T jy;
int32_T iy;
real32_T b_A[36];
int8_T ipiv[6];
int32_T i3;
int32_T iy;
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;
int32_T jy;
int32_T ijA;
real32_T Y[72];
for (jy = 0; jy < 6; jy++) {
for (i3 = 0; i3 < 6; i3++) {
for (iy = 0; iy < 6; iy++) {
b_A[iy + 6 * jy] = B[jy + 6 * iy];
b_A[iy + 6 * i3] = B[i3 + 6 * iy];
}
ipiv[jy] = (int8_T)(1 + jy);
ipiv[i3] = (int8_T)(1 + i3);
}
for (j = 0; j < 5; j++) {
mmj = -j;
jj = j * 7;
jp1j = jj + 1;
c = mmj + 6;
jy = 0;
ix = jj;
temp = (real32_T)fabsf(b_A[jj]);
for (k = 2; k <= c; k++) {
c = j * 7;
iy = 0;
ix = c;
temp = (real32_T)fabs(b_A[c]);
for (k = 2; k <= 6 - j; k++) {
ix++;
s = (real32_T)fabsf(b_A[ix]);
s = (real32_T)fabs(b_A[ix]);
if (s > temp) {
jy = k - 1;
iy = k - 1;
temp = s;
}
}
if ((real_T)b_A[jj + jy] != 0.0) {
if (jy != 0) {
ipiv[j] = (int8_T)((j + jy) + 1);
if (b_A[c + iy] != 0.0F) {
if (iy != 0) {
ipiv[j] = (int8_T)((j + iy) + 1);
ix = j;
iy = j + jy;
iy += j;
for (k = 0; k < 6; k++) {
temp = b_A[ix];
b_A[ix] = b_A[iy];
@@ -161,42 +156,42 @@ void c_mrdivide(const real32_T A[72], const real32_T B[36], real32_T y[72])
}
}
loop_ub = (jp1j + mmj) + 5;
for (iy = jp1j; iy + 1 <= loop_ub; iy++) {
b_A[iy] /= b_A[jj];
i3 = (c - j) + 6;
for (jy = c + 1; jy + 1 <= i3; jy++) {
b_A[jy] /= b_A[c];
}
}
c = 5 - j;
jy = jj + 6;
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) + 12;
for (k = 7 + jj; k + 1 <= loop_ub; k++) {
b_A[k] += b_A[ix] * temp;
iy = c;
jy = c + 6;
for (k = 1; k <= 5 - j; k++) {
temp = b_A[jy];
if (b_A[jy] != 0.0F) {
ix = c + 1;
i3 = (iy - j) + 12;
for (ijA = 7 + iy; ijA + 1 <= i3; ijA++) {
b_A[ijA] += b_A[ix] * -temp;
ix++;
}
}
jy += 6;
jj += 6;
iy += 6;
}
}
for (jy = 0; jy < 12; jy++) {
for (i3 = 0; i3 < 12; i3++) {
for (iy = 0; iy < 6; iy++) {
Y[iy + 6 * jy] = A[jy + 12 * iy];
Y[iy + 6 * i3] = A[i3 + 12 * iy];
}
}
for (iy = 0; iy < 6; iy++) {
if (ipiv[iy] != iy + 1) {
for (jy = 0; jy < 6; jy++) {
if (ipiv[jy] != jy + 1) {
for (j = 0; j < 12; j++) {
temp = Y[iy + 6 * j];
Y[iy + 6 * j] = Y[(ipiv[iy] + 6 * j) - 1];
Y[(ipiv[iy] + 6 * j) - 1] = temp;
temp = Y[jy + 6 * j];
Y[jy + 6 * j] = Y[(ipiv[jy] + 6 * j) - 1];
Y[(ipiv[jy] + 6 * j) - 1] = temp;
}
}
}
@@ -204,10 +199,10 @@ void c_mrdivide(const real32_T A[72], const real32_T B[36], real32_T y[72])
for (j = 0; j < 12; j++) {
c = 6 * j;
for (k = 0; k < 6; k++) {
jy = 6 * k;
if ((real_T)Y[k + c] != 0.0) {
for (iy = k + 2; iy < 7; iy++) {
Y[(iy + c) - 1] -= Y[k + c] * b_A[(iy + jy) - 1];
iy = 6 * k;
if (Y[k + c] != 0.0F) {
for (jy = k + 2; jy < 7; jy++) {
Y[(jy + c) - 1] -= Y[k + c] * b_A[(jy + iy) - 1];
}
}
}
@@ -216,19 +211,19 @@ void c_mrdivide(const real32_T A[72], const real32_T B[36], real32_T y[72])
for (j = 0; j < 12; j++) {
c = 6 * j;
for (k = 5; k > -1; k += -1) {
jy = 6 * 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];
iy = 6 * k;
if (Y[k + c] != 0.0F) {
Y[k + c] /= b_A[k + iy];
for (jy = 0; jy + 1 <= k; jy++) {
Y[jy + c] -= Y[k + c] * b_A[jy + iy];
}
}
}
}
for (jy = 0; jy < 6; jy++) {
for (i3 = 0; i3 < 6; i3++) {
for (iy = 0; iy < 12; iy++) {
y[iy + 12 * jy] = Y[jy + 6 * iy];
y[iy + 12 * i3] = Y[i3 + 6 * iy];
}
}
}
@@ -238,51 +233,46 @@ void c_mrdivide(const real32_T A[72], const real32_T B[36], real32_T y[72])
*/
void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108])
{
int32_T jy;
int32_T iy;
real32_T b_A[81];
int8_T ipiv[9];
int32_T i2;
int32_T iy;
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;
int32_T jy;
int32_T ijA;
real32_T Y[108];
for (jy = 0; jy < 9; jy++) {
for (i2 = 0; i2 < 9; i2++) {
for (iy = 0; iy < 9; iy++) {
b_A[iy + 9 * jy] = B[jy + 9 * iy];
b_A[iy + 9 * i2] = B[i2 + 9 * iy];
}
ipiv[jy] = (int8_T)(1 + jy);
ipiv[i2] = (int8_T)(1 + i2);
}
for (j = 0; j < 8; j++) {
mmj = -j;
jj = j * 10;
jp1j = jj + 1;
c = mmj + 9;
jy = 0;
ix = jj;
temp = (real32_T)fabsf(b_A[jj]);
for (k = 2; k <= c; k++) {
c = j * 10;
iy = 0;
ix = c;
temp = (real32_T)fabs(b_A[c]);
for (k = 2; k <= 9 - j; k++) {
ix++;
s = (real32_T)fabsf(b_A[ix]);
s = (real32_T)fabs(b_A[ix]);
if (s > temp) {
jy = k - 1;
iy = k - 1;
temp = s;
}
}
if ((real_T)b_A[jj + jy] != 0.0) {
if (jy != 0) {
ipiv[j] = (int8_T)((j + jy) + 1);
if (b_A[c + iy] != 0.0F) {
if (iy != 0) {
ipiv[j] = (int8_T)((j + iy) + 1);
ix = j;
iy = j + jy;
iy += j;
for (k = 0; k < 9; k++) {
temp = b_A[ix];
b_A[ix] = b_A[iy];
@@ -292,42 +282,42 @@ void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108])
}
}
loop_ub = (jp1j + mmj) + 8;
for (iy = jp1j; iy + 1 <= loop_ub; iy++) {
b_A[iy] /= b_A[jj];
i2 = (c - j) + 9;
for (jy = c + 1; jy + 1 <= i2; jy++) {
b_A[jy] /= b_A[c];
}
}
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;
iy = c;
jy = c + 9;
for (k = 1; k <= 8 - j; k++) {
temp = b_A[jy];
if (b_A[jy] != 0.0F) {
ix = c + 1;
i2 = (iy - j) + 18;
for (ijA = 10 + iy; ijA + 1 <= i2; ijA++) {
b_A[ijA] += b_A[ix] * -temp;
ix++;
}
}
jy += 9;
jj += 9;
iy += 9;
}
}
for (jy = 0; jy < 12; jy++) {
for (i2 = 0; i2 < 12; i2++) {
for (iy = 0; iy < 9; iy++) {
Y[iy + 9 * jy] = A[jy + 12 * iy];
Y[iy + 9 * i2] = A[i2 + 12 * iy];
}
}
for (iy = 0; iy < 9; iy++) {
if (ipiv[iy] != iy + 1) {
for (jy = 0; jy < 9; jy++) {
if (ipiv[jy] != jy + 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;
temp = Y[jy + 9 * j];
Y[jy + 9 * j] = Y[(ipiv[jy] + 9 * j) - 1];
Y[(ipiv[jy] + 9 * j) - 1] = temp;
}
}
}
@@ -335,10 +325,10 @@ void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108])
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];
iy = 9 * k;
if (Y[k + c] != 0.0F) {
for (jy = k + 2; jy < 10; jy++) {
Y[(jy + c) - 1] -= Y[k + c] * b_A[(jy + iy) - 1];
}
}
}
@@ -347,19 +337,19 @@ void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108])
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];
iy = 9 * k;
if (Y[k + c] != 0.0F) {
Y[k + c] /= b_A[k + iy];
for (jy = 0; jy + 1 <= k; jy++) {
Y[jy + c] -= Y[k + c] * b_A[jy + iy];
}
}
}
}
for (jy = 0; jy < 9; jy++) {
for (i2 = 0; i2 < 9; i2++) {
for (iy = 0; iy < 12; iy++) {
y[iy + 12 * jy] = Y[jy + 9 * iy];
y[iy + 12 * i2] = Y[i2 + 9 * iy];
}
}
}
apps/attitude_estimator_ekf/codegen/mrdivide.h
Executable → Regular
+2 -2
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'mrdivide'
*
* C source code generated on: Mon Oct 01 19:38:49 2012
* C source code generated on: Tue Oct 16 15:27:58 2012
*
*/
@@ -11,7 +11,7 @@
#define __MRDIVIDE_H__
/* Include files */
#include <math.h>
#include <stdio.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include "rt_defines.h"
apps/attitude_estimator_ekf/codegen/norm.c
Executable → Regular
+11 -19
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'norm'
*
* C source code generated on: Mon Oct 01 19:38:49 2012
* C source code generated on: Tue Oct 16 15:27:58 2012
*
*/
@@ -31,32 +31,24 @@ real32_T norm(const real32_T x[3])
{
real32_T y;
real32_T scale;
boolean_T firstNonZero;
int32_T k;
real32_T absxk;
real32_T t;
y = 0.0F;
scale = 0.0F;
firstNonZero = TRUE;
scale = 1.17549435E-38F;
for (k = 0; k < 3; k++) {
if (x[k] != 0.0F) {
absxk = (real32_T)fabsf(x[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;
}
absxk = (real32_T)fabs(x[k]);
if (absxk > scale) {
t = scale / absxk;
y = 1.0F + y * t * t;
scale = absxk;
} else {
t = absxk / scale;
y += t * t;
}
}
return scale * (real32_T)sqrtf(y);
return scale * (real32_T)sqrt(y);
}
/* End of code generation (norm.c) */
apps/attitude_estimator_ekf/codegen/norm.h
Executable → Regular
+2 -2
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'norm'
*
* C source code generated on: Mon Oct 01 19:38:49 2012
* C source code generated on: Tue Oct 16 15:27:58 2012
*
*/
@@ -11,7 +11,7 @@
#define __NORM_H__
/* Include files */
#include <math.h>
#include <stdio.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include "rt_defines.h"
apps/attitude_estimator_ekf/codegen/power.c
Executable → Regular
+13 -13
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'power'
*
* C source code generated on: Mon Oct 01 19:38:49 2012
* C source code generated on: Tue Oct 16 15:27:58 2012
*
*/
@@ -27,17 +27,17 @@ static real32_T rt_powf_snf(real32_T u0, real32_T u1);
static real32_T rt_powf_snf(real32_T u0, real32_T u1)
{
real32_T y;
real32_T f0;
real32_T f1;
real32_T f2;
if (rtIsNaNF(u0) || rtIsNaNF(u1)) {
y = ((real32_T)rtNaN);
} else {
f0 = (real32_T)fabsf(u0);
f1 = (real32_T)fabsf(u1);
f1 = (real32_T)fabs(u0);
f2 = (real32_T)fabs(u1);
if (rtIsInfF(u1)) {
if (f0 == 1.0F) {
if (f1 == 1.0F) {
y = ((real32_T)rtNaN);
} else if (f0 > 1.0F) {
} else if (f1 > 1.0F) {
if (u1 > 0.0F) {
y = ((real32_T)rtInf);
} else {
@@ -48,9 +48,9 @@ static real32_T rt_powf_snf(real32_T u0, real32_T u1)
} else {
y = ((real32_T)rtInf);
}
} else if (f1 == 0.0F) {
} else if (f2 == 0.0F) {
y = 1.0F;
} else if (f1 == 1.0F) {
} else if (f2 == 1.0F) {
if (u1 > 0.0F) {
y = u0;
} else {
@@ -59,11 +59,11 @@ static real32_T rt_powf_snf(real32_T u0, real32_T u1)
} else if (u1 == 2.0F) {
y = u0 * u0;
} else if ((u1 == 0.5F) && (u0 >= 0.0F)) {
y = (real32_T)sqrtf(u0);
} else if ((u0 < 0.0F) && (u1 > (real32_T)floorf(u1))) {
y = (real32_T)sqrt(u0);
} else if ((u0 < 0.0F) && (u1 > (real32_T)floor(u1))) {
y = ((real32_T)rtNaN);
} else {
y = (real32_T)powf(u0, u1);
y = (real32_T)pow(u0, u1);
}
}
@@ -73,11 +73,11 @@ static real32_T rt_powf_snf(real32_T u0, real32_T u1)
/*
*
*/
void power(const real32_T a[12], real_T b, real32_T y[12])
void power(const real32_T a[12], real32_T y[12])
{
int32_T k;
for (k = 0; k < 12; k++) {
y[k] = rt_powf_snf(a[k], (real32_T)b);
y[k] = rt_powf_snf(a[k], 2.0F);
}
}
apps/attitude_estimator_ekf/codegen/power.h
Executable → Regular
+3 -3
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'power'
*
* C source code generated on: Mon Oct 01 19:38:49 2012
* C source code generated on: Tue Oct 16 15:27:58 2012
*
*/
@@ -11,7 +11,7 @@
#define __POWER_H__
/* Include files */
#include <math.h>
#include <stdio.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include "rt_defines.h"
@@ -29,6 +29,6 @@
/* Variable Definitions */
/* Function Declarations */
extern void power(const real32_T a[12], real_T b, real32_T y[12]);
extern void power(const real32_T a[12], real32_T y[12]);
#endif
/* End of code generation (power.h) */
apps/attitude_estimator_ekf/codegen/rdivide.c
+38
View File
@@ -0,0 +1,38 @@
/*
* rdivide.c
*
* Code generation for function 'rdivide'
*
* C source code generated on: Tue Oct 16 15:27:58 2012
*
*/
/* Include files */
#include "rt_nonfinite.h"
#include "attitudeKalmanfilter.h"
#include "rdivide.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
/* Function Definitions */
/*
*
*/
void rdivide(const real32_T x[3], real32_T y, real32_T z[3])
{
int32_T i;
for (i = 0; i < 3; i++) {
z[i] = x[i] / y;
}
}
/* End of code generation (rdivide.c) */
apps/attitude_estimator_ekf/codegen/rdivide.h
+34
View File
@@ -0,0 +1,34 @@
/*
* rdivide.h
*
* Code generation for function 'rdivide'
*
* C source code generated on: Tue Oct 16 15:27:58 2012
*
*/
#ifndef __RDIVIDE_H__
#define __RDIVIDE_H__
/* Include files */
#include <math.h>
#include <stddef.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 rdivide(const real32_T x[3], real32_T y, real32_T z[3]);
#endif
/* End of code generation (rdivide.h) */
apps/attitude_estimator_ekf/codegen/rtGetInf.c
Executable → Regular
+1 -1
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Mon Oct 01 19:38:49 2012
* C source code generated on: Tue Oct 16 15:27:58 2012
*
*/
apps/attitude_estimator_ekf/codegen/rtGetInf.h
Executable → Regular
+1 -1
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Mon Oct 01 19:38:49 2012
* C source code generated on: Tue Oct 16 15:27:58 2012
*
*/
apps/attitude_estimator_ekf/codegen/rtGetNaN.c
Executable → Regular
+1 -1
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Mon Oct 01 19:38:49 2012
* C source code generated on: Tue Oct 16 15:27:58 2012
*
*/
apps/attitude_estimator_ekf/codegen/rtGetNaN.h
Executable → Regular
+1 -1
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Mon Oct 01 19:38:49 2012
* C source code generated on: Tue Oct 16 15:27:58 2012
*
*/
apps/attitude_estimator_ekf/codegen/rt_defines.h
Executable → Regular
+1 -1
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Mon Oct 01 19:38:49 2012
* C source code generated on: Tue Oct 16 15:27:58 2012
*
*/
apps/attitude_estimator_ekf/codegen/rt_nonfinite.c
Executable → Regular
+1 -1
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Mon Oct 01 19:38:49 2012
* C source code generated on: Tue Oct 16 15:27:58 2012
*
*/
apps/attitude_estimator_ekf/codegen/rt_nonfinite.h
Executable → Regular
+1 -1
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Mon Oct 01 19:38:49 2012
* C source code generated on: Tue Oct 16 15:27:58 2012
*
*/
apps/attitude_estimator_ekf/codegen/rtwtypes.h
Executable → Regular
+3 -3
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Mon Oct 01 19:38:49 2012
* C source code generated on: Tue Oct 16 15:27:58 2012
*
*/
@@ -26,8 +26,8 @@
* Number of bits: char: 8 short: 16 int: 32
* long: 32 native word size: 32
* Byte ordering: LittleEndian
* Signed integer division rounds to: Zero
* Shift right on a signed integer as arithmetic shift: on
* Signed integer division rounds to: Undefined
* Shift right on a signed integer as arithmetic shift: off
*=======================================================================*/
/*=======================================================================*
apps/commander/calibration_routines.c
+175
View File
@@ -0,0 +1,175 @@
#include <math.h>
#include "calibration_routines.h"
int sphere_fit_least_squares(const float x[], const float y[], const float z[],
unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z, float *sphere_radius) {
float x_sumplain = 0.0f;
float x_sumsq = 0.0f;
float x_sumcube = 0.0f;
float y_sumplain = 0.0f;
float y_sumsq = 0.0f;
float y_sumcube = 0.0f;
float z_sumplain = 0.0f;
float z_sumsq = 0.0f;
float z_sumcube = 0.0f;
float xy_sum = 0.0f;
float xz_sum = 0.0f;
float yz_sum = 0.0f;
float x2y_sum = 0.0f;
float x2z_sum = 0.0f;
float y2x_sum = 0.0f;
float y2z_sum = 0.0f;
float z2x_sum = 0.0f;
float z2y_sum = 0.0f;
for (unsigned int i = 0; i < size; i++) {
float x2 = x[i] * x[i];
float y2 = y[i] * y[i];
float z2 = z[i] * z[i];
x_sumplain += x[i];
x_sumsq += x2;
x_sumcube += x2 * x[i];
y_sumplain += y[i];
y_sumsq += y2;
y_sumcube += y2 * y[i];
z_sumplain += z[i];
z_sumsq += z2;
z_sumcube += z2 * z[i];
xy_sum += x[i] * y[i];
xz_sum += x[i] * z[i];
yz_sum += y[i] * z[i];
x2y_sum += x2 * y[i];
x2z_sum += x2 * z[i];
y2x_sum += y2 * x[i];
y2z_sum += y2 * z[i];
z2x_sum += z2 * x[i];
z2y_sum += z2 * y[i];
}
//
//Least Squares Fit a sphere A,B,C with radius squared Rsq to 3D data
//
// P is a structure that has been computed with the data earlier.
// P.npoints is the number of elements; the length of X,Y,Z are identical.
// P's members are logically named.
//
// X[n] is the x component of point n
// Y[n] is the y component of point n
// Z[n] is the z component of point n
//
// A is the x coordiante of the sphere
// B is the y coordiante of the sphere
// C is the z coordiante of the sphere
// Rsq is the radius squared of the sphere.
//
//This method should converge; maybe 5-100 iterations or more.
//
float x_sum = x_sumplain / size; //sum( X[n] )
float x_sum2 = x_sumsq / size; //sum( X[n]^2 )
float x_sum3 = x_sumcube / size; //sum( X[n]^3 )
float y_sum = y_sumplain / size; //sum( Y[n] )
float y_sum2 = y_sumsq / size; //sum( Y[n]^2 )
float y_sum3 = y_sumcube / size; //sum( Y[n]^3 )
float z_sum = z_sumplain / size; //sum( Z[n] )
float z_sum2 = z_sumsq / size; //sum( Z[n]^2 )
float z_sum3 = z_sumcube / size; //sum( Z[n]^3 )
float XY = xy_sum / size; //sum( X[n] * Y[n] )
float XZ = xz_sum / size; //sum( X[n] * Z[n] )
float YZ = yz_sum / size; //sum( Y[n] * Z[n] )
float X2Y = x2y_sum / size; //sum( X[n]^2 * Y[n] )
float X2Z = x2z_sum / size; //sum( X[n]^2 * Z[n] )
float Y2X = y2x_sum / size; //sum( Y[n]^2 * X[n] )
float Y2Z = y2z_sum / size; //sum( Y[n]^2 * Z[n] )
float Z2X = z2x_sum / size; //sum( Z[n]^2 * X[n] )
float Z2Y = z2y_sum / size; //sum( Z[n]^2 * Y[n] )
//Reduction of multiplications
float F0 = x_sum2 + y_sum2 + z_sum2;
float F1 = 0.5f * F0;
float F2 = -8.0f * (x_sum3 + Y2X + Z2X);
float F3 = -8.0f * (X2Y + y_sum3 + Z2Y);
float F4 = -8.0f * (X2Z + Y2Z + z_sum3);
//Set initial conditions:
float A = x_sum;
float B = y_sum;
float C = z_sum;
//First iteration computation:
float A2 = A*A;
float B2 = B*B;
float C2 = C*C;
float QS = A2 + B2 + C2;
float QB = -2.0f * (A*x_sum + B*y_sum + C*z_sum);
//Set initial conditions:
float Rsq = F0 + QB + QS;
//First iteration computation:
float Q0 = 0.5f * (QS - Rsq);
float Q1 = F1 + Q0;
float Q2 = 8.0f * ( QS - Rsq + QB + F0 );
float aA,aB,aC,nA,nB,nC,dA,dB,dC;
//Iterate N times, ignore stop condition.
int n = 0;
while( n < max_iterations ){
n++;
//Compute denominator:
aA = Q2 + 16.0f * (A2 - 2.0f * A*x_sum + x_sum2);
aB = Q2 + 16.0f *(B2 - 2.0f * B*y_sum + y_sum2);
aC = Q2 + 16.0f *(C2 - 2.0f * C*z_sum + z_sum2);
aA = (aA == 0.0f) ? 1.0f : aA;
aB = (aB == 0.0f) ? 1.0f : aB;
aC = (aC == 0.0f) ? 1.0f : aC;
//Compute next iteration
nA = A - ((F2 + 16.0f * ( B*XY + C*XZ + x_sum*(-A2 - Q0) + A*(x_sum2 + Q1 - C*z_sum - B*y_sum) ) )/aA);
nB = B - ((F3 + 16.0f * ( A*XY + C*YZ + y_sum*(-B2 - Q0) + B*(y_sum2 + Q1 - A*x_sum - C*z_sum) ) )/aB);
nC = C - ((F4 + 16.0f * ( A*XZ + B*YZ + z_sum*(-C2 - Q0) + C*(z_sum2 + Q1 - A*x_sum - B*y_sum) ) )/aC);
//Check for stop condition
dA = (nA - A);
dB = (nB - B);
dC = (nC - C);
if( (dA*dA + dB*dB + dC*dC) <= delta ){ break; }
//Compute next iteration's values
A = nA;
B = nB;
C = nC;
A2 = A*A;
B2 = B*B;
C2 = C*C;
QS = A2 + B2 + C2;
QB = -2.0f * (A*x_sum + B*y_sum + C*z_sum);
Rsq = F0 + QB + QS;
Q0 = 0.5f * (QS - Rsq);
Q1 = F1 + Q0;
Q2 = 8.0f * ( QS - Rsq + QB + F0 );
}
*sphere_x = A;
*sphere_y = B;
*sphere_z = C;
*sphere_radius = sqrtf(Rsq);
return 0;
}
apps/commander/calibration_routines.h
+21
View File
@@ -0,0 +1,21 @@
/**
* Least-squares fit of a sphere to a set of points.
*
* Fits a sphere to a set of points on the sphere surface.
*
* @param x point coordinates on the X axis
* @param y point coordinates on the Y axis
* @param z point coordinates on the Z axis
* @param size number of points
* @param max_iterations abort if maximum number of iterations have been reached. If unsure, set to 100.
* @param delta abort if error is below delta. If unsure, set to 0 to run max_iterations times.
* @param sphere_x coordinate of the sphere center on the X axis
* @param sphere_y coordinate of the sphere center on the Y axis
* @param sphere_z coordinate of the sphere center on the Z axis
* @param sphere_radius sphere radius
*
* @return 0 on success, 1 on failure
*/
int sphere_fit_least_squares(const float x[], const float y[], const float z[],
unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z, float *sphere_radius);
apps/commander/commander.c
+317 -218
View File
File diff suppressed because it is too large Load Diff
apps/commander/state_machine_helper.c
+3 -3
View File
@@ -504,7 +504,7 @@ void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *c
current_status->flag_control_manual_enabled = true;
/* enable attitude control per default */
current_status->flag_control_attitude_enabled = true;
current_status->flag_control_rates_enabled = false; // XXX
current_status->flag_control_rates_enabled = true;
if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) {
@@ -519,7 +519,7 @@ void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_
current_status->flight_mode = VEHICLE_FLIGHT_MODE_STABILIZED;
current_status->flag_control_manual_enabled = true;
current_status->flag_control_attitude_enabled = true;
current_status->flag_control_rates_enabled = false; // XXX
current_status->flag_control_rates_enabled = true;
if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) {
@@ -534,7 +534,7 @@ void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *cur
current_status->flight_mode = VEHICLE_FLIGHT_MODE_AUTO;
current_status->flag_control_manual_enabled = true;
current_status->flag_control_attitude_enabled = true;
current_status->flag_control_rates_enabled = false; // XXX
current_status->flag_control_rates_enabled = true;
if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_STABILIZED) {
apps/drivers/drv_mag.h
+7 -1
View File
@@ -99,7 +99,13 @@ ORB_DECLARE(sensor_mag);
/** copy the mag scaling constants to the structure pointed to by (arg) */
#define MAGIOCGSCALE _MAGIOC(3)
/** set the measurement range to handle (at least) arg Gauss */
#define MAGIOCSRANGE _MAGIOC(4)
/** perform self-calibration, update scale factors to canonical units */
#define MAGIOCCALIBRATE _MAGIOC(4)
#define MAGIOCCALIBRATE _MAGIOC(5)
/** excite strap */
#define MAGIOCEXSTRAP _MAGIOC(6)
#endif /* _DRV_MAG_H */
nuttx/configs/px4fmu/include/drv_tone_alarm.h → apps/drivers/drv_tone_alarm.h
+1 -3
View File
@@ -63,8 +63,6 @@
#define _TONE_ALARM_BASE 0x7400
#define TONE_SET_ALARM _IOC(_TONE_ALARM_BASE, 1)
extern int tone_alarm_init(void);
/* structure describing one note in a tone pattern */
struct tone_note {
uint8_t pitch;
@@ -127,4 +125,4 @@ enum tone_pitch {
TONE_NOTE_MAX
};
#endif /* DRV_TONE_ALARM_H_ */
#endif /* DRV_TONE_ALARM_H_ */
apps/drivers/hmc5883/Makefile
+1 -1
View File
@@ -37,6 +37,6 @@
APPNAME = hmc5883
PRIORITY = SCHED_PRIORITY_DEFAULT
STACKSIZE = 2048
STACKSIZE = 4096
include $(APPDIR)/mk/app.mk
apps/drivers/hmc5883/hmc5883.cpp
+364 -6
View File
@@ -175,6 +175,36 @@ private:
*/
void stop();
/**
* Perform the on-sensor scale calibration routine.
*
* @note The sensor will continue to provide measurements, these
* will however reflect the uncalibrated sensor state until
* the calibration routine has been completed.
*
* @param enable set to 1 to enable self-test strap, 0 to disable
*/
int calibrate(struct file *filp, unsigned enable);
/**
* Perform the on-sensor scale calibration routine.
*
* @note The sensor will continue to provide measurements, these
* will however reflect the uncalibrated sensor state until
* the calibration routine has been completed.
*
* @param enable set to 1 to enable self-test positive strap, -1 to enable
* negative strap, 0 to set to normal mode
*/
int set_excitement(unsigned enable);
/**
* Set the sensor range.
*
* Sets the internal range to handle at least the argument in Gauss.
*/
int set_range(unsigned range);
/**
* Perform a poll cycle; collect from the previous measurement
* and start a new one.
@@ -254,9 +284,9 @@ HMC5883::HMC5883(int bus) :
_next_report(0),
_oldest_report(0),
_reports(nullptr),
_range_scale(0), /* default range scale from counts to gauss */
_range_ga(1.3f),
_mag_topic(-1),
_range_scale(1.0f / 1090.0f), /* default range scale from counts to gauss */
_range_ga(0.88f),
_sample_perf(perf_alloc(PC_ELAPSED, "hmc5883_read")),
_comms_errors(perf_alloc(PC_COUNT, "hmc5883_comms_errors")),
_buffer_overflows(perf_alloc(PC_COUNT, "hmc5883_buffer_overflows"))
@@ -308,11 +338,71 @@ HMC5883::init()
if (_mag_topic < 0)
debug("failed to create sensor_mag object");
/* set range */
set_range(_range_ga);
ret = OK;
out:
return ret;
}
int HMC5883::set_range(unsigned range)
{
uint8_t range_bits;
if (range < 1) {
range_bits = 0x00;
_range_scale = 1.0f / 1370.0f;
_range_ga = 0.88f;
} else if (range <= 1) {
range_bits = 0x01;
_range_scale = 1.0f / 1090.0f;
_range_ga = 1.3f;
} else if (range <= 2) {
range_bits = 0x02;
_range_scale = 1.0f / 820.0f;
_range_ga = 1.9f;
} else if (range <= 3) {
range_bits = 0x03;
_range_scale = 1.0f / 660.0f;
_range_ga = 2.5f;
} else if (range <= 4) {
range_bits = 0x04;
_range_scale = 1.0f / 440.0f;
_range_ga = 4.0f;
} else if (range <= 4.7f) {
range_bits = 0x05;
_range_scale = 1.0f / 390.0f;
_range_ga = 4.7f;
} else if (range <= 5.6f) {
range_bits = 0x06;
_range_scale = 1.0f / 330.0f;
_range_ga = 5.6f;
} else {
range_bits = 0x07;
_range_scale = 1.0f / 230.0f;
_range_ga = 8.1f;
}
int ret;
/*
* Send the command to set the range
*/
ret = write_reg(ADDR_CONF_B, (range_bits << 5));
if (OK != ret)
perf_count(_comms_errors);
uint8_t range_bits_in;
ret = read_reg(ADDR_CONF_B, range_bits_in);
if (OK != ret)
perf_count(_comms_errors);
return !(range_bits_in == (range_bits << 5));
}
int
HMC5883::probe()
{
@@ -495,6 +585,9 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg)
/* not supported, always 1 sample per poll */
return -EINVAL;
case MAGIOCSRANGE:
return set_range(arg);
case MAGIOCSLOWPASS:
/* not supported, no internal filtering */
return -EINVAL;
@@ -510,8 +603,10 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg)
return 0;
case MAGIOCCALIBRATE:
/* XXX perform auto-calibration */
return -EINVAL;
return calibrate(filp, arg);
case MAGIOCEXSTRAP:
return set_excitement(arg);
default:
/* give it to the superclass */
@@ -718,6 +813,194 @@ out:
return ret;
}
int HMC5883::calibrate(struct file *filp, unsigned enable)
{
struct mag_report report;
ssize_t sz;
int ret = 1;
// XXX do something smarter here
int fd = (int)enable;
struct mag_scale mscale_previous = {
0.0f,
1.0f,
0.0f,
1.0f,
0.0f,
1.0f,
};
struct mag_scale mscale_null = {
0.0f,
1.0f,
0.0f,
1.0f,
0.0f,
1.0f,
};
float avg_excited[3] = {0.0f, 0.0f, 0.0f};
unsigned i;
warnx("starting mag scale calibration");
/* do a simple demand read */
sz = read(filp, (char*)&report, sizeof(report));
if (sz != sizeof(report)) {
warn("immediate read failed");
ret = 1;
goto out;
}
warnx("current measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z);
warnx("time: %lld", report.timestamp);
warnx("sampling 500 samples for scaling offset");
/* set the queue depth to 10 */
if (OK != ioctl(filp, SENSORIOCSQUEUEDEPTH, 10)) {
warn("failed to set queue depth");
ret = 1;
goto out;
}
/* start the sensor polling at 50 Hz */
if (OK != ioctl(filp, SENSORIOCSPOLLRATE, 50)) {
warn("failed to set 2Hz poll rate");
ret = 1;
goto out;
}
/* Set to 2.5 Gauss */
if (OK != ioctl(filp, MAGIOCSRANGE, 2)) {
warnx("failed to set 2.5 Ga range");
ret = 1;
goto out;
}
if (OK != ioctl(filp, MAGIOCEXSTRAP, 1)) {
warnx("failed to enable sensor calibration mode");
ret = 1;
goto out;
}
if (OK != ioctl(filp, MAGIOCGSCALE, (long unsigned int)&mscale_previous)) {
warn("WARNING: failed to get scale / offsets for mag");
ret = 1;
goto out;
}
if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_null)) {
warn("WARNING: failed to set null scale / offsets for mag");
ret = 1;
goto out;
}
/* read the sensor 10x and report each value */
for (i = 0; i < 500; i++) {
struct pollfd fds;
/* wait for data to be ready */
fds.fd = fd;
fds.events = POLLIN;
ret = ::poll(&fds, 1, 2000);
if (ret != 1) {
warn("timed out waiting for sensor data");
goto out;
}
/* now go get it */
sz = ::read(fd, &report, sizeof(report));
if (sz != sizeof(report)) {
warn("periodic read failed");
goto out;
} else {
avg_excited[0] += report.x;
avg_excited[1] += report.y;
avg_excited[2] += report.z;
}
//warnx("periodic read %u", i);
//warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z);
}
avg_excited[0] /= i;
avg_excited[1] /= i;
avg_excited[2] /= i;
warnx("done. Performed %u reads", i);
warnx("measurement avg: %.6f %.6f %.6f", (double)avg_excited[0], (double)avg_excited[1], (double)avg_excited[2]);
float scaling[3];
/* calculate axis scaling */
scaling[0] = fabsf(1.16f / avg_excited[0]);
/* second axis inverted */
scaling[1] = fabsf(1.16f / -avg_excited[1]);
scaling[2] = fabsf(1.08f / avg_excited[2]);
warnx("axes scaling: %.6f %.6f %.6f", (double)scaling[0], (double)scaling[1], (double)scaling[2]);
/* set back to normal mode */
/* Set to 1.1 Gauss */
if (OK != ::ioctl(fd, MAGIOCSRANGE, 1)) {
warnx("failed to set 1.1 Ga range");
goto out;
}
if (OK != ::ioctl(fd, MAGIOCEXSTRAP, 0)) {
warnx("failed to disable sensor calibration mode");
goto out;
}
/* set scaling in device */
mscale_previous.x_scale = scaling[0];
mscale_previous.y_scale = scaling[1];
mscale_previous.z_scale = scaling[2];
if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_previous)) {
warn("WARNING: failed to set new scale / offsets for mag");
goto out;
}
ret = OK;
out:
if (ret == OK) {
warnx("mag scale calibration successfully finished.");
} else {
warnx("mag scale calibration failed.");
}
return ret;
}
int HMC5883::set_excitement(unsigned enable)
{
int ret;
/* arm the excitement strap */
uint8_t conf_reg;
ret = read_reg(ADDR_CONF_A, conf_reg);
if (OK != ret)
perf_count(_comms_errors);
if (((int)enable) < 0) {
conf_reg |= 0x01;
} else if (enable > 0) {
conf_reg |= 0x02;
} else {
conf_reg &= ~0x03;
}
ret = write_reg(ADDR_CONF_A, conf_reg);
if (OK != ret)
perf_count(_comms_errors);
uint8_t conf_reg_ret;
read_reg(ADDR_CONF_A, conf_reg_ret);
return !(conf_reg == conf_reg_ret);
}
int
HMC5883::write_reg(uint8_t reg, uint8_t val)
{
@@ -775,6 +1058,7 @@ void start();
void test();
void reset();
void info();
int calibrate();
/**
* Start the driver.
@@ -872,6 +1156,69 @@ test()
errx(0, "PASS");
}
/**
* Automatic scale calibration.
*
* Basic idea:
*
* output = (ext field +- 1.1 Ga self-test) * scale factor
*
* and consequently:
*
* 1.1 Ga = (excited - normal) * scale factor
* scale factor = (excited - normal) / 1.1 Ga
*
* sxy = (excited - normal) / 766 | for conf reg. B set to 0x60 / Gain = 3
* sz = (excited - normal) / 713 | for conf reg. B set to 0x60 / Gain = 3
*
* By subtracting the non-excited measurement the pure 1.1 Ga reading
* can be extracted and the sensitivity of all axes can be matched.
*
* SELF TEST OPERATION
* To check the HMC5883L for proper operation, a self test feature in incorporated
* in which the sensor offset straps are excited to create a nominal field strength
* (bias field) to be measured. To implement self test, the least significant bits
* (MS1 and MS0) of configuration register A are changed from 00 to 01 (positive bias)
* or 10 (negetive bias), e.g. 0x11 or 0x12.
* Then, by placing the mode register into single-measurement mode (0x01),
* two data acquisition cycles will be made on each magnetic vector.
* The first acquisition will be a set pulse followed shortly by measurement
* data of the external field. The second acquisition will have the offset strap
* excited (about 10 mA) in the positive bias mode for X, Y, and Z axes to create
* about a ±1.1 gauss self test field plus the external field. The first acquisition
* values will be subtracted from the second acquisition, and the net measurement
* will be placed into the data output registers.
* Since self test adds ~1.1 Gauss additional field to the existing field strength,
* using a reduced gain setting prevents sensor from being saturated and data registers
* overflowed. For example, if the configuration register B is set to 0x60 (Gain=3),
* values around +766 LSB (1.16 Ga * 660 LSB/Ga) will be placed in the X and Y data
* output registers and around +713 (1.08 Ga * 660 LSB/Ga) will be placed in Z data
* output register. To leave the self test mode, change MS1 and MS0 bit of the
* configuration register A back to 00 (Normal Measurement Mode), e.g. 0x10.
* Using the self test method described above, the user can scale sensor
*/
int calibrate()
{
int ret;
int fd = open(MAG_DEVICE_PATH, O_RDONLY);
if (fd < 0)
err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", MAG_DEVICE_PATH);
if (OK != (ret = ioctl(fd, MAGIOCCALIBRATE, fd))) {
warnx("failed to enable sensor calibration mode");
}
close(fd);
if (ret == OK) {
errx(0, "PASS");
} else {
errx(1, "FAIL");
}
}
/**
* Reset the driver.
*/
@@ -930,8 +1277,19 @@ hmc5883_main(int argc, char *argv[])
/*
* Print driver information.
*/
if (!strcmp(argv[1], "info"))
if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status"))
hmc5883::info();
errx(1, "unrecognised command, try 'start', 'test', 'reset' or 'info'");
/*
* Autocalibrate the scaling
*/
if (!strcmp(argv[1], "calibrate")) {
if (hmc5883::calibrate() == 0) {
errx(0, "calibration successful");
} else {
errx(1, "calibration failed");
}
}
errx(1, "unrecognized command, try 'start', 'test', 'reset' 'calibrate' or 'info'");
}
apps/drivers/l3gd20/l3gd20.cpp
+1 -1
View File
@@ -864,5 +864,5 @@ l3gd20_main(int argc, char *argv[])
if (!strcmp(argv[1], "info"))
l3gd20::info();
errx(1, "unrecognised command, try 'start', 'test', 'reset' or 'info'");
errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
}
apps/drivers/stm32/tone_alarm/Makefile
+43
View File
@@ -0,0 +1,43 @@
############################################################################
#
# Copyright (C) 2012 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
#
# Tone alarm driver
#
APPNAME = tone_alarm
PRIORITY = SCHED_PRIORITY_DEFAULT
STACKSIZE = 2048
INCLUDES = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common
include $(APPDIR)/mk/app.mk
nuttx/configs/px4fmu/src/drv_tone_alarm.c → apps/drivers/stm32/tone_alarm/tone_alarm.cpp
+243 -122
View File
File diff suppressed because it is too large Load Diff
apps/fixedwing_control/fixedwing_control.c
+124 -40
View File
@@ -2,6 +2,7 @@
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Ivan Ovinnikov <oivan@ethz.ch>
* Modifications: Doug Weibel <douglas.weibel@colorado.edu>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -31,7 +32,7 @@
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
// Workflow test comment - DEW
/**
* @file fixedwing_control.c
* Implementation of a fixed wing attitude and position controller.
@@ -86,27 +87,61 @@ static void usage(const char *reason);
* Controller parameters, accessible via MAVLink
*
*/
PARAM_DEFINE_FLOAT(FW_ROLL_POS_P, 0.3f);
PARAM_DEFINE_FLOAT(FW_ROLL_POS_I, 0.002f);
PARAM_DEFINE_FLOAT(FW_ROLL_POS_AWU, 0.2f);
PARAM_DEFINE_FLOAT(FW_ROLL_POS_LIM, 0.6f);
// Roll control parameters
PARAM_DEFINE_FLOAT(FW_ROLLRATE_P, 0.3f);
// Need to add functionality to suppress integrator windup while on the ground
// Suggested value of FW_ROLLRATE_I is 0.0 till this is in place
PARAM_DEFINE_FLOAT(FW_ROLLRATE_I, 0.0f);
PARAM_DEFINE_FLOAT(FW_ROLLRATE_AWU, 0.0f);
PARAM_DEFINE_FLOAT(FW_ROLLRATE_LIM, 0.7f); // Roll rate limit in radians/sec
PARAM_DEFINE_FLOAT(FW_ROLL_P, 0.3f);
PARAM_DEFINE_FLOAT(FW_ROLL_LIM, 0.7f); // Roll angle limit in radians
//Pitch control parameters
PARAM_DEFINE_FLOAT(FW_PITCHRATE_P, 0.3f);
// Need to add functionality to suppress integrator windup while on the ground
// Suggested value of FW_PITCHRATE_I is 0.0 till this is in place
PARAM_DEFINE_FLOAT(FW_PITCHRATE_I, 0.0f);
PARAM_DEFINE_FLOAT(FW_PITCHRATE_AWU, 0.0f);
PARAM_DEFINE_FLOAT(FW_PITCHRATE_LIM, 0.35f); // Pitch rate limit in radians/sec
PARAM_DEFINE_FLOAT(FW_PITCH_P, 0.3f);
PARAM_DEFINE_FLOAT(FW_PITCH_LIM, 0.35f); // Pitch angle limit in radians
struct fw_att_control_params {
float roll_pos_p;
float roll_pos_i;
float roll_pos_awu;
float roll_pos_lim;
float rollrate_p;
float rollrate_i;
float rollrate_awu;
float rollrate_lim;
float roll_p;
float roll_lim;
float pitchrate_p;
float pitchrate_i;
float pitchrate_awu;
float pitchrate_lim;
float pitch_p;
float pitch_lim;
};
struct fw_att_control_param_handles {
param_t roll_pos_p;
param_t roll_pos_i;
param_t roll_pos_awu;
param_t roll_pos_lim;
param_t rollrate_p;
param_t rollrate_i;
param_t rollrate_awu;
param_t rollrate_lim;
param_t roll_p;
param_t roll_lim;
param_t pitchrate_p;
param_t pitchrate_i;
param_t pitchrate_awu;
param_t pitchrate_lim;
param_t pitch_p;
param_t pitch_lim;
};
// XXX outsource position control to a separate app some time
// TO_DO - Navigation control will be moved to a separate app
// Attitude control will just handle the inner angle and rate loops
// to control pitch and roll, and turn coordination via rudder and
// possibly throttle compensation for battery voltage sag.
PARAM_DEFINE_FLOAT(FW_HEADING_P, 0.1f);
PARAM_DEFINE_FLOAT(FW_HEADING_LIM, 0.15f);
@@ -147,9 +182,9 @@ static int pos_parameters_update(const struct fw_pos_control_param_handles *h, s
/**
* The fixed wing control main loop.
* The fixed wing control main thread.
*
* This loop executes continously and calculates the control
* The main loop executes continously and calculates the control
* response.
*
* @param argc number of arguments
@@ -223,19 +258,31 @@ int fixedwing_control_thread_main(int argc, char *argv[])
pos_parameters_init(&hpos);
pos_parameters_update(&hpos, &ppos);
PID_t roll_pos_controller;
pid_init(&roll_pos_controller, p.roll_pos_p, p.roll_pos_i, 0.0f, p.roll_pos_awu,
PID_MODE_DERIVATIV_SET);
// TO_DO Fix output limit functionallity of PID controller or add that function elsewhere
PID_t roll_rate_controller;
pid_init(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0.0f, p.rollrate_awu,
p.rollrate_lim,PID_MODE_DERIVATIV_NONE);
PID_t roll_angle_controller;
pid_init(&roll_angle_controller, p.roll_p, 0.0f, 0.0f, 0.0f,
p.roll_lim,PID_MODE_DERIVATIV_NONE);
PID_t pitch_rate_controller;
pid_init(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0.0f, p.pitchrate_awu,
p.pitchrate_lim,PID_MODE_DERIVATIV_NONE);
PID_t pitch_angle_controller;
pid_init(&pitch_angle_controller, p.pitch_p, 0.0f, 0.0f, 0.0f,
p.pitch_lim,PID_MODE_DERIVATIV_NONE);
PID_t heading_controller;
pid_init(&heading_controller, ppos.heading_p, 0.0f, 0.0f, 0.0f,
PID_MODE_DERIVATIV_SET);
100.0f,PID_MODE_DERIVATIV_SET); // Temporary arbitrarily large limit
// XXX remove in production
/* 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);
// This is the top of the main loop
while(!thread_should_exit) {
struct pollfd fds[1] = {
@@ -251,11 +298,20 @@ int fixedwing_control_thread_main(int argc, char *argv[])
} else {
// FIXME SUBSCRIBE
if (loopcounter % 20 == 0) {
if (loopcounter % 100 == 0) {
att_parameters_update(&h, &p);
pos_parameters_update(&hpos, &ppos);
pid_set_parameters(&roll_pos_controller, p.roll_pos_p, p.roll_pos_i, 0.0f, p.roll_pos_awu);
pid_set_parameters(&heading_controller, ppos.heading_p, 0.0f, 0.0f, 0.0f);
pid_set_parameters(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0.0f,
p.rollrate_awu, p.rollrate_lim);
pid_set_parameters(&roll_angle_controller, p.roll_p, 0.0f, 0.0f,
0.0f, p.roll_lim);
pid_set_parameters(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0.0f,
p.pitchrate_awu, p.pitchrate_lim);
pid_set_parameters(&pitch_angle_controller, p.pitch_p, 0.0f, 0.0f,
0.0f, p.pitch_lim);
pid_set_parameters(&heading_controller, ppos.heading_p, 0.0f, 0.0f, 0.0f, 90.0f);
//printf("[fixedwing control debug] p: %8.4f, i: %8.4f, limit: %8.4f \n",
//p.rollrate_p, p.rollrate_i, p.rollrate_lim);
}
/* if position updated, run position control loop */
@@ -359,14 +415,25 @@ int fixedwing_control_thread_main(int argc, char *argv[])
printf("[fixedwing control] roll sp: %8.4f, \n", att_sp.roll_body);
}
// actuators.control[0] = pid_calculate(&roll_pos_controller, att_sp.roll_body,
// att.roll, att.rollspeed, deltaTpos);
actuators.control[0] = pid_calculate(&roll_pos_controller, att_sp.roll_body,
att.roll, att.rollspeed, deltaTpos);
actuators.control[1] = 0;
actuators.control[2] = 0;
// actuator control[0] is aileron (or elevon roll control)
// Commanded roll rate from P controller on roll angle
float roll_rate_command = pid_calculate(&roll_angle_controller, att_sp.roll_body,
att.roll, 0.0f, deltaTpos);
// actuator control from PI controller on roll rate
actuators.control[0] = pid_calculate(&roll_rate_controller, roll_rate_command,
att.rollspeed, 0.0f, deltaTpos);
// actuator control[1] is elevator (or elevon pitch control)
// Commanded pitch rate from P controller on pitch angle
float pitch_rate_command = pid_calculate(&pitch_angle_controller, att_sp.pitch_body,
att.pitch, 0.0f, deltaTpos);
// actuator control from PI controller on pitch rate
actuators.control[1] = pid_calculate(&pitch_rate_controller, pitch_rate_command,
att.pitchspeed, 0.0f, deltaTpos);
// actuator control[3] is throttle
actuators.control[3] = att_sp.thrust;
/* publish attitude setpoint (for MAVLink) */
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
@@ -446,21 +513,38 @@ int fixedwing_control_main(int argc, char *argv[])
static int att_parameters_init(struct fw_att_control_param_handles *h)
{
/* PID parameters */
h->roll_pos_p = param_find("FW_ROLL_POS_P");
h->roll_pos_i = param_find("FW_ROLL_POS_I");
h->roll_pos_lim = param_find("FW_ROLL_POS_LIM");
h->roll_pos_awu = param_find("FW_ROLL_POS_AWU");
h->rollrate_p = param_find("FW_ROLLRATE_P");
h->rollrate_i = param_find("FW_ROLLRATE_I");
h->rollrate_awu = param_find("FW_ROLLRATE_AWU");
h->rollrate_lim = param_find("FW_ROLLRATE_LIM");
h->roll_p = param_find("FW_ROLL_P");
h->roll_lim = param_find("FW_ROLL_LIM");
h->pitchrate_p = param_find("FW_PITCHRATE_P");
h->pitchrate_i = param_find("FW_PITCHRATE_I");
h->pitchrate_awu = param_find("FW_PITCHRATE_AWU");
h->pitchrate_lim = param_find("FW_PITCHRATE_LIM");
h->pitch_p = param_find("FW_PITCH_P");
h->pitch_lim = param_find("FW_PITCH_LIM");
return OK;
}
static int att_parameters_update(const struct fw_att_control_param_handles *h, struct fw_att_control_params *p)
{
param_get(h->roll_pos_p, &(p->roll_pos_p));
param_get(h->roll_pos_i, &(p->roll_pos_i));
param_get(h->roll_pos_lim, &(p->roll_pos_lim));
param_get(h->roll_pos_awu, &(p->roll_pos_awu));
param_get(h->rollrate_p, &(p->rollrate_p));
param_get(h->rollrate_i, &(p->rollrate_i));
param_get(h->rollrate_awu, &(p->rollrate_awu));
param_get(h->rollrate_lim, &(p->rollrate_lim));
param_get(h->roll_p, &(p->roll_p));
param_get(h->roll_lim, &(p->roll_lim));
param_get(h->pitchrate_p, &(p->pitchrate_p));
param_get(h->pitchrate_i, &(p->pitchrate_i));
param_get(h->pitchrate_awu, &(p->pitchrate_awu));
param_get(h->pitchrate_lim, &(p->pitchrate_lim));
param_get(h->pitch_p, &(p->pitch_p));
param_get(h->pitch_lim, &(p->pitch_lim));
return OK;
}

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