zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Fixes and style, deamonized filter

sbg
Lorenz Meier 13 years ago
parent
dbd6cbea60 855fbe8543
commit
572efc3383
5 changed files with 166 additions and 204 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 99
      apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
  2. 28
      apps/examples/px4_deamon_app/px4_deamon_app.c
  3. 79
      apps/fixedwing_control/fixedwing_control.c
  4. 118
      apps/fixedwing_control/pid.c
  5. 46
      apps/fixedwing_control/pid.h

99
apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
Unescape View File

@ -87,9 +87,6 @@ static float P_aposteriori_k[81] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 100.f, 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, 100.f, 0,
0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 100.f,
0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0,
}; };
static float P_aposteriori[81] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0, 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, 100.f, 0, 0, 0, 0, 0, 0, 0,
@ -100,9 +97,6 @@ static float P_aposteriori[81] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 100.f, 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, 100.f, 0,
0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 100.f,
0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0,
}; /**< init: diagonal matrix with big values */ }; /**< init: diagonal matrix with big values */
static float knownConst[20] = {1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}; /**< knownConst has the following entries [PrvaA,PrvarM,PrvarWO,PrvarW||MsvarA,MsvarM,MsvarW] */ static float knownConst[20] = {1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}; /**< knownConst has the following entries [PrvaA,PrvarM,PrvarWO,PrvarW||MsvarA,MsvarM,MsvarW] */
static float Rot_matrix[9] = {1.f, 0, 0, static float Rot_matrix[9] = {1.f, 0, 0,
@ -110,6 +104,73 @@ static float Rot_matrix[9] = {1.f, 0, 0,
0, 0, 1.f 0, 0, 1.f
}; /**< init: identity matrix */ }; /**< init: identity matrix */
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, 4096, 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) * [Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst)
*/ */
@ -122,7 +183,7 @@ static float Rot_matrix[9] = {1.f, 0, 0,
* @param argc number of commandline arguments (plus command name) * @param argc number of commandline arguments (plus command name)
* @param argv strings containing the arguments * @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 // print text
printf("Extended Kalman Filter Attitude Estimator initialized..\n\n"); printf("Extended Kalman Filter Attitude Estimator initialized..\n\n");
@ -136,8 +197,8 @@ int attitude_estimator_ekf_main(int argc, char *argv[])
/* store start time to guard against too slow update rates */ /* store start time to guard against too slow update rates */
uint64_t last_run = hrt_absolute_time(); uint64_t last_run = hrt_absolute_time();
struct sensor_combined_s raw = {0}; struct sensor_combined_s raw;
struct vehicle_attitude_s att = {}; struct vehicle_attitude_s att;
uint64_t last_data = 0; uint64_t last_data = 0;
uint64_t last_measurement = 0; uint64_t last_measurement = 0;
@ -151,8 +212,10 @@ int attitude_estimator_ekf_main(int argc, char *argv[])
int loopcounter = 0; int loopcounter = 0;
int printcounter = 0; int printcounter = 0;
thread_running = true;
/* Main loop*/ /* Main loop*/
while (true) { while (!thread_should_exit) {
struct pollfd fds[1] = { struct pollfd fds[1] = {
{ .fd = sub_raw, .events = POLLIN }, { .fd = sub_raw, .events = POLLIN },
@ -216,7 +279,7 @@ int attitude_estimator_ekf_main(int argc, char *argv[])
/* print rotation matrix every 200th time */ /* print rotation matrix every 200th time */
if (printcounter % 200 == 0) { 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("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", euler[0], euler[1], euler[2]); 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), 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[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)(Rot_matrix[6] * 100), (int)(Rot_matrix[7] * 100), (int)(Rot_matrix[8] * 100));
@ -230,9 +293,9 @@ int attitude_estimator_ekf_main(int argc, char *argv[])
/* send out */ /* send out */
att.timestamp = raw.timestamp; att.timestamp = raw.timestamp;
att.roll = euler.x; att.roll = euler[0];
att.pitch = euler.y; att.pitch = euler[1];
att.yaw = euler.z; att.yaw = euler[2];
// att.rollspeed = rates.x; // att.rollspeed = rates.x;
// att.pitchspeed = rates.y; // att.pitchspeed = rates.y;
@ -245,11 +308,7 @@ int attitude_estimator_ekf_main(int argc, char *argv[])
loopcounter++; loopcounter++;
} }
/* Should never reach here */ thread_running = false;
return 0; return 0;
} }

28
apps/examples/px4_deamon_app/px4_deamon_app.c
Unescape View File

@ -60,20 +60,6 @@ int px4_deamon_thread_main(int argc, char *argv[]);
*/ */
static void usage(const char *reason); static void usage(const char *reason);
int px4_deamon_thread_main(int argc, char *argv[]) {
printf("[deamon] starting\n");
while (!thread_should_exit) {
printf("Hello Deamon!\n");
sleep(10);
}
printf("[deamon] exiting.\n");
return 0;
}
static void static void
usage(const char *reason) usage(const char *reason)
{ {
@ -127,3 +113,17 @@ int px4_deamon_app_main(int argc, char *argv[])
usage("unrecognized command"); usage("unrecognized command");
exit(1); exit(1);
} }
int px4_deamon_thread_main(int argc, char *argv[]) {
printf("[deamon] starting\n");
while (!thread_should_exit) {
printf("Hello Deamon!\n");
sleep(10);
}
printf("[deamon] exiting.\n");
return 0;
}

79
apps/fixedwing_control/fixedwing_control.c
Unescape View File

