2013-08-29 02:34:34 -03:00
/*
This program is free software : you can redistribute it and / or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation , either version 3 of the License , or
( at your option ) any later version .
This program is distributed in the hope that it will be useful ,
but WITHOUT ANY WARRANTY ; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE . See the
GNU General Public License for more details .
You should have received a copy of the GNU General Public License
along with this program . If not , see < http : //www.gnu.org/licenses/>.
*/
2013-01-11 20:59:20 -04:00
/*
* main loop scheduler for APM
* Author : Andrew Tridgell , January 2013
*
*/
2015-10-26 09:01:52 -03:00
# include "AP_Scheduler.h"
2013-01-11 20:59:20 -04:00
2015-08-11 03:28:45 -03:00
# include <AP_HAL/AP_HAL.h>
# include <AP_Param/AP_Param.h>
2015-11-30 15:59:07 -04:00
# include <AP_Vehicle/AP_Vehicle.h>
2016-04-21 01:50:15 -03:00
# include <stdio.h>
2015-11-30 15:59:07 -04:00
2016-02-22 16:48:31 -04:00
# if APM_BUILD_TYPE(APM_BUILD_ArduCopter) || APM_BUILD_TYPE(APM_BUILD_ArduSub)
2015-11-30 15:59:07 -04:00
# define SCHEDULER_DEFAULT_LOOP_RATE 400
# else
# define SCHEDULER_DEFAULT_LOOP_RATE 50
2016-06-04 22:20:09 -03:00
# endif
2013-01-11 20:59:20 -04:00
extern const AP_HAL : : HAL & hal ;
2014-07-02 20:09:11 -03:00
int8_t AP_Scheduler : : current_task = - 1 ;
2015-10-25 14:03:46 -03:00
const AP_Param : : GroupInfo AP_Scheduler : : var_info [ ] = {
2013-01-11 20:59:20 -04:00
// @Param: DEBUG
// @DisplayName: Scheduler debug level
2013-08-30 02:33:30 -03:00
// @Description: Set to non-zero to enable scheduler debug messages. When set to show "Slips" the scheduler will display a message whenever a scheduled task is delayed due to too much CPU load. When set to ShowOverruns the scheduled will display a message whenever a task takes longer than the limit promised in the task table.
2013-10-13 07:39:33 -03:00
// @Values: 0:Disabled,2:ShowSlips,3:ShowOverruns
2013-01-11 20:59:20 -04:00
// @User: Advanced
AP_GROUPINFO ( " DEBUG " , 0 , AP_Scheduler , _debug , 0 ) ,
2015-11-30 15:59:07 -04:00
// @Param: LOOP_RATE
// @DisplayName: Scheduling main loop rate
2018-01-15 22:22:32 -04:00
// @Description: This controls the rate of the main control loop in Hz. This should only be changed by developers. This only takes effect on restart. Values over 400 are considered highly experimental.
2016-01-03 17:37:31 -04:00
// @Values: 50:50Hz,100:100Hz,200:200Hz,250:250Hz,300:300Hz,400:400Hz
2015-11-30 15:59:07 -04:00
// @RebootRequired: True
// @User: Advanced
AP_GROUPINFO ( " LOOP_RATE " , 1 , AP_Scheduler , _loop_rate_hz , SCHEDULER_DEFAULT_LOOP_RATE ) ,
2013-01-11 20:59:20 -04:00
AP_GROUPEND
} ;
2015-11-30 15:59:07 -04:00
// constructor
AP_Scheduler : : AP_Scheduler ( void )
{
AP_Param : : setup_object_defaults ( this , var_info ) ;
2018-01-15 22:22:32 -04:00
// only allow 50 to 2000 Hz
2016-01-03 17:37:31 -04:00
if ( _loop_rate_hz < 50 ) {
2015-11-30 15:59:07 -04:00
_loop_rate_hz . set ( 50 ) ;
2018-01-15 22:22:32 -04:00
} else if ( _loop_rate_hz > 2000 ) {
_loop_rate_hz . set ( 2000 ) ;
2015-11-30 15:59:07 -04:00
}
}
2013-01-11 20:59:20 -04:00
// initialise the scheduler
2015-12-23 10:54:57 -04:00
void AP_Scheduler : : init ( const AP_Scheduler : : Task * tasks , uint8_t num_tasks )
2013-01-11 20:59:20 -04:00
{
_tasks = tasks ;
_num_tasks = num_tasks ;
_last_run = new uint16_t [ _num_tasks ] ;
2013-04-17 10:01:13 -03:00
memset ( _last_run , 0 , sizeof ( _last_run [ 0 ] ) * _num_tasks ) ;
_tick_counter = 0 ;
2013-01-11 20:59:20 -04:00
}
// one tick has passed
void AP_Scheduler : : tick ( void )
{
_tick_counter + + ;
}
/*
run one tick
this will run as many scheduler tasks as we can in the specified time
*/
2016-07-25 19:27:24 -03:00
void AP_Scheduler : : run ( uint32_t time_available )
2013-01-11 20:59:20 -04:00
{
2015-11-19 23:14:32 -04:00
uint32_t run_started_usec = AP_HAL : : micros ( ) ;
2013-10-10 00:13:44 -03:00
uint32_t now = run_started_usec ;
2017-04-29 03:44:18 -03:00
if ( _debug > 1 & & _perf_counters = = nullptr ) {
2016-04-21 01:50:15 -03:00
_perf_counters = new AP_HAL : : Util : : perf_counter_t [ _num_tasks ] ;
if ( _perf_counters ! = nullptr ) {
for ( uint8_t i = 0 ; i < _num_tasks ; i + + ) {
_perf_counters [ i ] = hal . util - > perf_alloc ( AP_HAL : : Util : : PC_ELAPSED , _tasks [ i ] . name ) ;
}
}
}
2013-01-11 20:59:20 -04:00
for ( uint8_t i = 0 ; i < _num_tasks ; i + + ) {
uint16_t dt = _tick_counter - _last_run [ i ] ;
2015-11-30 15:59:07 -04:00
uint16_t interval_ticks = _loop_rate_hz / _tasks [ i ] . rate_hz ;
if ( interval_ticks < 1 ) {
interval_ticks = 1 ;
}
2013-01-11 22:04:52 -04:00
if ( dt > = interval_ticks ) {
2013-01-11 20:59:20 -04:00
// this task is due to run. Do we have enough time to run it?
2015-12-23 10:54:23 -04:00
_task_time_allowed = _tasks [ i ] . max_time_micros ;
2013-01-11 22:04:52 -04:00
if ( dt > = interval_ticks * 2 ) {
// we've slipped a whole run of this task!
2017-04-29 03:44:18 -03:00
if ( _debug > 4 ) {
2016-04-21 01:50:15 -03:00
: : printf ( " Scheduler slip task[%u-%s] (%u/%u/%u) \n " ,
( unsigned ) i ,
_tasks [ i ] . name ,
( unsigned ) dt ,
( unsigned ) interval_ticks ,
( unsigned ) _task_time_allowed ) ;
2013-01-11 22:04:52 -04:00
}
}
2015-12-23 10:54:57 -04:00
2013-01-11 20:59:20 -04:00
if ( _task_time_allowed < = time_available ) {
// run it
2013-10-10 00:13:44 -03:00
_task_time_started = now ;
2014-07-02 20:09:11 -03:00
current_task = i ;
2017-04-29 03:44:18 -03:00
if ( _debug > 1 & & _perf_counters & & _perf_counters [ i ] ) {
2016-04-21 01:50:15 -03:00
hal . util - > perf_begin ( _perf_counters [ i ] ) ;
}
2015-12-23 10:54:23 -04:00
_tasks [ i ] . function ( ) ;
2017-04-29 03:44:18 -03:00
if ( _debug > 1 & & _perf_counters & & _perf_counters [ i ] ) {
2016-04-21 01:50:15 -03:00
hal . util - > perf_end ( _perf_counters [ i ] ) ;
}
2014-07-02 20:09:11 -03:00
current_task = - 1 ;
2015-12-23 10:54:57 -04:00
2013-01-11 20:59:20 -04:00
// record the tick counter when we ran. This drives
// when we next run the event
_last_run [ i ] = _tick_counter ;
2015-12-23 10:54:57 -04:00
2013-01-11 20:59:20 -04:00
// work out how long the event actually took
2015-11-19 23:14:32 -04:00
now = AP_HAL : : micros ( ) ;
2013-10-10 00:13:44 -03:00
uint32_t time_taken = now - _task_time_started ;
2015-12-23 10:54:57 -04:00
2013-01-11 20:59:20 -04:00
if ( time_taken > _task_time_allowed ) {
// the event overran!
2016-04-21 01:50:15 -03:00
if ( _debug > 4 ) {
: : printf ( " Scheduler overrun task[%u-%s] (%u/%u) \n " ,
( unsigned ) i ,
_tasks [ i ] . name ,
( unsigned ) time_taken ,
( unsigned ) _task_time_allowed ) ;
2013-01-11 20:59:20 -04:00
}
2013-10-13 07:38:50 -03:00
}
if ( time_taken > = time_available ) {
2013-07-23 02:00:00 -03:00
goto update_spare_ticks ;
2013-01-11 20:59:20 -04:00
}
time_available - = time_taken ;
}
}
}
2013-07-23 02:00:00 -03:00
// update number of spare microseconds
_spare_micros + = time_available ;
update_spare_ticks :
_spare_ticks + + ;
if ( _spare_ticks = = 32 ) {
_spare_ticks / = 2 ;
_spare_micros / = 2 ;
}
2013-01-11 20:59:20 -04:00
}
/*
return number of micros until the current task reaches its deadline
*/
uint16_t AP_Scheduler : : time_available_usec ( void )
{
2015-11-19 23:14:32 -04:00
uint32_t dt = AP_HAL : : micros ( ) - _task_time_started ;
2013-01-11 20:59:20 -04:00
if ( dt > _task_time_allowed ) {
return 0 ;
}
return _task_time_allowed - dt ;
}
2013-07-23 02:00:00 -03:00
/*
calculate load average as a number from 0 to 1
*/
2018-01-17 03:20:44 -04:00
float AP_Scheduler : : load_average ( )
2013-07-23 02:00:00 -03:00
{
2013-08-02 09:07:00 -03:00
if ( _spare_ticks = = 0 ) {
return 0.0f ;
}
2017-07-30 02:02:40 -03:00
const uint32_t loop_us = get_loop_period_us ( ) ;
const uint32_t used_time = loop_us - ( _spare_micros / _spare_ticks ) ;
return used_time / ( float ) loop_us ;
2013-07-23 02:00:00 -03:00
}