Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-06 16:49:51 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
px4dev
2012-09-27 19:50:20 -07:00
parent 62a4aa96b6 89ccd75949
commit 78c2f99f85
63 changed files with 3002 additions and 2296 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/ardrone_interface/ardrone_interface.c
+2 -3
View File
@@ -1,7 +1,7 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Lorenz Meier <lm@inf.ethz.ch>
* Author: Lorenz Meier <lm@inf.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -140,9 +140,7 @@ static int ardrone_open_uart(char *uart_name, struct termios *uart_config_origin
int speed = B115200;
int uart;
/* open uart */
printf("[ardrone_interface] UART is %s, baud rate is%d\n",uart_name,speed);
uart = open(uart_name, O_RDWR | O_NOCTTY);
/* Try to set baud rate */
@@ -329,6 +327,7 @@ int ardrone_interface_thread_main(int argc, char *argv[])
*/
if (armed.armed && !armed.lockdown) {
ardrone_mixing_and_output(ardrone_write, &actuator_controls);
} else {
/* Silently lock down motor speeds to zero */
ardrone_write_motor_commands(ardrone_write, 0, 0, 0, 0);
apps/ardrone_interface/ardrone_motor_control.c
+4 -6
View File
@@ -306,7 +306,7 @@ int ar_init_motors(int ardrone_uart, int gpios)
return errcounter;
}
/*
/**
* Sets the leds on the motor controllers, 1 turns led on, 0 off.
*/
void ar_set_leds(int ardrone_uart, uint8_t led1_red, uint8_t led1_green, uint8_t led2_red, uint8_t led2_green, uint8_t led3_red, uint8_t led3_green, uint8_t led4_red, uint8_t led4_green)
@@ -368,10 +368,11 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls
float yaw_control = actuators->control[2];
float motor_thrust = actuators->control[3];
//printf("AMO: Roll: %4.4f, Pitch: %4.4f, Yaw: %4.4f, Thrust: %4.4f\n",roll_control, pitch_control, yaw_control, motor_thrust);
const float min_thrust = 0.02f; /**< 2% minimum thrust */
const float max_thrust = 1.0f; /**< 100% max thrust */
const float scaling = 512.0f; /**< 100% thrust equals a value of 512 */
const float scaling = 500.0f; /**< 100% thrust equals a value of 500 which works, 512 leads to cutoff */
const float min_gas = min_thrust * scaling; /**< value range sent to motors, minimum */
const float max_gas = max_thrust * scaling; /**< value range sent to motors, maximum */
@@ -387,13 +388,10 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls
if (motor_thrust <= min_thrust) {
motor_thrust = min_thrust;
output_band = 0.0f;
} else if (motor_thrust < startpoint_full_control && motor_thrust > min_thrust) {
output_band = band_factor * (motor_thrust - min_thrust);
} else if (motor_thrust >= startpoint_full_control && motor_thrust < max_thrust - band_factor * startpoint_full_control) {
output_band = band_factor * startpoint_full_control;
} else if (motor_thrust >= max_thrust - band_factor * startpoint_full_control) {
output_band = band_factor * (max_thrust - motor_thrust);
}
apps/ardrone_interface/ardrone_motor_control.h
+3
View File
@@ -87,4 +87,7 @@ int ar_init_motors(int ardrone_uart, int gpio);
*/
void ar_set_leds(int ardrone_uart, uint8_t led1_red, uint8_t led1_green, uint8_t led2_red, uint8_t led2_green, uint8_t led3_red, uint8_t led3_green, uint8_t led4_red, uint8_t led4_green);
/**
* Mix motors and output actuators
*/
void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls_s *actuators);
apps/attitude_estimator_ekf/Makefile
+5 -2
View File
@@ -33,7 +33,7 @@
APPNAME = attitude_estimator_ekf
PRIORITY = SCHED_PRIORITY_DEFAULT
STACKSIZE = 20000
STACKSIZE = 2048
CSRCS = attitude_estimator_ekf_main.c \
codegen/eye.c \
@@ -44,7 +44,10 @@ CSRCS = attitude_estimator_ekf_main.c \
codegen/rt_nonfinite.c \
codegen/rtGetInf.c \
codegen/rtGetNaN.c \
codegen/norm.c
codegen/norm.c \
codegen/find.c \
codegen/diag.c \
codegen/cross.c
# XXX this is *horribly* broken
INCLUDES += $(TOPDIR)/../mavlink/include/mavlink
apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
+195 -84
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: Tobias Naegeli <naegelit@student.ethz.ch>
* Lorenz Meier <lm@inf.ethz.ch>
*
@@ -35,6 +35,7 @@
/*
* @file attitude_estimator_ekf_main.c
*
* Extended Kalman Filter for Attitude Estimation.
*/
@@ -54,6 +55,7 @@
#include <limits.h>
#include <math.h>
#include <uORB/uORB.h>
#include <uORB/topics/debug_key_value.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_attitude.h>
#include <arch/board/up_hrt.h>
@@ -69,34 +71,106 @@ __EXPORT int attitude_estimator_ekf_main(int argc, char *argv[]);
// #define PROJECTION_INITIALIZE_COUNTER_LIMIT 5000
// #define REPROJECTION_COUNTER_LIMIT 125
static unsigned int loop_interval_alarm = 4500; // loop interval in microseconds
static unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
static float dt = 1;
/* 0, 0, -9.81, 1, 1, 1, wo (gyro offset), w */
/* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */
static float z_k[9] = {0}; /**< Measurement vector */
static float x_aposteriori[12] = {0}; /**< */
static float P_aposteriori[144] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 100.f
static float x_aposteriori_k[9] = {0,0,0,0,0,-9.81,1,1,-1}; /**< */
static float x_aposteriori[9] = {0};
static float P_aposteriori_k[81] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0,
0, 100.f, 0, 0, 0, 0, 0, 0, 0,
0, 0, 100.f, 0, 0, 0, 0, 0, 0,
0, 0, 0, 100.f, 0, 0, 0, 0, 0,
0, 0, 0, 0, 100.f, 0, 0, 0, 0,
0, 0, 0, 0, 0, 100.f, 0, 0, 0,
0, 0, 0, 0, 0, 0, 100.f, 0, 0,
0, 0, 0, 0, 0, 0, 0, 100.f, 0,
0, 0, 0, 0, 0, 0, 0, 0, 100.f,
};
static float P_aposteriori[81] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0,
0, 100.f, 0, 0, 0, 0, 0, 0, 0,
0, 0, 100.f, 0, 0, 0, 0, 0, 0,
0, 0, 0, 100.f, 0, 0, 0, 0, 0,
0, 0, 0, 0, 100.f, 0, 0, 0, 0,
0, 0, 0, 0, 0, 100.f, 0, 0, 0,
0, 0, 0, 0, 0, 0, 100.f, 0, 0,
0, 0, 0, 0, 0, 0, 0, 100.f, 0,
0, 0, 0, 0, 0, 0, 0, 0, 100.f,
}; /**< init: diagonal matrix with big values */
static float knownConst[7] = {1, 1, 1, 1, 1, 1, 1}; /**< knownConst has the following entries [PrvaA,PrvarM,PrvarWO,PrvarW||MsvarA,MsvarM,MsvarW] */
static float knownConst[15] = {1, 1, 1, 1, 1, 0.04, 4, 0.1, 70, 70, -2000, 9.81, 1, 4, 1}; /**< knownConst has the following entries [PrvaA,PrvarM,PrvarWO,PrvarW||MsvarA,MsvarM,MsvarW] */
static float Rot_matrix[9] = {1.f, 0, 0,
0, 1.f, 0,
0, 0, 1.f
}; /**< init: identity matrix */
// static float x_aposteriori_k[12] = {0};
// static float P_aposteriori_k[144] = {0};
static bool thread_should_exit = false; /**< Deamon exit flag */
static bool thread_running = false; /**< Deamon status flag */
static int attitude_estimator_ekf_task; /**< Handle of deamon task / thread */
/**
* Mainloop of attitude_estimator_ekf.
*/
int attitude_estimator_ekf_thread_main(int argc, char *argv[]);
/**
* Print the correct usage.
*/
static void usage(const char *reason);
static void
usage(const char *reason)
{
if (reason)
fprintf(stderr, "%s\n", reason);
fprintf(stderr, "usage: attitude_estimator_ekf {start|stop|status} [-p <additional params>]\n\n");
exit(1);
}
/**
* The attitude_estimator_ekf app only briefly exists to start
* the background job. The stack size assigned in the
* Makefile does only apply to this management task.
*
* The actual stack size should be set in the call
* to task_create().
*/
int attitude_estimator_ekf_main(int argc, char *argv[])
{
if (argc < 1)
usage("missing command");
if (!strcmp(argv[1], "start")) {
if (thread_running) {
printf("attitude_estimator_ekf already running\n");
/* this is not an error */
exit(0);
}
thread_should_exit = false;
attitude_estimator_ekf_task = task_create("attitude_estimator_ekf", SCHED_PRIORITY_DEFAULT, 20000, attitude_estimator_ekf_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL);
exit(0);
}
if (!strcmp(argv[1], "stop")) {
thread_should_exit = true;
exit(0);
}
if (!strcmp(argv[1], "status")) {
if (thread_running) {
printf("\tattitude_estimator_ekf app is running\n");
} else {
printf("\tattitude_estimator_ekf app not started\n");
}
exit(0);
}
usage("unrecognized command");
exit(1);
}
/*
* [Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst)
@@ -110,7 +184,7 @@ static float Rot_matrix[9] = {1.f, 0, 0,
* @param argc number of commandline arguments (plus command name)
* @param argv strings containing the arguments
*/
int attitude_estimator_ekf_main(int argc, char *argv[])
int attitude_estimator_ekf_thread_main(int argc, char *argv[])
{
// print text
printf("Extended Kalman Filter Attitude Estimator initialized..\n\n");
@@ -124,14 +198,17 @@ int attitude_estimator_ekf_main(int argc, char *argv[])
/* store start time to guard against too slow update rates */
uint64_t last_run = hrt_absolute_time();
struct sensor_combined_s raw = {0};
struct vehicle_attitude_s att = {};
struct sensor_combined_s raw;
struct vehicle_attitude_s att;
uint64_t last_data = 0;
uint64_t last_measurement = 0;
/* subscribe to raw data */
int sub_raw = orb_subscribe(ORB_ID(sensor_combined));
/* rate-limit raw data updates to 200Hz */
orb_set_interval(sub_raw, 5);
/* advertise attitude */
orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);
@@ -139,20 +216,26 @@ int attitude_estimator_ekf_main(int argc, char *argv[])
int loopcounter = 0;
int printcounter = 0;
thread_running = true;
/* advertise debug value */
struct debug_key_value_s dbg = { .key = "", .value = 0.0f };
orb_advert_t pub_dbg = orb_advertise(ORB_ID(debug_key_value), &dbg);
/* Main loop*/
while (true) {
while (!thread_should_exit) {
struct pollfd fds[1] = {
{ .fd = sub_raw, .events = POLLIN },
};
int ret = poll(fds, 1, 1000);
/* check for a timeout */
if (ret == 0) {
/* */
/* update successful, copy data on every 2nd run and execute filter */
} else if (loopcounter & 0x01) {
if (ret < 0) {
/* XXX this is seriously bad - should be an emergency */
} else if (ret == 0) {
/* XXX this means no sensor data - should be critical or emergency */
printf("[attitude estimator ekf] WARNING: Not getting sensor data - sensor app running?\n");
} else {
orb_copy(ORB_ID(sensor_combined), sub_raw, &raw);
@@ -160,24 +243,19 @@ int attitude_estimator_ekf_main(int argc, char *argv[])
dt = (raw.timestamp - last_measurement) / 1000000.0f;
last_measurement = raw.timestamp;
// XXX Read out accel range via SPI on init, assuming 4G range at 14 bit res here
float range_g = 4.0f;
float mag_offset[3] = {0};
/* scale from 14 bit to m/s2 */
z_k[3] = ((raw.accelerometer_raw[0] * range_g) / 8192.0f) / 9.81f; // = accel * (1 / 32768.0f / 8.0f * 9.81f);
z_k[4] = ((raw.accelerometer_raw[1] * range_g) / 8192.0f) / 9.81f; // = accel * (1 / 32768.0f / 8.0f * 9.81f);
z_k[5] = ((raw.accelerometer_raw[2] * range_g) / 8192.0f) / 9.81f; // = accel * (1 / 32768.0f / 8.0f * 9.81f);
// XXX Read out mag range via I2C on init, assuming 0.88 Ga and 12 bit res here
z_k[0] = (raw.magnetometer_raw[0] - mag_offset[0]) * 0.01f;
z_k[1] = (raw.magnetometer_raw[1] - mag_offset[1]) * 0.01f;
z_k[2] = (raw.magnetometer_raw[2] - mag_offset[2]) * 0.01f;
/* Fill in gyro measurements */
z_k[6] = raw.gyro_raw[0] * 0.00026631611f /* = gyro * (500.0f / 180.0f * pi / 32768.0f ) */;
z_k[7] = raw.gyro_raw[1] * 0.00026631611f /* = gyro * (500.0f / 180.0f * pi / 32768.0f ) */;
z_k[8] = raw.gyro_raw[2] * 0.00026631611f /* = gyro * (500.0f / 180.0f * pi / 32768.0f ) */;
z_k[0] = raw.gyro_rad_s[0];
z_k[1] = raw.gyro_rad_s[1];
z_k[2] = raw.gyro_rad_s[2];
/* scale from 14 bit to m/s2 */
z_k[3] = raw.accelerometer_m_s2[0];
z_k[4] = raw.accelerometer_m_s2[1];
z_k[5] = raw.accelerometer_m_s2[2];
z_k[6] = raw.magnetometer_ga[0];
z_k[7] = raw.magnetometer_ga[1];
z_k[8] = raw.magnetometer_ga[2];
uint64_t now = hrt_absolute_time();
unsigned int time_elapsed = now - last_run;
@@ -186,60 +264,97 @@ int attitude_estimator_ekf_main(int argc, char *argv[])
if (time_elapsed > loop_interval_alarm) {
//TODO: add warning, cpu overload here
if (overloadcounter == 20) {
printf("CPU OVERLOAD DETECTED IN ATTITUDE ESTIMATOR BLACK MAGIC (%lu > %lu)\n", time_elapsed, loop_interval_alarm);
printf("CPU OVERLOAD DETECTED IN ATTITUDE ESTIMATOR EKF (%lu > %lu)\n", time_elapsed, loop_interval_alarm);
overloadcounter = 0;
}
overloadcounter++;
}
// now = hrt_absolute_time();
/* filter values */
/*
* function [Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst)
*/
int8_t update_vect[9] = {1, 1, 1, 1, 1, 1, 1, 1, 1};
float euler[3];
int32_t z_k_sizes = 9;
float u[4] = {0.0f, 0.0f, 0.0f, 0.0f};
static bool const_initialized = false;
/* initialize with good values once we have a reasonable dt estimate */
if (!const_initialized /*&& dt < 0.05 && dt > 0.005*/)
{
dt = 0.005f;
knownConst[0] = 0.6f*0.6f*dt;
knownConst[1] = 0.6f*0.6f*dt;
knownConst[2] = 0.2f*0.2f*dt;
knownConst[3] = 0.2f*0.2f*dt;
knownConst[4] = 0.000001f*0.000001f*dt; // -9.81,1,1,-1};
x_aposteriori_k[0] = z_k[0];
x_aposteriori_k[1] = z_k[1];
x_aposteriori_k[2] = z_k[2];
x_aposteriori_k[3] = z_k[3];
x_aposteriori_k[4] = z_k[4];
x_aposteriori_k[5] = z_k[5];
x_aposteriori_k[6] = z_k[6];
x_aposteriori_k[7] = z_k[7];
x_aposteriori_k[8] = z_k[8];
const_initialized = true;
}
/* do not execute the filter if not initialized */
if (!const_initialized) {
continue;
}
uint64_t timing_start = hrt_absolute_time();
attitudeKalmanfilter(dt, z_k, x_aposteriori, P_aposteriori, knownConst, Rot_matrix, x_aposteriori, P_aposteriori);
attitudeKalmanfilter(dt, update_vect, z_k, &z_k_sizes, u, x_aposteriori_k, P_aposteriori_k, knownConst, euler,
Rot_matrix, x_aposteriori, P_aposteriori);
/* swap values for next iteration */
memcpy(P_aposteriori_k, P_aposteriori, sizeof(P_aposteriori_k));
memcpy(x_aposteriori_k, x_aposteriori, sizeof(x_aposteriori_k));
uint64_t timing_diff = hrt_absolute_time() - timing_start;
/* print rotation matrix every 200th time */
if (printcounter % 200 == 0) {
printf("EKF attitude iteration: %d, runtime: %d us, dt: %d us (%d Hz)\n", loopcounter, (int)timing_diff, (int)(dt * 1000000.0f), (int)(1.0f / dt));
printf("\n%d\t%d\t%d\n%d\t%d\t%d\n%d\t%d\t%d\n", (int)(Rot_matrix[0] * 100), (int)(Rot_matrix[1] * 100), (int)(Rot_matrix[2] * 100),
(int)(Rot_matrix[3] * 100), (int)(Rot_matrix[4] * 100), (int)(Rot_matrix[5] * 100),
(int)(Rot_matrix[6] * 100), (int)(Rot_matrix[7] * 100), (int)(Rot_matrix[8] * 100));
if (printcounter % 2 == 0) {
// printf("x apo:\n%8.4f\t%8.4f\t%8.4f\n%8.4f\t%8.4f\t%8.4f\n%8.4f\t%8.4f\t%8.4f\n",
// x_aposteriori[0], x_aposteriori[1], x_aposteriori[2],
// x_aposteriori[3], x_aposteriori[4], x_aposteriori[5],
// x_aposteriori[6], x_aposteriori[7], x_aposteriori[8]);
// }
// printf("EKF attitude iteration: %d, runtime: %d us, dt: %d us (%d Hz)\n", loopcounter, (int)timing_diff, (int)(dt * 1000000.0f), (int)(1.0f / dt));
// printf("roll: %8.4f\tpitch: %8.4f\tyaw:%8.4f", (double)euler[0], (double)euler[1], (double)euler[2]);
// printf("\n%d\t%d\t%d\n%d\t%d\t%d\n%d\t%d\t%d\n", (int)(Rot_matrix[0] * 100), (int)(Rot_matrix[1] * 100), (int)(Rot_matrix[2] * 100),
// (int)(Rot_matrix[3] * 100), (int)(Rot_matrix[4] * 100), (int)(Rot_matrix[5] * 100),
// (int)(Rot_matrix[6] * 100), (int)(Rot_matrix[7] * 100), (int)(Rot_matrix[8] * 100));
}
int i = printcounter % 9;
// for (int i = 0; i < 9; i++) {
char name[10];
sprintf(name, "xapo #%d", i);
memcpy(dbg.key, name, sizeof(dbg.key));
dbg.value = x_aposteriori[i];
orb_publish(ORB_ID(debug_key_value), pub_dbg, &dbg);
printcounter++;
// time_elapsed = hrt_absolute_time() - now;
// if (blubb == 20)
// {
// printf("Estimator: %lu\n", time_elapsed);
// blubb = 0;
// }
// blubb++;
if (last_data > 0 && raw.timestamp - last_data > 12000) printf("[attitude estimator ekf] sensor data missed! (%llu)\n", raw.timestamp - last_data);
if (last_data > 0 && raw.timestamp - last_data > 8000) printf("sensor data missed! (%llu)\n", raw.timestamp - last_data);
// printf("%llu -> %llu = %llu\n", last_data, raw.timestamp, raw.timestamp - last_data);
last_data = raw.timestamp;
/* send out */
att.timestamp = raw.timestamp;
// att.roll = euler.x;
// att.pitch = euler.y;
// att.yaw = euler.z + F_M_PI;
att.roll = euler[0];
att.pitch = euler[1];
att.yaw = euler[2];
// if (att.yaw > 2 * F_M_PI) {
// att.yaw -= 2 * F_M_PI;
// }
// att.rollspeed = rates.x;
// att.pitchspeed = rates.y;
// att.yawspeed = rates.z;
// memcpy()R
att.rollspeed = x_aposteriori[0];
att.pitchspeed = x_aposteriori[1];
att.yawspeed = x_aposteriori[2];
// Broadcast
orb_publish(ORB_ID(vehicle_attitude), pub_att, &att);
@@ -248,11 +363,7 @@ int attitude_estimator_ekf_main(int argc, char *argv[])
loopcounter++;
}
/* Should never reach here */
thread_running = false;
return 0;
}
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c
+556 -475
View File
File diff suppressed because it is too large Load Diff
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h
+4 -2
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Wed Jul 11 08:38:35 2012
* C source code generated on: Fri Sep 21 13:56:43 2012
*
*/
@@ -14,6 +14,8 @@
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "rt_defines.h"
#include "rt_nonfinite.h"
#include "rtwtypes.h"
#include "attitudeKalmanfilter_types.h"
@@ -27,6 +29,6 @@
/* Variable Definitions */
/* Function Declarations */
extern void attitudeKalmanfilter(real32_T dt, const real32_T z_k[9], const real32_T x_aposteriori_k[12], const real32_T P_aposteriori_k[144], const real32_T knownConst[7], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T P_aposteriori[144]);
extern void attitudeKalmanfilter(real32_T dt, const int8_T updVect[9], const real32_T z_k_data[9], const int32_T z_k_sizes[1], const real32_T u[4], const real32_T x_aposteriori_k[9], const real32_T P_aposteriori_k[81], const real32_T knownConst[20], real32_T eulerAngles[3], real32_T Rot_matrix[9], real32_T x_aposteriori[9], real32_T P_aposteriori[81]);
#endif
/* End of code generation (attitudeKalmanfilter.h) */
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c
+1 -1
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter_initialize'
*
* C source code generated on: Wed Jul 11 08:38:35 2012
* C source code generated on: Fri Sep 21 13:56:44 2012
*
*/
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h
+3 -1
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter_initialize'
*
* C source code generated on: Wed Jul 11 08:38:35 2012
* C source code generated on: Fri Sep 21 13:56:44 2012
*
*/
@@ -14,6 +14,8 @@
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "rt_defines.h"
#include "rt_nonfinite.h"
#include "rtwtypes.h"
#include "attitudeKalmanfilter_types.h"
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_ref.rsp
View File
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_rtw.bat
-11
View File
@@ -1,11 +0,0 @@
@echo off
call "%VS100COMNTOOLS%..\..\VC\vcvarsall.bat" AMD64
cd .
nmake -f attitudeKalmanfilter_rtw.mk GENERATE_REPORT=1 ADD_MDL_NAME_TO_GLOBALS=0
@if errorlevel 1 goto error_exit
exit /B 0
:error_exit
echo The make command returned an error of %errorlevel%
An_error_occurred_during_the_call_to_make
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_rtw.mk
-328
View File
@@ -1,328 +0,0 @@
# Copyright 1994-2010 The MathWorks, Inc.
#
# File : xrt_vcx64.tmf $Revision: 1.1.6.1 $
#
# Abstract:
# Code generation template makefile for building a Windows-based,
# stand-alone real-time version of MATLAB-code using generated C/C++
# code and the
# Microsoft Visual C/C++ compiler version 8, 9, 10 for x64
#
# Note that this template is automatically customized by the
# code generation build procedure to create "<model>.mk"
#
# The following defines can be used to modify the behavior of the
# build:
#
# OPT_OPTS - Optimization option. See DEFAULT_OPT_OPTS in
# vctools.mak for default.
# OPTS - User specific options.
# CPP_OPTS - C++ compiler options.
# USER_SRCS - Additional user sources, such as files needed by
# S-functions.
# USER_INCLUDES - Additional include paths
# (i.e. USER_INCLUDES="-Iwhere-ever -Iwhere-ever2")
#
# To enable debugging:
# set OPT_OPTS=-Zi (may vary with compiler version, see compiler doc)
# modify tmf LDFLAGS to include /DEBUG
#
#------------------------ Macros read by make_rtw -----------------------------
#
# The following macros are read by the code generation build procedure:
#
# MAKECMD - This is the command used to invoke the make utility
# HOST - What platform this template makefile is targeted for
# (i.e. PC or UNIX)
# BUILD - Invoke make from the code generation build procedure
# (yes/no)?
# SYS_TARGET_FILE - Name of system target file.
MAKECMD = nmake
HOST = PC
BUILD = yes
SYS_TARGET_FILE = ert.tlc
BUILD_SUCCESS = ^#^#^# Created
COMPILER_TOOL_CHAIN = vcx64
#---------------------- Tokens expanded by make_rtw ---------------------------
#
# The following tokens, when wrapped with "|>" and "<|" are expanded by the
# code generation build procedure.
#
# MODEL_NAME - Name of the Simulink block diagram
# MODEL_MODULES - Any additional generated source modules
# MAKEFILE_NAME - Name of makefile created from template makefile <model>.mk
# MATLAB_ROOT - Path to where MATLAB is installed.
# MATLAB_BIN - Path to MATLAB executable.
# S_FUNCTIONS - List of S-functions.
# S_FUNCTIONS_LIB - List of S-functions libraries to link.
# SOLVER - Solver source file name
# NUMST - Number of sample times
# TID01EQ - yes (1) or no (0): Are sampling rates of continuous task
# (tid=0) and 1st discrete task equal.
# NCSTATES - Number of continuous states
# BUILDARGS - Options passed in at the command line.
# MULTITASKING - yes (1) or no (0): Is solver mode multitasking
# EXT_MODE - yes (1) or no (0): Build for external mode
# TMW_EXTMODE_TESTING - yes (1) or no (0): Build ext_test.c for external mode
# testing.
# EXTMODE_TRANSPORT - Index of transport mechanism (e.g. tcpip, serial) for extmode
# EXTMODE_STATIC - yes (1) or no (0): Use static instead of dynamic mem alloc.
# EXTMODE_STATIC_SIZE - Size of static memory allocation buffer.
MODEL = attitudeKalmanfilter
MODULES = attitudeKalmanfilter.c eye.c mrdivide.c norm.c attitudeKalmanfilter_initialize.c attitudeKalmanfilter_terminate.c rt_nonfinite.c rtGetNaN.c rtGetInf.c
MAKEFILE = attitudeKalmanfilter_rtw.mk
MATLAB_ROOT = C:\Program Files\MATLAB\R2011a
ALT_MATLAB_ROOT = C:\PROGRA~1\MATLAB\R2011a
MATLAB_BIN = C:\Program Files\MATLAB\R2011a\bin
ALT_MATLAB_BIN = C:\PROGRA~1\MATLAB\R2011a\bin
S_FUNCTIONS =
S_FUNCTIONS_LIB =
SOLVER =
NUMST = 1
TID01EQ = 0
NCSTATES = 0
BUILDARGS = GENERATE_REPORT=1 ADD_MDL_NAME_TO_GLOBALS=0
MULTITASKING = 0
EXT_MODE = 0
TMW_EXTMODE_TESTING = 0
EXTMODE_TRANSPORT = 0
EXTMODE_STATIC = 0
EXTMODE_STATIC_SIZE = 1000000
MODELREFS =
SHARED_SRC =
SHARED_SRC_DIR =
SHARED_BIN_DIR =
SHARED_LIB =
TARGET_LANG_EXT = c
OPTIMIZATION_FLAGS = /O2 /Oy-
ADDITIONAL_LDFLAGS =
#--------------------------- Model and reference models -----------------------
MODELLIB = attitudeKalmanfilter.lib
MODELREF_LINK_LIBS =
MODELREF_LINK_RSPFILE = attitudeKalmanfilter_ref.rsp
MODELREF_INC_PATH =
RELATIVE_PATH_TO_ANCHOR = F:\CODEGE~1
MODELREF_TARGET_TYPE = RTW
!if "$(MATLAB_ROOT)" != "$(ALT_MATLAB_ROOT)"
MATLAB_ROOT = $(ALT_MATLAB_ROOT)
!endif
!if "$(MATLAB_BIN)" != "$(ALT_MATLAB_BIN)"
MATLAB_BIN = $(ALT_MATLAB_BIN)
!endif
#--------------------------- Tool Specifications ------------------------------
CPU = AMD64
!include $(MATLAB_ROOT)\rtw\c\tools\vctools.mak
APPVER = 5.02
PERL = $(MATLAB_ROOT)\sys\perl\win32\bin\perl
#------------------------------ Include/Lib Path ------------------------------
MATLAB_INCLUDES = $(MATLAB_ROOT)\simulink\include
MATLAB_INCLUDES = $(MATLAB_INCLUDES);$(MATLAB_ROOT)\extern\include
MATLAB_INCLUDES = $(MATLAB_INCLUDES);$(MATLAB_ROOT)\rtw\c\src
MATLAB_INCLUDES = $(MATLAB_INCLUDES);$(MATLAB_ROOT)\rtw\c\src\ext_mode\common
# Additional file include paths
MATLAB_INCLUDES = $(MATLAB_INCLUDES);F:\codegenerationMatlabAttFilter\codegen\lib\attitudeKalmanfilter
MATLAB_INCLUDES = $(MATLAB_INCLUDES);F:\codegenerationMatlabAttFilter
INCLUDE = .;$(RELATIVE_PATH_TO_ANCHOR);$(MATLAB_INCLUDES);$(INCLUDE);$(MODELREF_INC_PATH)
!if "$(SHARED_SRC_DIR)" != ""
INCLUDE = $(INCLUDE);$(SHARED_SRC_DIR)
!endif
#------------------------ External mode ---------------------------------------
# Uncomment -DVERBOSE to have information printed to stdout
# To add a new transport layer, see the comments in
# <matlabroot>/toolbox/simulink/simulink/extmode_transports.m
!if $(EXT_MODE) == 1
EXT_CC_OPTS = -DEXT_MODE # -DVERBOSE
!if $(EXTMODE_TRANSPORT) == 0 #tcpip
EXT_SRC = updown.c rtiostream_interface.c rtiostream_tcpip.c
EXT_LIB = wsock32.lib
!endif
!if $(EXTMODE_TRANSPORT) == 1 #serial_win32
EXT_SRC = ext_svr.c updown.c ext_work.c ext_svr_serial_transport.c
EXT_SRC = $(EXT_SRC) ext_serial_pkt.c ext_serial_win32_port.c
EXT_LIB =
!endif
!if $(TMW_EXTMODE_TESTING) == 1
EXT_SRC = $(EXT_SRC) ext_test.c
EXT_CC_OPTS = $(EXT_CC_OPTS) -DTMW_EXTMODE_TESTING
!endif
!if $(EXTMODE_STATIC) == 1
EXT_SRC = $(EXT_SRC) mem_mgr.c
EXT_CC_OPTS = $(EXT_CC_OPTS) -DEXTMODE_STATIC -DEXTMODE_STATIC_SIZE=$(EXTMODE_STATIC_SIZE)
!endif
!else
EXT_SRC =
EXT_CC_OPTS =
EXT_LIB =
!endif
#------------------------ rtModel ----------------------------------------------
RTM_CC_OPTS = -DUSE_RTMODEL
#----------------- Compiler and Linker Options --------------------------------
# Optimization Options
# Set OPT_OPTS=-Zi for debugging (may depend on compiler version)
OPT_OPTS = $(DEFAULT_OPT_OPTS)
# General User Options
OPTS =
!if "$(OPTIMIZATION_FLAGS)" != ""
CC_OPTS = $(OPTS) $(EXT_CC_OPTS) $(RTM_CC_OPTS) $(OPTIMIZATION_FLAGS)
!else
CC_OPTS = $(OPT_OPTS) $(OPTS) $(EXT_CC_OPTS) $(RTM_CC_OPTS)
!endif
CPP_REQ_DEFINES = -DMODEL=$(MODEL) -DRT -DNUMST=$(NUMST) \
-DTID01EQ=$(TID01EQ) -DNCSTATES=$(NCSTATES) \
-DMT=$(MULTITASKING) -DHAVESTDIO
# Uncomment this line to move warning level to W4
# cflags = $(cflags:W3=W4)
CFLAGS = $(CC_OPTS) $(CPP_REQ_DEFINES) $(USER_INCLUDES) \
$(cflags) $(cvarsmt) /wd4996
CPPFLAGS = $(CPP_OPTS) $(CC_OPTS) $(CPP_REQ_DEFINES) $(USER_INCLUDES) \
$(cflags) $(cvarsmt) /wd4996 /EHsc-
LDFLAGS = $(ldebug) $(conflags) $(EXT_LIB) $(conlibs) libcpmt.lib $(ADDITIONAL_LDFLAGS)
# libcpmt.lib is the multi-threaded, static lib version of the C++ standard lib
#----------------------------- Source Files -----------------------------------
#Standalone executable
!if "$(MODELREF_TARGET_TYPE)" == "NONE"
PRODUCT = $(RELATIVE_PATH_TO_ANCHOR)\$(MODEL).exe
REQ_SRCS = $(MODULES) $(EXT_SRC)
#Model Reference Target
!else
PRODUCT = $(MODELLIB)
REQ_SRCS = $(MODULES) $(EXT_SRC)
!endif
USER_SRCS =
SRCS = $(REQ_SRCS) $(USER_SRCS) $(S_FUNCTIONS)
OBJS_CPP_UPPER = $(SRCS:.CPP=.obj)
OBJS_CPP_LOWER = $(OBJS_CPP_UPPER:.cpp=.obj)
OBJS_C_UPPER = $(OBJS_CPP_LOWER:.C=.obj)
OBJS = $(OBJS_C_UPPER:.c=.obj)
SHARED_OBJS = $(SHARED_SRC:.c=.obj)
# ------------------------- Additional Libraries ------------------------------
LIBS =
LIBS = $(LIBS)
# ---------------------------- Linker Script ----------------------------------
CMD_FILE = $(MODEL).lnk
GEN_LNK_SCRIPT = $(MATLAB_ROOT)\rtw\c\tools\mkvc_lnk.pl
#--------------------------------- Rules --------------------------------------
all: set_environment_variables $(PRODUCT)
!if "$(MODELREF_TARGET_TYPE)" == "NONE"
#--- Stand-alone model ---
$(PRODUCT) : $(OBJS) $(SHARED_LIB) $(LIBS) $(MODELREF_LINK_LIBS)
@cmd /C "echo ### Linking ..."
$(PERL) $(GEN_LNK_SCRIPT) $(CMD_FILE) $(OBJS)
$(LD) $(LDFLAGS) $(S_FUNCTIONS_LIB) $(SHARED_LIB) $(LIBS) $(MAT_LIBS) @$(CMD_FILE) @$(MODELREF_LINK_RSPFILE) -out:$@
@del $(CMD_FILE)
@cmd /C "echo $(BUILD_SUCCESS) executable $(MODEL).exe"
!else
#--- Model reference code generation Target ---
$(PRODUCT) : $(OBJS) $(SHARED_LIB)
@cmd /C "echo ### Linking ..."
$(PERL) $(GEN_LNK_SCRIPT) $(CMD_FILE) $(OBJS)
$(LD) -lib /OUT:$(PRODUCT) @$(CMD_FILE) $(S_FUNCTIONS_LIB)
@cmd /C "echo $(BUILD_SUCCESS) static library $(MODELLIB)"
!endif
{$(MATLAB_ROOT)\rtw\c\src}.c.obj :
@cmd /C "echo ### Compiling $<"
$(CC) $(CFLAGS) $<
{$(MATLAB_ROOT)\rtw\c\src\ext_mode\common}.c.obj :
@cmd /C "echo ### Compiling $<"
$(CC) $(CFLAGS) $<
{$(MATLAB_ROOT)\rtw\c\src\rtiostream\rtiostreamtcpip}.c.obj :
@cmd /C "echo ### Compiling $<"
$(CC) $(CFLAGS) $<
{$(MATLAB_ROOT)\rtw\c\src\ext_mode\serial}.c.obj :
@cmd /C "echo ### Compiling $<"
$(CC) $(CFLAGS) $<
{$(MATLAB_ROOT)\rtw\c\src\ext_mode\custom}.c.obj :
@cmd /C "echo ### Compiling $<"
$(CC) $(CFLAGS) $<
# Additional sources
# Put these rule last, otherwise nmake will check toolboxes first
{$(RELATIVE_PATH_TO_ANCHOR)}.c.obj :
@cmd /C "echo ### Compiling $<"
$(CC) $(CFLAGS) $<
{$(RELATIVE_PATH_TO_ANCHOR)}.cpp.obj :
@cmd /C "echo ### Compiling $<"
$(CC) $(CPPFLAGS) $<
.c.obj :
@cmd /C "echo ### Compiling $<"
$(CC) $(CFLAGS) $<
.cpp.obj :
@cmd /C "echo ### Compiling $<"
$(CC) $(CPPFLAGS) $<
!if "$(SHARED_LIB)" != ""
$(SHARED_LIB) : $(SHARED_SRC)
@cmd /C "echo ### Creating $@"
@$(CC) $(CFLAGS) -Fo$(SHARED_BIN_DIR)\ @<<
$?
<<
@$(LIBCMD) /nologo /out:$@ $(SHARED_OBJS)
@cmd /C "echo ### $@ Created"
!endif
set_environment_variables:
@set INCLUDE=$(INCLUDE)
@set LIB=$(LIB)
# Libraries:
#----------------------------- Dependencies -----------------------------------
$(OBJS) : $(MAKEFILE) rtw_proj.tmw
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c
+1 -1
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter_terminate'
*
* C source code generated on: Wed Jul 11 08:38:35 2012
* C source code generated on: Fri Sep 21 13:56:44 2012
*
*/
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h
+3 -1
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter_terminate'
*
* C source code generated on: Wed Jul 11 08:38:35 2012
* C source code generated on: Fri Sep 21 13:56:44 2012
*
*/
@@ -14,6 +14,8 @@
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "rt_defines.h"
#include "rt_nonfinite.h"
#include "rtwtypes.h"
#include "attitudeKalmanfilter_types.h"
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h
+1 -1
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Wed Jul 11 08:38:35 2012
* C source code generated on: Fri Sep 21 13:56:42 2012
*
*/
apps/attitude_estimator_ekf/codegen/buildInfo.mat
BIN
View File
Binary file not shown.
apps/attitude_estimator_ekf/codegen/cross.c
Executable
+37
View File
@@ -0,0 +1,37 @@
/*
* cross.c
*
* Code generation for function 'cross'
*
* C source code generated on: Fri Sep 21 13:56:44 2012
*
*/
/* Include files */
#include "rt_nonfinite.h"
#include "attitudeKalmanfilter.h"
#include "cross.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
/* Function Definitions */
/*
*
*/
void cross(const real32_T a[3], const real32_T b[3], real32_T c[3])
{
c[0] = a[1] * b[2] - a[2] * b[1];
c[1] = a[2] * b[0] - a[0] * b[2];
c[2] = a[0] * b[1] - a[1] * b[0];
}
/* End of code generation (cross.c) */
apps/attitude_estimator_ekf/codegen/cross.h
Executable
+34
View File
@@ -0,0 +1,34 @@
/*
* cross.h
*
* Code generation for function 'cross'
*
* C source code generated on: Fri Sep 21 13:56:44 2012
*
*/
#ifndef __CROSS_H__
#define __CROSS_H__
/* Include files */
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "rt_defines.h"
#include "rt_nonfinite.h"
#include "rtwtypes.h"
#include "attitudeKalmanfilter_types.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
extern void cross(const real32_T a[3], const real32_T b[3], real32_T c[3]);
#endif
/* End of code generation (cross.h) */
apps/attitude_estimator_ekf/codegen/diag.c
Executable
+42
View File
@@ -0,0 +1,42 @@
/*
* diag.c
*
* Code generation for function 'diag'
*
* C source code generated on: Fri Sep 21 13:56:43 2012
*
*/
/* Include files */
#include "rt_nonfinite.h"
#include "attitudeKalmanfilter.h"
#include "diag.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
/* Function Definitions */
/*
*
*/
void diag(const real32_T v[3], real32_T d[9])
{
int32_T j;
for (j = 0; j < 9; j++) {
d[j] = 0.0F;
}
for (j = 0; j < 3; j++) {
d[j + 3 * j] = v[j];
}
}
/* End of code generation (diag.c) */
apps/attitude_estimator_ekf/codegen/diag.h
Executable
+34
View File
@@ -0,0 +1,34 @@
/*
* diag.h
*
* Code generation for function 'diag'
*
* C source code generated on: Fri Sep 21 13:56:43 2012
*
*/
#ifndef __DIAG_H__
#define __DIAG_H__
/* Include files */
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "rt_defines.h"
#include "rt_nonfinite.h"
#include "rtwtypes.h"
#include "attitudeKalmanfilter_types.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
extern void diag(const real32_T v[3], real32_T d[9]);
#endif
/* End of code generation (diag.h) */
apps/attitude_estimator_ekf/codegen/eye.c
+5 -5
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'eye'
*
* C source code generated on: Wed Jul 11 08:38:35 2012
* C source code generated on: Fri Sep 21 13:56:43 2012
*
*/
@@ -27,12 +27,12 @@
/*
*
*/
void b_eye(real_T I[144])
void b_eye(real_T I[81])
{
int32_T i;
memset((void *)&I[0], 0, 144U * sizeof(real_T));
for (i = 0; i < 12; i++) {
I[i + 12 * i] = 1.0;
memset((void *)&I[0], 0, 81U * sizeof(real_T));
for (i = 0; i < 9; i++) {
I[i + 9 * i] = 1.0;
}
}
apps/attitude_estimator_ekf/codegen/eye.h
+4 -2
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'eye'
*
* C source code generated on: Wed Jul 11 08:38:35 2012
* C source code generated on: Fri Sep 21 13:56:43 2012
*
*/
@@ -14,6 +14,8 @@
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "rt_defines.h"
#include "rt_nonfinite.h"
#include "rtwtypes.h"
#include "attitudeKalmanfilter_types.h"
@@ -27,7 +29,7 @@
/* Variable Definitions */
/* Function Declarations */
extern void b_eye(real_T I[144]);
extern void b_eye(real_T I[81]);
extern void eye(real_T I[9]);
#endif
/* End of code generation (eye.h) */
apps/attitude_estimator_ekf/codegen/find.c
Executable
+77
View File
@@ -0,0 +1,77 @@
/*
* find.c
*
* Code generation for function 'find'
*
* C source code generated on: Fri Sep 21 13:56:43 2012
*
*/
/* Include files */
#include "rt_nonfinite.h"
#include "attitudeKalmanfilter.h"
#include "find.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
/* Function Definitions */
/*
*
*/
void find(const int8_T x[9], real_T i_data[9], int32_T i_sizes[1])
{
int32_T idx;
int32_T ii;
boolean_T exitg1;
boolean_T guard1 = FALSE;
int32_T i2;
int8_T b_i_data[9];
idx = 0;
i_sizes[0] = 9;
ii = 1;
exitg1 = 0U;
while ((exitg1 == 0U) && (ii <= 9)) {
guard1 = FALSE;
if (x[ii - 1] != 0) {
idx++;
i_data[idx - 1] = (real_T)ii;
if (idx >= 9) {
exitg1 = 1U;
} else {
guard1 = TRUE;
}
} else {
guard1 = TRUE;
}
if (guard1 == TRUE) {
ii++;
}
}
if (1 > idx) {
idx = 0;
}
ii = idx - 1;
for (i2 = 0; i2 <= ii; i2++) {
b_i_data[i2] = (int8_T)i_data[i2];
}
i_sizes[0] = idx;
ii = idx - 1;
for (i2 = 0; i2 <= ii; i2++) {
i_data[i2] = (real_T)b_i_data[i2];
}
}
/* End of code generation (find.c) */
apps/attitude_estimator_ekf/codegen/find.h
Executable
+34
View File
@@ -0,0 +1,34 @@
/*
* find.h
*
* Code generation for function 'find'
*
* C source code generated on: Fri Sep 21 13:56:43 2012
*
*/
#ifndef __FIND_H__
#define __FIND_H__
/* Include files */
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "rt_defines.h"
#include "rt_nonfinite.h"
#include "rtwtypes.h"
#include "attitudeKalmanfilter_types.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
extern void find(const int8_T x[9], real_T i_data[9], int32_T i_sizes[1]);
#endif
/* End of code generation (find.h) */
apps/attitude_estimator_ekf/codegen/mrdivide.c
+667 -84
View File
File diff suppressed because it is too large Load Diff
apps/attitude_estimator_ekf/codegen/mrdivide.h
+4 -2
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'mrdivide'
*
* C source code generated on: Wed Jul 11 08:38:35 2012
* C source code generated on: Fri Sep 21 13:56:44 2012
*
*/
@@ -14,6 +14,8 @@
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "rt_defines.h"
#include "rt_nonfinite.h"
#include "rtwtypes.h"
#include "attitudeKalmanfilter_types.h"
@@ -27,6 +29,6 @@
/* Variable Definitions */
/* Function Declarations */
extern void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108]);
extern void mrdivide(const real32_T A_data[81], const int32_T A_sizes[2], const real32_T B_data[81], const int32_T B_sizes[2], real32_T y_data[81], int32_T y_sizes[2]);
#endif
/* End of code generation (mrdivide.h) */
apps/attitude_estimator_ekf/codegen/norm.c
+3 -3
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'norm'
*
* C source code generated on: Wed Jul 11 08:38:35 2012
* C source code generated on: Fri Sep 21 13:56:43 2012
*
*/
@@ -40,7 +40,7 @@ real32_T norm(const real32_T x[3])
firstNonZero = TRUE;
for (k = 0; k < 3; k++) {
if (x[k] != 0.0F) {
absxk = fabsf(x[k]);
absxk = (real32_T)fabsf(x[k]);
if (firstNonZero) {
scale = absxk;
y = 1.0F;
@@ -56,7 +56,7 @@ real32_T norm(const real32_T x[3])
}
}
return scale * sqrtf(y);
return scale * (real32_T)sqrtf(y);
}
/* End of code generation (norm.c) */
apps/attitude_estimator_ekf/codegen/norm.h
+3 -1
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'norm'
*
* C source code generated on: Wed Jul 11 08:38:35 2012
* C source code generated on: Fri Sep 21 13:56:44 2012
*
*/
@@ -14,6 +14,8 @@
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "rt_defines.h"
#include "rt_nonfinite.h"
#include "rtwtypes.h"
#include "attitudeKalmanfilter_types.h"
apps/attitude_estimator_ekf/codegen/rtGetInf.c
+1 -1
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Wed Jul 11 08:38:35 2012
* C source code generated on: Fri Sep 21 13:56:45 2012
*
*/
apps/attitude_estimator_ekf/codegen/rtGetInf.h
+1 -1
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Wed Jul 11 08:38:35 2012
* C source code generated on: Fri Sep 21 13:56:45 2012
*
*/
apps/attitude_estimator_ekf/codegen/rtGetNaN.c
+1 -1
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Wed Jul 11 08:38:35 2012
* C source code generated on: Fri Sep 21 13:56:45 2012
*
*/
apps/attitude_estimator_ekf/codegen/rtGetNaN.h
+1 -1
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Wed Jul 11 08:38:35 2012
* C source code generated on: Fri Sep 21 13:56:44 2012
*
*/
apps/attitude_estimator_ekf/codegen/rt_defines.h
Executable
+24
View File
@@ -0,0 +1,24 @@
/*
* rt_defines.h
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Fri Sep 21 13:56:44 2012
*
*/
#ifndef __RT_DEFINES_H__
#define __RT_DEFINES_H__
#include <stdlib.h>
#define RT_PI 3.14159265358979323846
#define RT_PIF 3.1415927F
#define RT_LN_10 2.30258509299404568402
#define RT_LN_10F 2.3025851F
#define RT_LOG10E 0.43429448190325182765
#define RT_LOG10EF 0.43429449F
#define RT_E 2.7182818284590452354
#define RT_EF 2.7182817F
#endif
/* End of code generation (rt_defines.h) */
apps/attitude_estimator_ekf/codegen/rt_nonfinite.c
+1 -1
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Wed Jul 11 08:38:35 2012
* C source code generated on: Fri Sep 21 13:56:44 2012
*
*/
apps/attitude_estimator_ekf/codegen/rt_nonfinite.h
+1 -1
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Wed Jul 11 08:38:35 2012
* C source code generated on: Fri Sep 21 13:56:44 2012
*
*/
apps/attitude_estimator_ekf/codegen/rtw_proj.tmw
-1
View File
@@ -1 +0,0 @@
Code generation project for attitudeKalmanfilter using C:\Program Files\MATLAB\R2011a\toolbox\coder\coder\rtw\c\xrt\xrt_vcx64.tmf. MATLAB root = C:\Program Files\MATLAB\R2011a.
apps/attitude_estimator_ekf/codegen/rtwtypes.h
+1 -1
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Wed Jul 11 08:38:35 2012
* C source code generated on: Fri Sep 21 13:56:44 2012
*
*/
apps/commander/commander.c
+261 -246
View File
File diff suppressed because it is too large Load Diff
apps/commander/state_machine_helper.c
+2 -1
View File
@@ -223,7 +223,8 @@ void publish_armed_status(const struct vehicle_status_s *current_status) {
/* lock down actuators if required */
// XXX FIXME Currently any loss of RC will completely disable all actuators
// needs proper failsafe
armed.lockdown = (current_status->rc_signal_lost || current_status->flag_hil_enabled) ? true : false;
armed.lockdown = ((current_status->rc_signal_lost && current_status->offboard_control_signal_lost)
|| current_status->flag_hil_enabled) ? true : false;
orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
}
apps/fixedwing_control/fixedwing_control.c
+301 -772
View File
File diff suppressed because it is too large Load Diff
apps/mavlink/mavlink.c
+142 -59
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);
apps/multirotor_att_control/multirotor_att_control_main.c
+129 -45
View File
@@ -1,7 +1,7 @@
/****************************************************************************
*
* Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
* Author: @author Lorenz Meier <lm@inf.ethz.ch>
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: Lorenz Meier <lm@inf.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -36,6 +36,8 @@
* @file multirotor_att_control_main.c
*
* Implementation of multirotor attitude control main loop.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
*/
#include <nuttx/config.h>
@@ -54,36 +56,92 @@
#include <sys/prctl.h>
#include <arch/board/up_hrt.h>
#include <uORB/uORB.h>
#include <drivers/drv_gyro.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/ardrone_motors_setpoint.h>
#include <uORB/topics/offboard_control_setpoint.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/actuator_controls.h>
#include <systemlib/perf_counter.h>
#include "multirotor_attitude_control.h"
#include "multirotor_rate_control.h"
__EXPORT int multirotor_att_control_main(int argc, char *argv[]);
static enum {
CONTROL_MODE_RATES = 0,
CONTROL_MODE_ATTITUDE = 1,
} control_mode;
static bool thread_should_exit;
static int mc_task;
static bool motor_test_mode = false;
static orb_advert_t actuator_pub;
static struct vehicle_status_s state;
/**
* Perform rate control right after gyro reading
*/
static void *rate_control_thread_main(void *arg)
{
prctl(PR_SET_NAME, "mc rate control", getpid());
struct actuator_controls_s actuators;
int gyro_sub = orb_subscribe(ORB_ID(sensor_gyro));
int rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
struct pollfd fds = { .fd = gyro_sub, .events = POLLIN };
struct gyro_report gyro_report;
struct vehicle_rates_setpoint_s rates_sp;
memset(&rates_sp, 0, sizeof(rates_sp));
float gyro_lp[3] = {0.0f, 0.0f, 0.0f};
while (!thread_should_exit) {
/* rate control at maximum rate */
/* wait for a sensor update, check for exit condition every 1000 ms */
int ret = poll(&fds, 1, 1000);
if (ret < 0) {
/* XXX this is seriously bad - should be an emergency */
} else if (ret == 0) {
/* XXX this means no sensor data - should be critical or emergency */
printf("[mc att control] WARNING: Not getting gyro data, no rate control\n");
} else {
/* get data */
orb_copy(ORB_ID(sensor_gyro), gyro_sub, &gyro_report);
bool rates_sp_valid = false;
orb_check(rates_sp_sub, &rates_sp_valid);
if (rates_sp_valid) {
orb_copy(ORB_ID(vehicle_rates_setpoint), rates_sp_sub, &rates_sp);
}
/* perform local lowpass */
/* apply controller */
if (state.flag_control_rates_enabled) {
/* lowpass gyros */
// XXX
gyro_lp[0] = gyro_report.x;
gyro_lp[1] = gyro_report.y;
gyro_lp[2] = gyro_report.z;
multirotor_control_rates(&rates_sp, gyro_lp, &actuators);
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
}
}
}
return NULL;
}
static int
mc_thread_main(int argc, char *argv[])
{
/* declare and safely initialize all structs */
struct vehicle_status_s state;
memset(&state, 0, sizeof(state));
struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att));
@@ -93,18 +151,20 @@ mc_thread_main(int argc, char *argv[])
memset(&manual, 0, sizeof(manual));
struct sensor_combined_s raw;
memset(&raw, 0, sizeof(raw));
struct ardrone_motors_setpoint_s setpoint;
memset(&setpoint, 0, sizeof(setpoint));
struct offboard_control_setpoint_s offboard_sp;
memset(&offboard_sp, 0, sizeof(offboard_sp));
struct vehicle_rates_setpoint_s rates_sp;
memset(&rates_sp, 0, sizeof(rates_sp));
struct actuator_controls_s actuators;
/* subscribe to attitude, motor setpoints and system state */
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
int setpoint_sub = orb_subscribe(ORB_ID(offboard_control_setpoint));
int state_sub = orb_subscribe(ORB_ID(vehicle_status));
int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
int sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
int setpoint_sub = orb_subscribe(ORB_ID(ardrone_motors_setpoint));
/*
* Do not rate-limit the loop to prevent aliasing
@@ -121,8 +181,9 @@ mc_thread_main(int argc, char *argv[])
actuators.control[i] = 0.0f;
}
orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators);
actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators);
orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
orb_advert_t rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
/* register the perf counter */
perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "multirotor_att_control");
@@ -130,6 +191,13 @@ mc_thread_main(int argc, char *argv[])
/* welcome user */
printf("[multirotor_att_control] starting\n");
/* ready, spawn pthread */
pthread_attr_t rate_control_attr;
pthread_attr_init(&rate_control_attr);
pthread_attr_setstacksize(&rate_control_attr, 2048);
pthread_t rate_control_thread;
pthread_create(&rate_control_thread, &rate_control_attr, rate_control_thread_main, NULL);
while (!thread_should_exit) {
/* wait for a sensor update, check for exit condition every 500 ms */
@@ -138,13 +206,22 @@ mc_thread_main(int argc, char *argv[])
perf_begin(mc_loop_perf);
/* get a local copy of system state */
orb_copy(ORB_ID(vehicle_status), state_sub, &state);
bool updated;
orb_check(state_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_status), state_sub, &state);
}
/* get a local copy of manual setpoint */
orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual);
/* get a local copy of attitude */
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
/* get a local copy of attitude setpoint */
orb_copy(ORB_ID(vehicle_attitude_setpoint), att_setpoint_sub, &att_sp);
/* get a local copy of rates setpoint */
orb_check(setpoint_sub, &updated);
if (updated) {
orb_copy(ORB_ID(offboard_control_setpoint), setpoint_sub, &offboard_sp);
}
/* get a local copy of the current sensor values */
orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw);
@@ -155,41 +232,57 @@ mc_thread_main(int argc, char *argv[])
/* manual inputs, from RC control or joystick */
att_sp.roll_body = manual.roll;
att_sp.pitch_body = manual.pitch;
att_sp.yaw_rate_body = manual.yaw;
att_sp.yaw_body = manual.yaw; // XXX Hack, remove, switch to yaw rate controller
/* set yaw rate */
rates_sp.yaw = manual.yaw;
att_sp.thrust = manual.throttle;
att_sp.timestamp = hrt_absolute_time();
/* STEP 2: publish the result to the vehicle actuators */
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
if (motor_test_mode) {
att_sp.roll_body = 0.0f;
att_sp.pitch_body = 0.0f;
att_sp.yaw_body = 0.0f;
att_sp.roll_rate_body = 0.0f;
att_sp.pitch_rate_body = 0.0f;
att_sp.yaw_rate_body = 0.0f;
att_sp.thrust = 0.1f;
} else {
att_sp.thrust = manual.throttle;
att_sp.timestamp = hrt_absolute_time();
/* STEP 2: publish the result to the vehicle actuators */
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
}
} else if (state.flag_control_offboard_enabled) {
/* offboard inputs */
if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES) {
rates_sp.roll = offboard_sp.p1;
rates_sp.pitch = offboard_sp.p2;
rates_sp.yaw = offboard_sp.p3;
rates_sp.thrust = offboard_sp.p4;
rates_sp.timestamp = hrt_absolute_time();
orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
} else if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE) {
att_sp.roll_body = offboard_sp.p1;
att_sp.pitch_body = offboard_sp.p2;
att_sp.yaw_body = offboard_sp.p3;
att_sp.thrust = offboard_sp.p4;
att_sp.timestamp = hrt_absolute_time();
/* STEP 2: publish the result to the vehicle actuators */
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
}
/* decide wether we want rate or position input */
}
/** STEP 2: Identify the controller setup to run and set up the inputs correctly */
/** STEP 3: Identify the controller setup to run and set up the inputs correctly */
/* run attitude controller */
if (state.flag_control_attitude_enabled) {
multirotor_control_attitude(&att_sp, &att, &actuators);
if (state.flag_control_attitude_enabled && !state.flag_control_rates_enabled) {
multirotor_control_attitude(&att_sp, &att, NULL, &actuators);
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
} else if (state.flag_control_attitude_enabled && state.flag_control_rates_enabled) {
multirotor_control_attitude(&att_sp, &att, &rates_sp, NULL);
orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
}
/* XXX could be also run in an independent loop */
if (state.flag_control_rates_enabled) {
multirotor_control_rates(&att_sp, &raw.gyro_rad_s, &actuators);
}
/* STEP 3: publish the result to the vehicle actuators */
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
perf_end(mc_loop_perf);
}
@@ -211,6 +304,8 @@ mc_thread_main(int argc, char *argv[])
perf_print_counter(mc_loop_perf);
perf_free(mc_loop_perf);
pthread_join(rate_control_thread, NULL);
fflush(stdout);
exit(0);
}
@@ -229,22 +324,10 @@ usage(const char *reason)
int multirotor_att_control_main(int argc, char *argv[])
{
int ch;
control_mode = CONTROL_MODE_RATES;
unsigned int optioncount = 0;
while ((ch = getopt(argc, argv, "tm:")) != EOF) {
switch (ch) {
case 'm':
if (!strcmp(optarg, "rates")) {
control_mode = CONTROL_MODE_RATES;
} else if (!strcmp(optarg, "attitude")) {
control_mode = CONTROL_MODE_RATES;
} else {
usage("unrecognized -m value");
}
optioncount += 2;
break;
case 't':
motor_test_mode = true;
optioncount += 1;
@@ -255,6 +338,7 @@ int multirotor_att_control_main(int argc, char *argv[])
default:
fprintf(stderr, "option: -%c\n", ch);
usage("unrecognized option");
break;
}
}
argc -= optioncount;
apps/multirotor_att_control/multirotor_attitude_control.c
+19 -9
View File
@@ -188,7 +188,7 @@ static int parameters_update(const struct mc_att_control_param_handles *h, struc
}
void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
const struct vehicle_attitude_s *att, struct actuator_controls_s *actuators)
const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, struct actuator_controls_s *actuators)
{
static uint64_t last_run = 0;
const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
@@ -214,11 +214,11 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
// pid_init(&yaw_pos_controller, p.yaw_p, p.yaw_i, p.yaw_d, p.yaw_awu,
// PID_MODE_DERIVATIV_SET, 154);
pid_init(&yaw_speed_controller, p.yawrate_p, p.yawrate_i, p.yawrate_d, p.yawrate_awu,
PID_MODE_DERIVATIV_SET, 155);
PID_MODE_DERIVATIV_SET);
pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, p.att_awu,
PID_MODE_DERIVATIV_SET, 156);
PID_MODE_DERIVATIV_SET);
pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, p.att_awu,
PID_MODE_DERIVATIV_SET, 157);
PID_MODE_DERIVATIV_SET);
initialized = true;
}
@@ -243,7 +243,7 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
float roll_control = pid_calculate(&roll_controller, att_sp->roll_body + p.att_yoff,
att->roll, att->rollspeed, deltaT);
/* control yaw rate */
float yaw_rate_control = pid_calculate(&yaw_speed_controller, att_sp->yaw_rate_body, att->yawspeed, 0.0f, deltaT);
float yaw_rate_control = pid_calculate(&yaw_speed_controller, att_sp->yaw_body, att->yawspeed, 0.0f, deltaT);
/*
* compensate the vertical loss of thrust
@@ -305,10 +305,20 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
roll_controller.saturated = 1;
}
actuators->control[0] = roll_control;
actuators->control[1] = pitch_control;
actuators->control[2] = yaw_rate_control;
actuators->control[3] = motor_thrust;
if (actuators) {
actuators->control[0] = roll_control;
actuators->control[1] = pitch_control;
actuators->control[2] = yaw_rate_control;
actuators->control[3] = motor_thrust;
}
// XXX change yaw rate to yaw pos controller
if (rates_sp) {
rates_sp->roll = roll_control;
rates_sp->pitch = pitch_control;
rates_sp->yaw = yaw_rate_control;
rates_sp->thrust = motor_thrust;
}
motor_skip_counter++;
}
apps/multirotor_att_control/multirotor_attitude_control.h
+3 -2
View File
@@ -48,9 +48,10 @@
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/actuator_controls.h>
void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att,
struct actuator_controls_s *actuators);
void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, struct actuator_controls_s *actuators);
#endif /* MULTIROTOR_ATTITUDE_CONTROL_H_ */
apps/multirotor_att_control/multirotor_rate_control.c
+7 -7
View File
@@ -132,7 +132,7 @@ static int parameters_update(const struct mc_rate_control_param_handles *h, stru
return OK;
}
void multirotor_control_rates(const struct vehicle_attitude_setpoint_s *rate_sp,
void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
const float rates[], struct actuator_controls_s *actuators)
{
static uint64_t last_run = 0;
@@ -157,11 +157,11 @@ void multirotor_control_rates(const struct vehicle_attitude_setpoint_s *rate_sp,
parameters_update(&h, &p);
pid_init(&yaw_speed_controller, p.yawrate_p, 0, p.yawrate_i, p.yawrate_awu,
PID_MODE_DERIVATIV_SET, 155);
PID_MODE_DERIVATIV_SET);
pid_init(&pitch_controller, p.attrate_p, p.attrate_i, 0, p.attrate_awu,
PID_MODE_DERIVATIV_SET, 156);
PID_MODE_DERIVATIV_SET);
pid_init(&roll_controller, p.attrate_p, p.attrate_i, 0, p.attrate_awu,
PID_MODE_DERIVATIV_SET, 157);
PID_MODE_DERIVATIV_SET);
initialized = true;
}
@@ -179,13 +179,13 @@ void multirotor_control_rates(const struct vehicle_attitude_setpoint_s *rate_sp,
/* calculate current control outputs */
/* control pitch (forward) output */
float pitch_control = pid_calculate(&pitch_controller, rate_sp->pitch_rate_body,
float pitch_control = pid_calculate(&pitch_controller, rate_sp->pitch,
rates[1], 0.0f, deltaT);
/* control roll (left/right) output */
float roll_control = pid_calculate(&roll_controller, rate_sp->roll_rate_body,
float roll_control = pid_calculate(&roll_controller, rate_sp->roll,
rates[0], 0.0f, deltaT);
/* control yaw rate */
float yaw_rate_control = pid_calculate(&yaw_speed_controller, rate_sp->yaw_rate_body, rates[2], 0.0f, deltaT);
float yaw_rate_control = pid_calculate(&yaw_speed_controller, rate_sp->yaw, rates[2], 0.0f, deltaT);
/*
* compensate the vertical loss of thrust
apps/multirotor_att_control/multirotor_rate_control.h
+2 -2
View File
@@ -47,10 +47,10 @@
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/actuator_controls.h>
void multirotor_control_rates(const struct vehicle_attitude_setpoint_s *rate_sp,
void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
const float rates[], struct actuator_controls_s *actuators);
#endif /* MULTIROTOR_RATE_CONTROL_H_ */
apps/multirotor_pos_control/position_control.c
-40
View File
@@ -51,46 +51,6 @@
#include "multirotor_position_control.h"
float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next)
{
double lat_now_rad = lat_now / 180.0 * M_PI;
double lon_now_rad = lon_now / 180.0 * M_PI;
double lat_next_rad = lat_next / 180.0 * M_PI;
double lon_next_rad = lon_next / 180.0 * M_PI;
double d_lat = lat_next_rad - lat_now_rad;
double d_lon = lon_next_rad - lon_now_rad;
double a = sin(d_lat / 2.0) * sin(d_lat / 2.0) + sin(d_lon / 2.0) * sin(d_lon / 2.0) * cos(lat_now_rad) * cos(lat_next_rad);
double c = 2 * atan2(sqrt(a), sqrt(1 - a));
const double radius_earth = 6371000.0;
return radius_earth * c;
}
float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next)
{
double lat_now_rad = lat_now / 180.0 * M_PI;
double lon_now_rad = lon_now / 180.0 * M_PI;
double lat_next_rad = lat_next / 180.0 * M_PI;
double lon_next_rad = lon_next / 180.0 * M_PI;
double d_lat = lat_next_rad - lat_now_rad;
double d_lon = lon_next_rad - lon_now_rad;
/* conscious mix of double and float trig function to maximize speed and efficiency */
float theta = atan2f(sin(d_lon) * cos(lat_next_rad) , cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon));
// XXX wrapping check is incomplete
if (theta < 0.0f) {
theta = theta + 2.0f * M_PI_F;
}
return theta;
}
void control_multirotor_position(const struct vehicle_state_s *vstatus, const struct vehicle_manual_control_s *manual,
const struct vehicle_attitude_s *att, const struct vehicle_local_position_s *local_pos,
const struct vehicle_local_position_setpoint_s *local_pos_sp, struct vehicle_attitude_setpoint_s *att_sp)
apps/px4/ground_estimator/ground_estimator.c
+2 -2
View File
@@ -75,7 +75,7 @@ int ground_estimator_thread_main(int argc, char *argv[]) {
/* subscribe to raw data */
int sub_raw = orb_subscribe(ORB_ID(sensor_combined));
/* advertise attitude */
/* advertise debug value */
struct debug_key_value_s dbg = { .key = "posx", .value = 0.0f };
orb_advert_t pub_dbg = orb_advertise(ORB_ID(debug_key_value), &dbg);
@@ -97,7 +97,7 @@ int ground_estimator_thread_main(int argc, char *argv[]) {
/* XXX this is seriously bad - should be an emergency */
} else if (ret == 0) {
/* XXX this means no sensor data - should be critical or emergency */
printf("[attitude estimator bm] WARNING: Not getting sensor data - sensor app running?\n");
printf("[ground estimator bm] WARNING: Not getting sensor data - sensor app running?\n");
} else {
struct sensor_combined_s s;
orb_copy(ORB_ID(sensor_combined), sub_raw, &s);
apps/sdlog/sdlog.c
+3 -1
View File
@@ -289,6 +289,8 @@ int sdlog_thread_main(int argc, char *argv[]) {
/* subscribe to ORB for sensors raw */
subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
fds[fdsc_count].fd = subs.sensor_sub;
/* rate-limit raw data updates to 200Hz */
orb_set_interval(subs.sensor_sub, 5);
fds[fdsc_count].events = POLLIN;
fdsc_count++;
@@ -459,7 +461,7 @@ int sdlog_thread_main(int argc, char *argv[]) {
sysvector_bytes += write(sysvector_file, (const char*)&sysvector, sizeof(sysvector));
usleep(4900);
usleep(10000);
}
unsigned bytes = sensor_combined_bytes + actuator_outputs_bytes + blackbox_file_bytes + actuator_controls_bytes;
apps/sensors/sensors.cpp
+7 -4
View File
@@ -567,7 +567,7 @@ Sensors::accel_init()
_fd_bma180 = open("/dev/bma180", O_RDONLY);
if (_fd_bma180 < 0) {
warn("/dev/bma180");
errx(1, "FATAL: no accelerometer found");
warn("FATAL: no accelerometer found");
}
/* discard first (junk) reading */
@@ -600,7 +600,7 @@ Sensors::gyro_init()
_fd_gyro_l3gd20 = open("/dev/l3gd20", O_RDONLY);
if (_fd_gyro_l3gd20 < 0) {
warn("/dev/l3gd20");
errx(1, "FATAL: no gyro found");
warn("FATAL: no gyro found");
}
/* discard first (junk) reading */
@@ -968,7 +968,7 @@ Sensors::ppm_poll()
_rc.chan[i].scaled = (ppm_buffer[i] - _parameters.trim[i]) / (float)(_parameters.max[i] - _parameters.trim[i]);
} else if (ppm_buffer[i] < (_parameters.trim[i] - _parameters.dz[i])) {
/* division by zero impossible for trim == min (as for throttle), as this falls in the above if clause */
_rc.chan[i].scaled = -1.0f + ((ppm_buffer[i] - _parameters.min[i]) / (float)(_parameters.trim[i] - _parameters.min[i]));
_rc.chan[i].scaled = -((_parameters.trim[i] - ppm_buffer[i]) / (float)(_parameters.trim[i] - _parameters.min[i]));
} else {
/* in the configured dead zone, output zero */
@@ -988,6 +988,8 @@ Sensors::ppm_poll()
_rc.chan_count = ppm_decoded_channels;
_rc.timestamp = ppm_last_valid_decode;
manual_control.timestamp = ppm_last_valid_decode;
/* roll input - rolling right is stick-wise and rotation-wise positive */
manual_control.roll = _rc.chan[_rc.function[ROLL]].scaled;
if (manual_control.roll < -1.0f) manual_control.roll = -1.0f;
@@ -1027,6 +1029,7 @@ Sensors::ppm_poll()
orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc);
orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual_control);
}
#endif
@@ -1090,7 +1093,7 @@ Sensors::task_main()
/* advertise the manual_control topic */
{
struct manual_control_setpoint_s manual_control;
manual_control.mode = ROLLPOS_PITCHPOS_YAWRATE_THROTTLE;
manual_control.mode = MANUAL_CONTROL_MODE_ATT_YAW_RATE;
manual_control.roll = 0.0f;
manual_control.pitch = 0.0f;
manual_control.yaw = 0.0f;

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