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

Merge remote-tracking branch 'upstream/ardrone'

Browse Source
This commit is contained in:
Julian Oes 2012-09-25 18:19:12 +02:00
commit 0eae48d480
59 changed files with 2925 additions and 2113 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
apps/attitude_estimator_ekf/Makefile
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

279
apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
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;
}

1031
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c
View File

File diff suppressed because it is too large Load Diff

6
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h
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) */

2
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c
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
*
*/

4
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h
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"

0
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_ref.rsp
View File

11
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_rtw.bat
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

328
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_rtw.mk
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

2
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c
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
*
*/

4
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h
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"

2
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h
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
*
*/

BIN
apps/attitude_estimator_ekf/codegen/buildInfo.mat
View File

Binary file not shown.

37
apps/attitude_estimator_ekf/codegen/cross.c Executable file
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) */

34
apps/attitude_estimator_ekf/codegen/cross.h Executable file
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) */

42
apps/attitude_estimator_ekf/codegen/diag.c Executable file
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) */

34
apps/attitude_estimator_ekf/codegen/diag.h Executable file
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) */

10
apps/attitude_estimator_ekf/codegen/eye.c
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;
}
}

6
apps/attitude_estimator_ekf/codegen/eye.h
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) */

77
apps/attitude_estimator_ekf/codegen/find.c Executable file
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) */

34
apps/attitude_estimator_ekf/codegen/find.h Executable file
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) */

751
apps/attitude_estimator_ekf/codegen/mrdivide.c
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];
}
}
}

6
apps/attitude_estimator_ekf/codegen/mrdivide.h
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) */

6
apps/attitude_estimator_ekf/codegen/norm.c
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) */

4
apps/attitude_estimator_ekf/codegen/norm.h
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"

2
apps/attitude_estimator_ekf/codegen/rtGetInf.c
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
*
*/

2
apps/attitude_estimator_ekf/codegen/rtGetInf.h
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
*
*/

2
apps/attitude_estimator_ekf/codegen/rtGetNaN.c
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
*
*/

2
apps/attitude_estimator_ekf/codegen/rtGetNaN.h
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
*
*/

24
apps/attitude_estimator_ekf/codegen/rt_defines.h Executable file
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) */

2
apps/attitude_estimator_ekf/codegen/rt_nonfinite.c
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
*
*/

2
apps/attitude_estimator_ekf/codegen/rt_nonfinite.h
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
*
*/

1
apps/attitude_estimator_ekf/codegen/rtw_proj.tmw
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.

2
apps/attitude_estimator_ekf/codegen/rtwtypes.h
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
*
*/

282
apps/commander/commander.c
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 */
@ -709,6 +714,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:
// {
@ -990,6 +1020,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");
@ -1061,10 +1095,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;
@ -1083,11 +1122,15 @@ 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);
orb_copy(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man);
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);
@ -1198,10 +1241,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) {
@ -1256,89 +1299,175 @@ 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) {
/* Start RC state check */
bool prev_lost = current_status.rc_signal_lost;
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_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_lost || ((hrt_absolute_time() - last_print_time) > 5000000)) {
/* only complain if the offboard control is NOT active */
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() - 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;
}
// 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();
/* 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 change */
publish_armed_status(&current_status);
}
/* 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) {
if ((hrt_absolute_time() - sp_offboard.timestamp) < 5000000) {
/* set up control mode */
if (current_status.flag_control_attitude_enabled !=
(sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE)) {
current_status.flag_control_attitude_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE);
state_changed = true;
}
if (current_status.flag_control_rates_enabled !=
(sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES)) {
current_status.flag_control_attitude_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES);
state_changed = true;
}
/* handle the case where RC 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_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_lost || ((hrt_absolute_time() - last_print_time) > 5000000)) {
/* only complain if the RC control is NOT active */
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();
@ -1353,8 +1482,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 */
@ -1372,7 +1503,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);

3
apps/commander/state_machine_helper.c
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);
}

1073
apps/fixedwing_control/fixedwing_control.c
View File

File diff suppressed because it is too large Load Diff

201
apps/mavlink/mavlink.c
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);
@ -931,29 +936,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 +952,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 +1055,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 +1071,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 +1139,8 @@ mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg)
}
}
#define MAVLINK_OFFBOARD_CONTROL_FLAG_ARMED 0x10
/****************************************************************************
* Public Functions
****************************************************************************/
@ -1242,30 +1250,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 +1415,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 +1428,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 */
@ -1479,7 +1562,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 */
@ -1582,7 +1665,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);

165
apps/multirotor_att_control/multirotor_att_control_main.c
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,30 +56,83 @@
#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 struct actuator_controls_s actuators;
static orb_advert_t actuator_pub;
/**
* Perform rate control right after gyro reading
*/
static void *rate_control_thread_main(void *arg)
{
prctl(PR_SET_NAME, "mc rate control", getpid());
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[])
@ -93,18 +148,18 @@ 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 actuator_controls_s actuators;
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));
/* 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 +176,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 +186,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 */
@ -145,6 +208,12 @@ mc_thread_main(int argc, char *argv[])
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 */
bool updated;
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 +224,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 +296,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 +316,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;

28
apps/multirotor_att_control/multirotor_attitude_control.c
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++;
}

5
apps/multirotor_att_control/multirotor_attitude_control.h
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_ */

14
apps/multirotor_att_control/multirotor_rate_control.c
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

4
apps/multirotor_att_control/multirotor_rate_control.h
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_ */

40
apps/multirotor_pos_control/position_control.c
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)

4
apps/px4/ground_estimator/ground_estimator.c
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);

4
apps/sdlog/sdlog.c
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;

8
apps/sensors/sensors.cpp
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 */
@ -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;
@ -1090,7 +1092,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;

3
apps/systemlib/Makefile
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

88
apps/systemlib/geo/geo.c Normal file
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;
}

49
apps/systemlib/geo/geo.h Normal file
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);

91
apps/systemlib/pid/pid.c
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;
}

7
apps/systemlib/pid/pid.h
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);

8
apps/uORB/objects_common.cpp
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);

50
apps/uORB/topics/manual_control_setpoint.h
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 */

94
apps/uORB/topics/offboard_control_setpoint.h Normal file
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

17
apps/uORB/topics/vehicle_attitude_setpoint.h
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 */

4
apps/uORB/topics/vehicle_command.h
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. */

28
apps/uORB/topics/ardrone_motors_setpoint.h → apps/uORB/topics/vehicle_rates_setpoint.h
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

12
apps/uORB/topics/vehicle_status.h
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,25 @@ 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;
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;