task_timing.c
Go to the documentation of this file.
00001 /*------------------------------------------------------------------------
00002  *---------------------           RT-WMP              --------------------
00003  *------------------------------------------------------------------------
00004  *                                                         V7.0B  11/05/10
00005  *
00006  *
00007  *  File: ./src/plugins/long_messages/long_messages.c
00008  *  Authors: Danilo Tardioli
00009  *  ----------------------------------------------------------------------
00010  *  Copyright (C) 2000-2010, Universidad de Zaragoza, SPAIN
00011  *
00012  *  Contact Addresses: Danilo Tardioli                   dantard@unizar.es
00013  *
00014  *  RT-WMP is free software; you can  redistribute it and/or  modify it
00015  *  under the terms of the GNU General Public License  as published by the
00016  *  Free Software Foundation;  either  version 2, or (at  your option) any
00017  *  later version.
00018  *
00019  *  RT-WMP  is distributed  in the  hope  that  it will be   useful, but
00020  *  WITHOUT  ANY  WARRANTY;     without  even the   implied   warranty  of
00021  *  MERCHANTABILITY  or  FITNESS FOR A  PARTICULAR PURPOSE.    See the GNU
00022  *  General Public License for more details.
00023  *
00024  *  You should have received  a  copy of  the  GNU General Public  License
00025  *  distributed with RT-WMP;  see file COPYING.   If not,  write to the
00026  *  Free Software  Foundation,  59 Temple Place  -  Suite 330,  Boston, MA
00027  *  02111-1307, USA.
00028  *
00029  *  As a  special exception, if you  link this  unit  with other  files to
00030  *  produce an   executable,   this unit  does  not  by  itself cause  the
00031  *  resulting executable to be covered by the  GNU General Public License.
00032  *  This exception does  not however invalidate  any other reasons why the
00033  *  executable file might be covered by the GNU Public License.
00034  *
00035  *----------------------------------------------------------------------*/
00036 #include <stdarg.h>
00037 #include "config/compiler.h"
00038 #include "core/interface/wmp_interface.h"
00039 #include "core/include/global.h"
00040 #include "core/include/frames.h"
00041 #include "include/queue_core.h"
00042 #include "core/include/queues.h"
00043 #include "core/include/wmp_misc.h"
00044 #include "core/include/queue_core.h"
00045 
00046 typedef struct {
00047         long long last_push_ts;
00048         int period;
00049         int active;
00050         int initied;
00051         int fixed;
00052         int fixed_period;
00053         int mm[10];
00054         int mm_idx;
00055         int mm_initied;
00056 } task_t;
00057 
00058 static task_t tasks[128];
00059 
00060 int task_get_next_predicted_push_delay_for_priority(int priority) {
00061         long long delay, elapsed, period_us;
00062 
00063         if (!tasks[priority].active) {
00064                 return INT_MAX;
00065         }
00066 
00067         period_us = tasks[priority].period * 1000;
00068         elapsed = (getRawActualTimeus() - tasks[priority].last_push_ts);
00069 
00070         delay = elapsed > period_us ? 0 : period_us - elapsed;
00071         delay = delay > (long long) INT_MAX ? INT_MAX : delay;
00072 
00073         return (((int) delay) / 1000);
00074 
00075 }
00076 
00077 static int task_get_mm(int priority, int last_period){
00078         int i;
00079         if (!tasks[priority].mm_initied){
00080                 for (i=0; i<10; i++){
00081                         tasks[priority].mm[i]=last_period;
00082                 }
00083                 tasks[priority].mm_initied = 1;
00084                 return last_period;
00085         }else{
00086                 int sum = 0;
00087                 tasks[priority].mm_idx++;
00088                 if (tasks[priority].mm_idx >= 10){
00089                         tasks[priority].mm_idx = 0;
00090                 }
00091                 tasks[priority].mm[tasks[priority].mm_idx] = last_period;
00092                 for (i = 0; i < 10; i++) {
00093                         sum += tasks[priority].mm[i];
00094                 }
00095                 return (int)(sum/10);
00096         }
00097 }
00098 
00099 void task_init(void) {
00100         int i;
00101         memset(tasks, 0, sizeof(tasks));
00102         for (i = 0; i < 128; i++) {
00103                 tasks[i].period = INT_MAX;
00104         }
00105 }
00106 
00107 void task_push(int priority) {
00108         /* Calculate period */
00109         if (priority >= 0 && priority < 128) {
00110                 if (tasks[priority].initied) {
00111                         int candidate_period = (((int) (getRawActualTimeus() - tasks[priority].last_push_ts)) / 1000);
00112                         if (tasks[priority].fixed && candidate_period < tasks[priority].fixed_period){
00113                                 candidate_period = tasks[priority].fixed_period;
00114                         }
00115                         tasks[priority].period = task_get_mm(priority,candidate_period);
00116                         tasks[priority].active = 1;
00117                 } else {
00118                         tasks[priority].initied = 1;
00119                 }
00120                 tasks[priority].last_push_ts = getRawActualTimeus();
00121         }
00122 }
00123 
00124 static void flush_unactive(void) {
00125         int i;
00126         for (i = 0; i < 128; i++) {
00127                 if (tasks[i].active) {
00128                         long long elapsed, period_us;
00129 
00130                         period_us = tasks[i].period * 1000;
00131                         elapsed = (getRawActualTimeus() - tasks[i].last_push_ts);
00132 
00133                         if (elapsed > (long long) 2 * period_us) {
00134                                 tasks[i].active = 0;
00135                                 tasks[i].initied = 0;
00136                                 tasks[i].mm_initied = 0;
00137                         }
00138                 }
00139         }
00140 }
00141 
00142 int task_get_next_predicted_push_delay(void) {
00143         int i, min = INT_MAX;
00144         flush_unactive();
00145         for (i = 0; i < 128; i++) {
00146                 if (tasks[i].active){
00147                         int delay = task_get_next_predicted_push_delay_for_priority(i);
00148                         if (delay < min) {
00149                                 min = delay;
00150                         }
00151                 }
00152         }
00153         return min;
00154 }
00155 
00156 int task_get_next_predicted_push_delay_most_priority_than(int priority) {
00157         int i, min = INT_MAX;
00158         flush_unactive();
00159         for (i = priority; i < 128; i++) {
00160                 if (tasks[i].active){
00161                         int delay = task_get_next_predicted_push_delay_for_priority(i);
00162                         if (delay < min) {
00163                                 min = delay;
00164                         }
00165                 }
00166         }
00167         return min;
00168 }
00169 
00170 int task_get_priority_period(int priority){
00171         if (priority >=0 && priority < 128){
00172                 return tasks[priority].period;
00173         }else{
00174                 return -1;
00175         }
00176 }
00177 
00178 int task_set_fixed_period(int priority, int period){
00179         if (priority >=0 && priority < 128){
00180                 tasks[priority].fixed_period = period;
00181                 tasks[priority].fixed = 1;
00182                 tasks[priority].active = 0;
00183                 return 1;
00184         }else{
00185                 return 0;
00186         }
00187 }
00188 
00189 int wmpSetTaskMinimumSeparation(int priority, int period){
00190         return task_set_fixed_period(priority,period);
00191 }


ros_rt_wmp
Author(s): Danilo Tardioli, dantard@unizar.es
autogenerated on Mon Oct 6 2014 08:27:11