//************************************************************
void BLIInit()
{
if (_BLIInit)
return; // BLI has been initialized
//----------------------------------------------------
_BLIInit = 1; // BLI Initialized
//----------------------------------------------------
//*******************************************************************
// BLI Component relies on Timer functions, so if Timer is not
// initialized, it is a good time to initialize it now...
//-------------------------------------------------------------------
TMRInitDefault();
//*******************************************************************
// LED / Buzzer control port
//-------------------------------------------------------------------
// Configure RA10 as OUTPUT
_TRISA10 = 0;
_LATA10 = 0; // Reset port
//-------------------------------------------------------------------
return;
}
//==============================================================================
// Common initialization routine
//==============================================================================
void _MCM_InitLocal(MCMConf frameConfig, float Period, uint UpTime)
{
if (_MC_Init)
return; // Already initialized
//--------------------------------------------------------------------------
_MC_Init = 1;
_MC_Config = frameConfig;
//--------------------------------------------------------------------------
// MCM Initialization utilizes TMRDelay(...) function, so if Timer is not
// initialized, it is a good time to initialize it now...
//--------------------------------------------------------------------------
TMRInitDefault();
//==========================================================================
// The Output Compare module by default uses Timer2; we will stay with
// default....
//--------------------------------------------------------------------------
// Stop/Disable Timer2; reset all flags
//--------------------------------------------------------------------------
T2CON = 0;
// Enable Timer2 if disabled through PMD
_T2MD = 0; // Timer2 module enabled
//==========================================================================
// Timer pre-scaler and rollover counter are calculated assuming
// Fcy = 64 MHz based upon specified refresh Period.
//==========================================================================
T2CONbits.TCKPS = 0b10; // Set Timer2 pre-scaler to 1:64 (tick = 1 usec)
//-----------------------
_MC_Base_Value = 1000; // 1 msec at current pre-scaler
//--------------------------------------------------------------------------
// Re-set Timer2 counter
TMR2 = 0;
// Set Timer2 rollover threshold (MC frequency counter - number of 1 usec
// ticks comprising Period set in msec)
PR2 = floorf(Period * 1000) - 1;
//--------------------------------------------------------------------------
// Configure Timer2 interrupts
//--------------------------------------------------------------------------
_T2IF = 0 ; // Clear TMR2 interrupt flag
_T2IE = 0 ; // Disable TMR2 CPU interrupt
//==========================================================================
//==========================================================================
// Output-Compare module is being used in this application to generate PPM
// signal to control 4 motors through respective ESCs.
// 4 OC (Output Compare) channels are being used for Motor Control.
//--------------------------------------------------------------------------
// Disable OC modules for configuration and reset all control bits
//--------------------------------------------------------------------------
//<editor-fold defaultstate="collapsed" desc="OCxCON1 bits definitions">
// bit 15-14 Unimplemented: Read as ?0?
// bit 13 OCSIDL:
// Stop Output Compare x in Idle Mode Control bit
// 1 = Output Compare x Halts in CPU Idle mode
// 0 = Output Compare x continues to operate in CPU Idle mode
// bit 12-10 OCTSEL<2:0>:
// Output Compare x Clock Select bits
// 111 = Peripheral clock (FP)
// 110 = Reserved
// 101 = Reserved
// 100 = Clock source of T1CLK is the clock source of OCx
// (only the synchronous clock is supported)
// 011 = Clock source of T5CLK is the clock source of OCx
// 010 = Clock source of T4CLK is the clock source of OCx
// 001 = Clock source of T3CLK is the clock source of OCx
// 000 = Clock source of T2CLK is the clock source of OCx
// bit 9 ENFLTC:
// Fault C Input Enable bit
// 1 = Output Compare Fault C input (OCFC) is enabled
// 0 = Output Compare Fault C input (OCFC) is disabled
// bit 8 ENFLTB:
// Fault B Input Enable bit
// 1 = Output Compare Fault B input (OCFB) is enabled
// 0 = Output Compare Fault B input (OCFB) is disabled
// bit 7 ENFLTA:
// Fault A Input Enable bit
// 1 = Output Compare Fault A input (OCFA) is enabled
// 0 = Output Compare Fault A input (OCFA) is disabled
// bit 6 OCFLTC:
// PWM Fault C Condition Status bit
// 1 = PWM Fault C condition on OCFC pin has occurred
// 0 = No PWM Fault C condition on OCFC pin has occurred
// bit 5 OCFLTB:
// PWM Fault B Condition Status bit
// 1 = PWM Fault B condition on OCFB pin has occurred
// 0 = No PWM Fault B condition on OCFB pin has occurred
// bit 4 OCFLTA:
// PWM Fault A Condition Status bit
// 1 = PWM Fault A condition on OCFA pin has occurred
// 0 = No PWM Fault A condition on OCFA pin has occurred
// bit 3 TRIGMODE:
// Trigger Status Mode Select bit
// 1 = TRIGSTAT (OCxCON2<6>) is cleared when OCxRS = OCxTMR
// or in software
// 0 = TRIGSTAT is cleared only by software
// bit 2-0 OCM<2:0>:
// Output Compare Mode Select bits
// 111 = Center-Aligned PWM mode: Output set high when
// OCxTMR = OCxR and set low when OCxTMR = OCxRS
// 110 = Edge-Aligned PWM mode: Output set high when OCxTMR = 0
// and set low when OCxTMR = OCxR
// 101 = Double Compare Continuous Pulse mode: Initializes OCx pin
// low, toggles OCx state continuously on alternate matches
// of OCxR and OCxRS
// 100 = Double Compare Single-Shot mode: Initializes OCx pin low,
// toggles OCx state on matches of OCxR and OCxRS once
// 011 = Single Compare mode: Compares events with OCxR,
// continuously toggles OCx pin
// 010 = Single Compare Single-Shot mode: Initializes OCx pin high,
// compares event with OCxR, forces OCx pin low
// 001 = Single Compare Single-Shot mode: Initializes OCx pin low,
// compares event with OCxR, forces OCx pin high
// 000 = Output compare channel is disabled//
//</editor-fold>
//--------------------------------------------------------------------------
// Setting OCxCON1 to 0 disables the module, sets clock source to Timer2,
// and disables all fault control.
//--------------------------------------------------------------------------
OC1CON1 = OC2CON1 = OC3CON1 = OC4CON1 = 0;
//--------------------------------------------------------------------------
//<editor-fold defaultstate="collapsed" desc="OCxCON2 bits definitiona">
// bit 15 FLTMD:
// Fault Mode Select bit
// 1 = Fault mode is maintained until the Fault source is removed;
// the corresponding OCFLTx bit is cleared in software and a
// new PWM period starts
// 0 = Fault mode is maintained until the Fault source is removed
// and a new PWM period starts
// bit 14 FLTOUT:
// Fault Out bit
// 1 = PWM output is driven high on a Fault
// 0 = PWM output is driven low on a Fault
// bit 13 FLTTRIEN:
// Fault Output State Select bit
// 1 = OCx pin is tri-stated on Fault condition
// 0 = OCx pin I/O state defined by FLTOUT bit on Fault condition
// bit 12 OCINV:
// OCMP Invert bit
// 1 = OCx output is inverted
// 0 = OCx output is not inverted
// bit 11-9
// Unimplemented: Read as ?0?
// bit 8 OC32:
// Cascade Two OCx Modules Enable bit (32-bit operation)
// 1 = Cascade module operation is enabled
// 0 = Cascade module operation is disabled
// bit 7 OCTRIG:
// OCx Trigger/Sync Select bit
// 1 = Triggers OCx from source designated by SYNCSELx bits
// 0 = Synchronizes OCx with source designated by SYNCSELx bits
//bit 6 TRIGSTAT:
// Timer Trigger Status bit
// 1 = Timer source has been triggered and is running
// 0 = Timer source has not been triggered and is being held clear
// bit 5 OCTRIS:
// OCx Output Pin Direction Select bit
// 1 = OCx is tri-stated
// 0 = Output compare module drives the OCx pin
// bit 4-0 SYNCSEL<4:0>:
// Trigger/Synchronization Source Selection bits
// 11111 = No Sync or Trigger source for OCx
// 11110 = INT2 pin synchronizes or triggers OCx
// 11101 = INT1 pin synchronizes or triggers OCx
// 11100 = Reserved
// 11011 = ADC1 module synchronizes or triggers OCx
// 11010 = CMP3 module synchronizes or triggers OCx
// 11001 = CMP2 module synchronizes or triggers OCx
// 11000 = CMP1 module synchronizes or triggers OCx
// 10111 = IC8 module synchronizes or triggers OCx
// 10110 = IC7 module synchronizes or triggers OCx
// 10101 = IC6 module synchronizes or triggers OCx
// 10100 = IC5 module synchronizes or triggers OCx
// 10011 = IC4 module synchronizes or triggers OCx
// 10010 = IC3 module synchronizes or triggers OCx
// 10001 = IC2 module synchronizes or triggers OCx
// 10000 = IC1 module synchronizes or triggers OCx
// 01111 = Timer5 synchronizes or triggers OCx
// 01110 = Timer4 synchronizes or triggers OCx
// 01101 = Timer3 synchronizes or triggers OCx
// 01100 = Timer2 synchronizes or triggers OCx (default)
// 01011 = Timer1 synchronizes or triggers OCx
// 01010 = No Sync or Trigger source for OCx
// 01001 = OC9 module synchronizes or triggers OCx
// 01000 = OC8 module synchronizes or triggers OCx
// 00111 = OC7 module synchronizes or triggers OCx
// 00110 = OC6 module synchronizes or triggers OCx
// 00101 = OC5 module synchronizes or triggers OCx
// 00100 = OC4 module synchronizes or triggers OCx
// 00011 = OC3 module synchronizes or triggers OCx
// 00010 = OC2 module synchronizes or triggers OCx
// 00001 = OC1 module synchronizes or triggers OCx
// 00000 = No Sync or Trigger source for OCx
// </editor-fold>
//--------------------------------------------------------------------------
// Setting OCxCON2 to 0 disables fault mode, sets OCx pins as non-inverted
// and driven by OCx module (vs. tri-state), and disables OCx trigger
//--------------------------------------------------------------------------
OC1CON2 = OC2CON2 = OC3CON2 = OC4CON2 = 0;
//--------------------------------------------------------------------------
// Enable OC1-OC4 in PMD
//--------------------------------------------------------------------------
_OC1MD = 0;
_OC2MD = 0;
_OC3MD = 0;
_OC4MD = 0;
//--------------------------------------------------------------------------
// Enabling OCx in PMD sets control registers to their default values,
// which includes setting SYNCSEL (OCxCON2) to 0b01100; this default
// setting establishes Timer2 as synchronization source for OC1-OC4
//--------------------------------------------------------------------------
// Now that OCx modules are enabled in PMD, we may assign output pins
//--------------------------------------------------------------------------
_MCM_InitPinMap();
//--------------------------------------------------------------------------
//--------------------------------------------------------------------------
// In this application we do not use Output Compare interrupts so we need
// to reset any outstanding requests and block OC interrupts.
//--------------------------------------------------------------------------
_OC1IE = _OC2IE = _OC3IE = _OC4IE = 0;
_OC1IF = _OC2IF = _OC3IF = _OC4IF = 0;
_OC1IP = _OC2IP = _OC3IP = _OC4IP = 0;
//==========================================================================
//==========================================================================
// Configure OC modules:
//--------------------------------------------------------------------------
// Timer2 is already selected as clock source for OC1-OC4 by OCxCON1=0
//--------------------------------------------------------------------------
// Put all OC modules into the Edge-Aligned PWM mode
//--------------------------------------------------------------------------
OC1CON1bits.OCM = 0b110; // Edge-Aligned PWM mode on OC1
OC2CON1bits.OCM = 0b110; // Edge-Aligned PWM mode on OC2
OC3CON1bits.OCM = 0b110; // Edge-Aligned PWM mode on OC3
OC4CON1bits.OCM = 0b110; // Edge-Aligned PWM mode on OC4
//==========================================================================
//==========================================================================
// Set initial duty cycle (OCxR) registers for PPM mode.
//--------------------------------------------------------------------------
if (UpTime > 0)
{
//----------------------------------------------------------------------
// Special case: start with 100% motor control output so that ESC can
// capture "high" throttle
//----------------------------------------------------------------------
uint MaxValue = 2 * _MC_Base_Value;
//-----------------------------------
OC1R = OC2R = OC3R = OC4R = MaxValue;
}
else
{
//----------------------------------------------------------------------
// Normal start: Set Motor Control to 0%
//----------------------------------------------------------------------
OC1R = OC2R = OC3R = OC4R = _MC_Base_Value;
//----------------------------------------------------------------------
}
//==========================================================================
// Start OC1-OC4 by enabling Timer2, which provides both clock source and
// synchronization source for selected Output Compare modules
//--------------------------------------------------------------------------
T2CONbits.TON = 0b1;
//==========================================================================
if (UpTime > 0)
{
//----------------------------------------------------------------------
// Keep output HIGH for defined UpTime
//----------------------------------------------------------------------
TMRDelay(UpTime);
//----------------------------------------------------------------------
// Reset Motor Control to 0%
//----------------------------------------------------------------------
OC1R = OC2R = OC3R = OC4R = _MC_Base_Value;
//----------------------------------------------------------------------
// Wait until ESC captures Low Throttle Value
//----------------------------------------------------------------------
TMRDelay(UpTime);
}
}
//**********************************************************
// SR04Init(...) function initializes SR04 interface and
// respective IC Module.
//**********************************************************
void SR04Init(int IL) // Parameter (1<=IL<=7) defines the
// priority of IC interrupt routine.
{
if (_SR04Init)
return; // Component already initialized
_SR04Init = 1; // Set initialization flag
//====================================================
//----------------------------------------------------
// Validate requested Interrup Level value
//----------------------------------------------------
if (IL < 1) IL = 1;
if (IL > 7) IL = 7;
_SR04IL = IL;
//----------------------------------------------------
//************************************************
// SR04 group of functions depend on timer providing
// timestamps for interval measurement, thus we
// make sure that timer is intitialized.
//************************************************
TMRInitDefault();
//---------------------------------------------------------
ICCON = 0; // Disable ICx for configuration
//---------------------------------------------------------
// Please NOTE that the Init program disables all PERIPHERAL
// MODULES using PMDx registers. To continue configuring and
// later enabling Input Capture module we need to enable
// module in PMDx
//---------------------------------------------------------
ICMD = 0; // Enable ICx module
Nop(); // Skip one cycle to let module enable
//---------------------------------------------------------
// Configure Interrupts
//---------------------------------------------------------
ICIE = 0; // Disable ICx interrupt
ICIF = 0; // Clear ICx interrupt flag
ICIP = _SR04IL; // Set ICx interrupt priority
//---------------------------------------------------------
//---------------------------------------------------------
// Map MCU pins used by Input Capture module through
// REMAPPABLE PIN SELECT feature.
//---------------------------------------------------------
_SR04InitPinMap();
//---------------------------------------------------------
// After configuration complete enable ICx module
//---------------------------------------------------------
// NOTE: For this application we do not capture the
// actual timer value associated with the module
// (we will use common timer module), so we do not
// care configuring timer associated with ICx
//---------------------------------------------------------
ICM = 1; // Capture mode, interrupt on every edge
//---------------------------------------------------------
// Start processing pulses...
//---------------------------------------------------------
ICIE = 1; // Enable ICx interrupts
//---------------------------------------------------------
return;
}
//==============================================================================
void DCMReset(BOOL UseAcc, BOOL UseMag, BOOL MagYawOnly,
Vector* Acc, Vector* Mag)
{
// First, some "housekeeping" ...
TMRInitDefault(); // Most probably timer is already initialized
// so this call is just a NOP
//------------------------------------------------------------------
// Set gyro drift compensation options
//------------------------------------------------------------------
_DCM_UseAcc = UseAcc;
_DCM_UseMag = UseMag;
_DCM_UseMagYawOnly = MagYawOnly;
//------------------------------------------------------------------
// Hopefully the Acceleration vector A we received was averaged over
// some time (maybe, 200-300 msec) and is rather stable as we will
// derive from it initial orientation of the quad...
//------------------------------------------------------------------
// Please NOTE: As we have Axis Z pointing down and gravity
// acceleration vector pointing up (-1.0), we have to change sign
// on arguments of the Atan2 function to get the correct quadrant
float Roll = atan2f(-Acc->Y, -Acc->Z);
//
float Pitch = atan2f(Acc->X, sqrtf(Acc->Y*Acc->Y + Acc->Z*Acc->Z));
float Yaw = 0.0;
// Using calculated values for Roll, Pitch, and Yaw we may build
// initial Rotation Matrix
MatrixEulerRotation(Roll, Pitch, Yaw, &_DCMRM);
//------------------------------------------------------------------
//------------------------------------------------------------------
// Now that we established rotation matrix, we may convert magnetic
// vector measured in Body frame to Earth frame, normalize (as we do
// not care about the strength of magnetic field, just its direction)
// and store for future use...
//------------------------------------------------------------------
DCMToEarth(Mag, &_DCM_BaseMAG);
VectorNormalize(&_DCM_BaseMAG, &_DCM_BaseMAG);
//-----------------------------------------------
// ...and then calculate Base Azimuth
//-----------------------------------------------
if (0 != _DCM_BaseMAG.X || 0 != _DCM_BaseMAG.Y )
_DCM_BaseAzimuth = atan2f(-_DCM_BaseMAG.Y, _DCM_BaseMAG.X);
else
_DCM_BaseAzimuth = 0.0;
//------------------------------------------------------------------
// If magnetic vector is used ONLY for Yaw drift compensation we
// should nullify Z-axis component
//------------------------------------------------------------------
if (TRUE == _DCM_UseMag && TRUE == _DCM_UseMagYawOnly)
_DCM_BaseMAG.Z = 0.0;
//==================================================================
// Accelerometer-based and Magnetometer-based integral correction
// terms are cumulative by nature, so we need to reset their values
//==================================================================
VectorSet(0.0, 0.0, 0.0, &_DCMAccIterm);
VectorSet(0.0, 0.0, 0.0, &_DCMMagIterm);
//==================================================================
_DCMInit = 1; // DCM initialization is successful
// Timestamps of DCM initialization - later used as the last timestamp
// of respective sensor measurements
_DCM_GyroTS = _DCM_AccTS = _DCM_MagTS = TMRGetTS();
}
