mirror of https://github.com/ArduPilot/ardupilot
removed AperiodicProcess code
this code was never being used, and was more complex than it was worth.
This commit is contained in:
parent
ed17f64459
commit
4bbd5392ef
|
@ -61,10 +61,9 @@ And much more so PLEASE PM me on DIYDRONES to add your contribution to the List
|
||||||
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
|
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
|
||||||
#include <AP_InertialSensor.h> // ArduPilot Mega Inertial Sensor (accel & gyro) Library
|
#include <AP_InertialSensor.h> // ArduPilot Mega Inertial Sensor (accel & gyro) Library
|
||||||
#include <AP_IMU.h> // ArduPilot Mega IMU Library
|
#include <AP_IMU.h> // ArduPilot Mega IMU Library
|
||||||
#include <AP_PeriodicProcess.h> // Parent header of Timer and TimerAperiodic
|
#include <AP_PeriodicProcess.h> // Parent header of Timer
|
||||||
// (only included for makefile libpath to work)
|
// (only included for makefile libpath to work)
|
||||||
#include <AP_TimerProcess.h> // TimerProcess is the scheduler for MPU6000 reads.
|
#include <AP_TimerProcess.h> // TimerProcess is the scheduler for MPU6000 reads.
|
||||||
#include <AP_TimerAperiodicProcess.h> // TimerAperiodicProcess is the scheduler for ADC reads.
|
|
||||||
#include <AP_DCM.h> // ArduPilot Mega DCM Library
|
#include <AP_DCM.h> // ArduPilot Mega DCM Library
|
||||||
#include <APM_PI.h> // PI library
|
#include <APM_PI.h> // PI library
|
||||||
#include <RC_Channel.h> // RC Channel Library
|
#include <RC_Channel.h> // RC Channel Library
|
||||||
|
|
|
@ -34,7 +34,7 @@ version 2.1 of the License, or (at your option) any later version.
|
||||||
#include <DataFlash.h> // ArduPilot Mega Flash Memory Library
|
#include <DataFlash.h> // ArduPilot Mega Flash Memory Library
|
||||||
#include <AP_ADC.h> // ArduPilot Mega Analog to Digital Converter Library
|
#include <AP_ADC.h> // ArduPilot Mega Analog to Digital Converter Library
|
||||||
#include <AP_AnalogSource.h>// ArduPilot Mega polymorphic analog getter
|
#include <AP_AnalogSource.h>// ArduPilot Mega polymorphic analog getter
|
||||||
#include <AP_PeriodicProcess.h> // ArduPilot Mega TimerProcess and TimerAperiodicProcess
|
#include <AP_PeriodicProcess.h> // ArduPilot Mega TimerProcess
|
||||||
#include <AP_Baro.h> // ArduPilot barometer library
|
#include <AP_Baro.h> // ArduPilot barometer library
|
||||||
#include <AP_Compass.h> // ArduPilot Mega Magnetometer Library
|
#include <AP_Compass.h> // ArduPilot Mega Magnetometer Library
|
||||||
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
|
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
|
||||||
|
|
|
@ -12,7 +12,7 @@
|
||||||
FastSerialPort0(Serial); // FTDI/console
|
FastSerialPort0(Serial); // FTDI/console
|
||||||
|
|
||||||
Arduino_Mega_ISR_Registry isr_registry;
|
Arduino_Mega_ISR_Registry isr_registry;
|
||||||
AP_TimerAperiodicProcess adc_scheduler;
|
AP_PeriodicProcess adc_scheduler;
|
||||||
|
|
||||||
|
|
||||||
unsigned long timer;
|
unsigned long timer;
|
||||||
|
|
|
@ -7,10 +7,9 @@
|
||||||
#include <FastSerial.h>
|
#include <FastSerial.h>
|
||||||
#include <AP_IMU.h>
|
#include <AP_IMU.h>
|
||||||
#include <AP_IMU_MPU6000.h> // Experimental MPU6000 IMU library
|
#include <AP_IMU_MPU6000.h> // Experimental MPU6000 IMU library
|
||||||
#include <AP_PeriodicProcess.h> // Parent header of Timer and TimerAperiodic
|
#include <AP_PeriodicProcess.h> // Parent header of Timer
|
||||||
// (only included for makefile libpath to work)
|
// (only included for makefile libpath to work)
|
||||||
#include <AP_TimerProcess.h> // TimerProcess is the scheduler for MPU6000 reads.
|
#include <AP_TimerProcess.h> // TimerProcess is the scheduler for MPU6000 reads.
|
||||||
#include <AP_TimerAperiodicProcess.h> // TimerAperiodicProcess is the scheduler for ADC reads.
|
|
||||||
#include <AP_ADC.h>
|
#include <AP_ADC.h>
|
||||||
#include <AP_Math.h>
|
#include <AP_Math.h>
|
||||||
#include <AP_Common.h>
|
#include <AP_Common.h>
|
||||||
|
|
|
@ -17,7 +17,7 @@
|
||||||
FastSerialPort(Serial, 0);
|
FastSerialPort(Serial, 0);
|
||||||
|
|
||||||
Arduino_Mega_ISR_Registry isr_registry;
|
Arduino_Mega_ISR_Registry isr_registry;
|
||||||
AP_TimerAperiodicProcess adc_scheduler;
|
AP_TimerPeriodicProcess adc_scheduler;
|
||||||
|
|
||||||
AP_ADC_ADS7844 adc;
|
AP_ADC_ADS7844 adc;
|
||||||
AP_InertialSensor_Oilpan oilpan_ins(&adc);
|
AP_InertialSensor_Oilpan oilpan_ins(&adc);
|
||||||
|
|
|
@ -5,6 +5,5 @@
|
||||||
#include "PeriodicProcess.h"
|
#include "PeriodicProcess.h"
|
||||||
#include "AP_PeriodicProcessStub.h"
|
#include "AP_PeriodicProcessStub.h"
|
||||||
#include "AP_TimerProcess.h"
|
#include "AP_TimerProcess.h"
|
||||||
#include "AP_TimerAperiodicProcess.h"
|
|
||||||
|
|
||||||
#endif // __AP_PERIODIC_PROCESS_H__
|
#endif // __AP_PERIODIC_PROCESS_H__
|
||||||
|
|
|
@ -1,41 +0,0 @@
|
||||||
|
|
||||||
#include "AP_TimerAperiodicProcess.h"
|
|
||||||
|
|
||||||
extern "C" {
|
|
||||||
#include <inttypes.h>
|
|
||||||
#include <stdint.h>
|
|
||||||
#include "WConstants.h"
|
|
||||||
}
|
|
||||||
// TCNT2 values for various interrupt rates,
|
|
||||||
// assuming 256 prescaler. Note that these values
|
|
||||||
// assume a zero-time ISR. The actual rate will be a
|
|
||||||
// bit lower than this
|
|
||||||
#define TCNT2_781_HZ (256-80)
|
|
||||||
#define TCNT2_1008_HZ (256-62)
|
|
||||||
#define TCNT2_1302_HZ (256-48)
|
|
||||||
|
|
||||||
uint8_t AP_TimerAperiodicProcess::_timer_offset;
|
|
||||||
|
|
||||||
void AP_TimerAperiodicProcess::init( Arduino_Mega_ISR_Registry * isr_reg )
|
|
||||||
{
|
|
||||||
// Enable Timer2 Overflow interrupt to trigger process.
|
|
||||||
TIMSK2 = 0; // Disable interrupts
|
|
||||||
TCCR2A = 0; // normal counting mode
|
|
||||||
TCCR2B = _BV(CS21) | _BV(CS22); // Set prescaler of clk/256
|
|
||||||
TCNT2 = 0; // Set count to zero, so it goes off right away.
|
|
||||||
TIFR2 = _BV(TOV2); // clear pending interrupts;
|
|
||||||
TIMSK2 = _BV(TOIE2); // enable the overflow interrupt
|
|
||||||
|
|
||||||
isr_reg->register_signal(ISR_REGISTRY_TIMER2_OVF, AP_TimerAperiodicProcess::run);
|
|
||||||
}
|
|
||||||
|
|
||||||
void AP_TimerAperiodicProcess::run(void)
|
|
||||||
{
|
|
||||||
_timer_offset = (_timer_offset + 49) % 32;
|
|
||||||
_period = TCNT2_781_HZ + _timer_offset;
|
|
||||||
TCNT2 = _period;
|
|
||||||
for (int i = 0; i < _pidx; i++) {
|
|
||||||
if (_proc[i] != NULL)
|
|
||||||
_proc[i]();
|
|
||||||
}
|
|
||||||
}
|
|
|
@ -1,19 +0,0 @@
|
||||||
|
|
||||||
#ifndef __AP_TIMER_APERIODIC_PROCESS_H__
|
|
||||||
#define __AP_TIMER_APERIODIC_PROCESS_H__
|
|
||||||
|
|
||||||
#include <stdint.h>
|
|
||||||
|
|
||||||
#include "../Arduino_Mega_ISR_Registry/Arduino_Mega_ISR_Registry.h"
|
|
||||||
#include "AP_TimerProcess.h"
|
|
||||||
|
|
||||||
class AP_TimerAperiodicProcess : public AP_TimerProcess
|
|
||||||
{
|
|
||||||
public:
|
|
||||||
void init( Arduino_Mega_ISR_Registry * isr_reg );
|
|
||||||
static void run(void);
|
|
||||||
private:
|
|
||||||
static uint8_t _timer_offset;
|
|
||||||
};
|
|
||||||
|
|
||||||
#endif // __AP_TIMER_APERIODIC_PROCESS_H__
|
|
|
@ -25,7 +25,6 @@
|
||||||
#include <wiring.h>
|
#include <wiring.h>
|
||||||
#include <AP_PeriodicProcess.h>
|
#include <AP_PeriodicProcess.h>
|
||||||
#include <AP_TimerProcess.h>
|
#include <AP_TimerProcess.h>
|
||||||
#include <AP_TimerAperiodicProcess.h>
|
|
||||||
#include "sitl_adc.h"
|
#include "sitl_adc.h"
|
||||||
#include "sitl_rc.h"
|
#include "sitl_rc.h"
|
||||||
#include "desktop.h"
|
#include "desktop.h"
|
||||||
|
|
Loading…
Reference in New Issue