@ -63,8 +63,79 @@
#include <uORB/topics/actuator_controls.h> #include <uORB/topics/actuator_controls.h>
#include <systemlib/param/param.h> #include <systemlib/param/param.h>
static bool thread_should_exit = false; /**< Deamon exit flag */
static bool thread_running = false; /**< Deamon status flag */
static int deamon_task; /**< Handle of deamon task / thread */
/**
* Deamon management function.
*/
__EXPORT int fixedwing_control_main(int argc, char *argv[]); __EXPORT int fixedwing_control_main(int argc, char *argv[]);
/**
* Mainloop of deamon.
*/
int fixedwing_control_thread_main(int argc, char *argv[]);
/**
* Print the correct usage.
*/
static void usage(const char *reason);
static void
usage(const char *reason)
{
if (reason)
fprintf(stderr, "%s\n", reason);
fprintf(stderr, "usage: fixedwing_control {start|stop|status}\n\n");
exit(1);
}
/**
* The deamon app only briefly exists to start
* the background job. The stack size assigned in the
* Makefile does only apply to this management task.
*
* The actual stack size should be set in the call
* to task_create().
*/
int fixedwing_control_main(int argc, char *argv[])
{
if (argc < 1)
usage("missing command");
if (!strcmp(argv[1], "start")) {
if (thread_running) {
printf("fixedwing_control already running\n");
/* this is not an error */
exit(0);
}
thread_should_exit = false;
deamon_task = task_create("fixedwing_control", SCHED_PRIORITY_MAX - 20, 4096, fixedwing_control_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL);
thread_running = true;
exit(0);
}
if (!strcmp(argv[1], "stop")) {
thread_should_exit = true;
exit(0);
}
if (!strcmp(argv[1], "status")) {
if (thread_running) {
printf("\tfixedwing_control is running\n");
} else {
printf("\tfixedwing_control not started\n");
}
exit(0);
}
usage("unrecognized command");
exit(1);
}
#define PID_DT 0.01f #define PID_DT 0.01f
#define PID_SCALER 0.1f #define PID_SCALER 0.1f
#define PID_DERIVMODE_CALC 0 #define PID_DERIVMODE_CALC 0
@ -614,14 +685,12 @@ static float calc_pitch_setpoint()
} }
/** /**
* calc_throttle_setpoint
* *
* Calculates the throttle setpoint for different flight modes * Calculates the throttle setpoint for different flight modes
* *
* @return throttle output setpoint * @return throttle output setpoint
* *
*/ */
static float calc_throttle_setpoint() static float calc_throttle_setpoint()
{ {
float setpoint = 0; float setpoint = 0;
@ -644,8 +713,7 @@ static float calc_throttle_setpoint()
return setpoint; return setpoint;
} }
/* /**
* fixedwing_control_main
* *
* @param argc number of arguments * @param argc number of arguments
* @param argv argument array * @param argv argument array
@ -653,8 +721,7 @@ static float calc_throttle_setpoint()
* @return 0 * @return 0
* *
*/ */
int fixedwing_control_thread_main(int argc, char *argv[])
int fixedwing_control_main(int argc, char *argv[])
{ {
/* default values for arguments */ /* default values for arguments */
char *fixedwing_uart_name = "/dev/ttyS1"; char *fixedwing_uart_name = "/dev/ttyS1";

118
apps/fixedwing_control/pid.c
Unescape View File

@ -1,118 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Ivan Ovinnikov <oivan@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 pid.c
* Implementation of a fixed wing attitude and position controller.
*/
#include "pid.h"
#include "fixedwing_control.h"
/*******************************************************************************
* pid()
*
* Calculates the PID control output given an error
*
* Input: float error, uint16_t dt, float scaler, float K_p, float K_i, float K_d
*
* Output: PID control value
*
******************************************************************************/
static float pid(float error, float error_deriv, uint16_t dt, float scaler, float K_p, float K_i, float K_d, float intmax)
{
// PID parameters
float Kp = K_p;
float Ki = K_i;
float Kd = K_d;
float delta_time = dt; // delta time
float lerror; // last error value
float imax = intmax; // max integral value
float integrator;
float derivative;
float lderiv;
int fCut = 20; // anything above 20 Hz is considered noise - low pass filter for the derivative
float output = 0; // the output of the PID controller
output += error * Kp;
if ((fabs(Kd) > 0) && (dt > 0)) {
if (PID_DERIVMODE_CALC) {
derivative = (error - lerror) / delta_time;
// discrete low pass filter, cuts out the
// high frequency noise that can drive the controller crazy
float RC = 1 / (2 * M_PI * fCut);
derivative = lderiv +
(delta_time / (RC + delta_time)) * (derivative - lderiv);
// update state
lerror = error;
lderiv = derivative;
} else {
derivative = error_deriv;
}
// add in derivative component
output += Kd * derivative;
}
printf("PID derivative %i\n", (int)(1000 * derivative));
// scale the P and D components
output *= scaler;
// Compute integral component if time has elapsed
if ((fabs(Ki) > 0) && (dt > 0)) {
integrator += (error * Ki) * scaler * delta_time;
if (integrator < -imax) {
integrator = -imax;
} else if (integrator > imax) {
integrator = imax;
}
output += integrator;
}
printf("PID Integrator %i\n", (int)(1000 * integrator));
return output;
}

46
apps/fixedwing_control/pid.h
Unescape View File

@ -1,46 +0,0 @@
/****************************************************************************
* pid.h
*
* Copyright (C) 2012 Ivan Ovinnikov. All rights reserved.
* Authors: Ivan Ovinnikov <oivan@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 NuttX 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.
*
****************************************************************************/
/****************************************************************************
* Included Files
****************************************************************************/
#ifndef PID_H_
#define PID_H_
static float pid(float error, float error_deriv, uint16_t dt, float scaler, float K_p, float K_i, float K_d, float intmax);
#endif /* PID_H_ */
Write Preview
Loading…
Cancel
Save