diff options
author | Julian Oes <julian@oes.ch> | 2013-07-08 10:31:32 +0200 |
---|---|---|
committer | Julian Oes <julian@oes.ch> | 2013-07-08 10:31:32 +0200 |
commit | 88389ea2554c6f56a4fdd86cdd86a1e7b6affc21 (patch) | |
tree | 3ef98ded43e5f3a2cdc30a836a3a7329fe5757bb /src/modules/systemlib | |
parent | 76346bfe19c816491a6982abfa10f48cd9d258f6 (diff) | |
parent | cf2dbdf9a1ae06c7d0e0a7963916a3709a1bc075 (diff) | |
download | px4-firmware-88389ea2554c6f56a4fdd86cdd86a1e7b6affc21.tar.gz px4-firmware-88389ea2554c6f56a4fdd86cdd86a1e7b6affc21.tar.bz2 px4-firmware-88389ea2554c6f56a4fdd86cdd86a1e7b6affc21.zip |
Merge branch 'master' into new_state_machine
compiling again
Conflicts:
src/modules/fixedwing_att_control/fixedwing_att_control_att.c
src/modules/fixedwing_att_control/fixedwing_att_control_rate.c
src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c
src/modules/mavlink/orb_listener.c
src/modules/multirotor_att_control/multirotor_attitude_control.c
src/modules/multirotor_att_control/multirotor_rate_control.c
src/modules/systemlib/pid/pid.c
src/modules/systemlib/pid/pid.h
src/modules/uORB/objects_common.cpp
Diffstat (limited to 'src/modules/systemlib')
-rw-r--r-- | src/modules/systemlib/conversions.h | 6 | ||||
-rw-r--r-- | src/modules/systemlib/cpuload.c | 54 | ||||
-rw-r--r-- | src/modules/systemlib/cpuload.h | 6 | ||||
-rw-r--r-- | src/modules/systemlib/err.c | 4 | ||||
-rw-r--r-- | src/modules/systemlib/geo/geo.h | 16 | ||||
-rw-r--r-- | src/modules/systemlib/pid/pid.c | 70 | ||||
-rw-r--r-- | src/modules/systemlib/pid/pid.h | 31 | ||||
-rw-r--r-- | src/modules/systemlib/systemlib.c | 6 | ||||
-rw-r--r-- | src/modules/systemlib/systemlib.h | 6 |
9 files changed, 108 insertions, 91 deletions
diff --git a/src/modules/systemlib/conversions.h b/src/modules/systemlib/conversions.h index 5d485b01f..064426f21 100644 --- a/src/modules/systemlib/conversions.h +++ b/src/modules/systemlib/conversions.h @@ -43,11 +43,7 @@ #define CONVERSIONS_H_ #include <float.h> #include <stdint.h> - -#define CONSTANTS_ONE_G 9.80665f // m/s^2 -#define CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C 1.225f // kg/m^3 -#define CONSTANTS_AIR_GAS_CONST 287.1f // J/(kg * K) -#define CONSTANTS_ABSOLUTE_NULL_CELSIUS -273.15f // °C +#include <systemlib/geo/geo.h> __BEGIN_DECLS diff --git a/src/modules/systemlib/cpuload.c b/src/modules/systemlib/cpuload.c index 20b711fa6..8fdff8ac0 100644 --- a/src/modules/systemlib/cpuload.c +++ b/src/modules/systemlib/cpuload.c @@ -1,9 +1,8 @@ /**************************************************************************** - * configs/px4fmu/src/up_leds.c - * arch/arm/src/board/up_leds.c * - * Copyright (C) 2011 Gregory Nutt. All rights reserved. - * Author: Gregory Nutt <gnutt@nuttx.org> + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier <lm@inf.ethz.ch> + * Petri Tanskanen <petri.tanskanen@inf.ethz.ch> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -15,7 +14,7 @@ * 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 + * 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. * @@ -34,18 +33,26 @@ * ****************************************************************************/ -/**************************************************************************** - * Included Files - ****************************************************************************/ - +/** + * @file cpuload.c + * + * Measurement of CPU load of each individual task. + * + * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch> + */ #include <nuttx/config.h> +#include <nuttx/sched.h> +#include <sys/types.h> #include <stdint.h> #include <stdbool.h> + +#include <arch/arch.h> + #include <debug.h> #include <sys/time.h> -#include <sched.h> #include <arch/board/board.h> #include <drivers/drv_hrt.h> @@ -54,26 +61,13 @@ #ifdef CONFIG_SCHED_INSTRUMENTATION -/**************************************************************************** - * Definitions - ****************************************************************************/ - - -/**************************************************************************** - * Public Functions - ****************************************************************************/ - -__EXPORT void sched_note_start(FAR _TCB *tcb); -__EXPORT void sched_note_stop(FAR _TCB *tcb); -__EXPORT void sched_note_switch(FAR _TCB *pFromTcb, FAR _TCB *pToTcb); - -/**************************************************************************** - * Name: - ****************************************************************************/ +__EXPORT void sched_note_start(FAR struct tcb_s *tcb); +__EXPORT void sched_note_stop(FAR struct tcb_s *tcb); +__EXPORT void sched_note_switch(FAR struct tcb_s *pFromTcb, FAR struct tcb_s *pToTcb); __EXPORT struct system_load_s system_load; -extern FAR _TCB *sched_gettcb(pid_t pid); +extern FAR struct _TCB *sched_gettcb(pid_t pid); void cpuload_initialize_once() { @@ -109,7 +103,7 @@ void cpuload_initialize_once() // } } -void sched_note_start(FAR _TCB *tcb) +void sched_note_start(FAR struct tcb_s *tcb) { /* search first free slot */ int i; @@ -128,7 +122,7 @@ void sched_note_start(FAR _TCB *tcb) } } -void sched_note_stop(FAR _TCB *tcb) +void sched_note_stop(FAR struct tcb_s *tcb) { int i; @@ -145,7 +139,7 @@ void sched_note_stop(FAR _TCB *tcb) } } -void sched_note_switch(FAR _TCB *pFromTcb, FAR _TCB *pToTcb) +void sched_note_switch(FAR struct tcb_s *pFromTcb, FAR struct tcb_s *pToTcb) { uint64_t new_time = hrt_absolute_time(); diff --git a/src/modules/systemlib/cpuload.h b/src/modules/systemlib/cpuload.h index a97047ea8..c7aa18d3c 100644 --- a/src/modules/systemlib/cpuload.h +++ b/src/modules/systemlib/cpuload.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -43,7 +43,7 @@ struct system_load_taskinfo_s { uint64_t total_runtime; ///< Runtime since start (start_time - total_runtime)/(start_time - current_time) = load uint64_t curr_start_time; ///< Start time of the current scheduling slot uint64_t start_time; ///< FIRST start time of task - FAR struct _TCB *tcb; ///< + FAR struct tcb_s *tcb; ///< bool valid; ///< Task is currently active / valid }; @@ -60,4 +60,6 @@ __EXPORT extern struct system_load_s system_load; __EXPORT void cpuload_initialize_once(void); +__END_DECLS + #endif diff --git a/src/modules/systemlib/err.c b/src/modules/systemlib/err.c index daf17ef8b..6c0e876d1 100644 --- a/src/modules/systemlib/err.c +++ b/src/modules/systemlib/err.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -61,7 +61,7 @@ const char * getprogname(void) { #if CONFIG_TASK_NAME_SIZE > 0 - _TCB *thisproc = sched_self(); + FAR struct tcb_s *thisproc = sched_self(); return thisproc->name; #else diff --git a/src/modules/systemlib/geo/geo.h b/src/modules/systemlib/geo/geo.h index 84097b49f..dadec51ec 100644 --- a/src/modules/systemlib/geo/geo.h +++ b/src/modules/systemlib/geo/geo.h @@ -45,9 +45,23 @@ * Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu> */ +#pragma once + +__BEGIN_DECLS #include <stdbool.h> +#define CONSTANTS_ONE_G 9.80665f /* m/s^2 */ +#define CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C 1.225f /* kg/m^3 */ +#define CONSTANTS_AIR_GAS_CONST 287.1f /* J/(kg * K) */ +#define CONSTANTS_ABSOLUTE_NULL_CELSIUS -273.15f /* °C */ +#define CONSTANTS_RADIUS_OF_EARTH 6371000 /* meters (m) */ + +/* compatibility aliases */ +#define RADIUS_OF_EARTH CONSTANTS_RADIUS_OF_EARTH +#define GRAVITY_MSS CONSTANTS_ONE_G + +// XXX remove struct crosstrack_error_s { bool past_end; // Flag indicating we are past the end of the line/arc segment float distance; // Distance in meters to closest point on line/arc @@ -111,3 +125,5 @@ __EXPORT float _wrap_180(float bearing); __EXPORT float _wrap_360(float bearing); __EXPORT float _wrap_pi(float bearing); __EXPORT float _wrap_2pi(float bearing); + +__END_DECLS diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c index 2aafe0636..9308100b0 100644 --- a/src/modules/systemlib/pid/pid.c +++ b/src/modules/systemlib/pid/pid.c @@ -1,9 +1,11 @@ /**************************************************************************** * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: @author Laurens Mackay <mackayl@student.ethz.ch> - * @author Tobias Naegeli <naegelit@student.ethz.ch> - * @author Martin Rutschmann <rutmarti@student.ethz.ch> + * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. + * Author: Laurens Mackay <mackayl@student.ethz.ch> + * Tobias Naegeli <naegelit@student.ethz.ch> + * Martin Rutschmann <rutmarti@student.ethz.ch> + * Anton Babushkin <anton.babushkin@me.com> + * Julian Oes <joes@student.ethz.ch> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -36,14 +38,21 @@ /** * @file pid.c - * Implementation of generic PID control interface + * + * Implementation of generic PID control interface. + * + * @author Laurens Mackay <mackayl@student.ethz.ch> + * @author Tobias Naegeli <naegelit@student.ethz.ch> + * @author Martin Rutschmann <rutmarti@student.ethz.ch> + * @author Anton Babushkin <anton.babushkin@me.com> + * @author Julian Oes <joes@student.ethz.ch> */ #include "pid.h" #include <math.h> __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, - float limit, float diff_filter_factor, uint8_t mode) + float limit, uint8_t mode, float dt_min) { pid->kp = kp; pid->ki = ki; @@ -51,17 +60,15 @@ __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, pid->intmax = intmax; pid->limit = limit; pid->mode = mode; - pid->diff_filter_factor = diff_filter_factor; + pid->dt_min = dt_min; pid->count = 0.0f; pid->saturated = 0.0f; pid->last_output = 0.0f; - pid->sp = 0.0f; - pid->error_previous_filtered = 0.0f; - pid->control_previous = 0.0f; + pid->error_previous = 0.0f; pid->integral = 0.0f; } -__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, float diff_filter_factor) +__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit) { int ret = 0; @@ -100,13 +107,6 @@ __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float ret = 1; } - if (isfinite(diff_filter_factor)) { - pid->diff_filter_factor = diff_filter_factor; - - } else { - ret = 1; - } - return ret; } @@ -144,20 +144,16 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo // Calculated current error value float error = pid->sp - val; - float error_filtered; // Calculate or measured current error derivative - if (pid->mode == PID_MODE_DERIVATIV_CALC) { + d = (error - pid->error_previous) / fmaxf(dt, pid->dt_min); + pid->error_previous = error; - error_filtered = pid->error_previous_filtered + (error - pid->error_previous_filtered) * pid->diff_filter_factor; - d = (error_filtered - pid->error_previous_filtered) / fmaxf(dt, 0.003f); // fail-safe for too low dt - pid->error_previous_filtered = error_filtered; } else if (pid->mode == PID_MODE_DERIVATIV_CALC_NO_SP) { + d = (-val - pid->error_previous) / fmaxf(dt, pid->dt_min); + pid->error_previous = -val; - error_filtered = pid->error_previous_filtered + (- val - pid->error_previous_filtered) * pid->diff_filter_factor; - d = (error_filtered - pid->error_previous_filtered) / fmaxf(dt, 0.003f); // fail-safe for too low dt - pid->error_previous_filtered = error_filtered; } else if (pid->mode == PID_MODE_DERIVATIV_SET) { d = -val_dot; @@ -165,6 +161,10 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo d = 0.0f; } + if (!isfinite(d)) { + d = 0.0f; + } + // Calculate the error integral and check for saturation i = pid->integral + (error * dt); @@ -175,7 +175,7 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo } else { if (!isfinite(i)) { - i = 0; + i = 0.0f; } pid->integral = i; @@ -184,19 +184,17 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo // Calculate the output. Limit output magnitude to pid->limit float output = (error * pid->kp) + (i * pid->ki) + (d * pid->kd); - if (output > pid->limit) output = pid->limit; - - if (output < -pid->limit) output = -pid->limit; if (isfinite(output)) { - pid->last_output = output; - } + if (output > pid->limit) { + output = pid->limit; - pid->control_previous = pid->last_output; + } else if (output < -pid->limit) { + output = -pid->limit; + } - // if (isfinite(error)) { // Why is this necessary? DEW - // pid->error_previous = error; - // } + pid->last_output = output; + } *ctrl_p = (error * pid->kp); *ctrl_i = (i * pid->ki); diff --git a/src/modules/systemlib/pid/pid.h b/src/modules/systemlib/pid/pid.h index eb9df96cc..5f2650f69 100644 --- a/src/modules/systemlib/pid/pid.h +++ b/src/modules/systemlib/pid/pid.h @@ -1,9 +1,11 @@ /**************************************************************************** * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: @author Laurens Mackay <mackayl@student.ethz.ch> - * @author Tobias Naegeli <naegelit@student.ethz.ch> - * @author Martin Rutschmann <rutmarti@student.ethz.ch> + * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. + * Author: Laurens Mackay <mackayl@student.ethz.ch> + * Tobias Naegeli <naegelit@student.ethz.ch> + * Martin Rutschmann <rutmarti@student.ethz.ch> + * Anton Babushkin <anton.babushkin@me.com> + * Julian Oes <joes@student.ethz.ch> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -36,7 +38,14 @@ /** * @file pid.h - * Definition of generic PID control interface + * + * Definition of generic PID control interface. + * + * @author Laurens Mackay <mackayl@student.ethz.ch> + * @author Tobias Naegeli <naegelit@student.ethz.ch> + * @author Martin Rutschmann <rutmarti@student.ethz.ch> + * @author Anton Babushkin <anton.babushkin@me.com> + * @author Julian Oes <joes@student.ethz.ch> */ #ifndef PID_H_ @@ -44,6 +53,8 @@ #include <stdint.h> +__BEGIN_DECLS + /* PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error * val_dot in pid_calculate() will be ignored */ #define PID_MODE_DERIVATIV_CALC 0 @@ -62,22 +73,22 @@ typedef struct { float intmax; float sp; float integral; - float error_previous_filtered; - float control_previous; + float error_previous; float last_output; float limit; + float dt_min; uint8_t mode; - float diff_filter_factor; uint8_t count; uint8_t saturated; } PID_t; -__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, float diff_filter_factor, uint8_t mode); -__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, float diff_filter_factor); +__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, uint8_t mode, float dt_min); +__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit); //void pid_set(PID_t *pid, float sp); __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt, float *ctrl_p, float *ctrl_i, float *ctrl_d); __EXPORT void pid_reset_integral(PID_t *pid); +__END_DECLS #endif /* PID_H_ */ diff --git a/src/modules/systemlib/systemlib.c b/src/modules/systemlib/systemlib.c index afb7eca29..3283aad8a 100644 --- a/src/modules/systemlib/systemlib.c +++ b/src/modules/systemlib/systemlib.c @@ -50,7 +50,7 @@ #include "systemlib.h" -static void kill_task(FAR _TCB *tcb, FAR void *arg); +static void kill_task(FAR struct tcb_s *tcb, FAR void *arg); void killall() { @@ -60,12 +60,12 @@ void killall() sched_foreach(kill_task, NULL); } -static void kill_task(FAR _TCB *tcb, FAR void *arg) +static void kill_task(FAR struct tcb_s *tcb, FAR void *arg) { kill(tcb->pid, SIGUSR1); } -int task_spawn(const char *name, int scheduler, int priority, int stack_size, main_t entry, const char *argv[]) +int task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, main_t entry, const char *argv[]) { int pid; diff --git a/src/modules/systemlib/systemlib.h b/src/modules/systemlib/systemlib.h index 2c53c648b..0194b5e52 100644 --- a/src/modules/systemlib/systemlib.h +++ b/src/modules/systemlib/systemlib.h @@ -42,11 +42,11 @@ #include <float.h> #include <stdint.h> -__BEGIN_DECLS - /** Reboots the board */ extern void up_systemreset(void) noreturn_function; +__BEGIN_DECLS + /** Sends SIGUSR1 to all processes */ __EXPORT void killall(void); @@ -58,7 +58,7 @@ __EXPORT void killall(void); #endif /** Starts a task and performs any specific accounting, scheduler setup, etc. */ -__EXPORT int task_spawn(const char *name, +__EXPORT int task_spawn_cmd(const char *name, int priority, int scheduler, int stack_size, |