// called by hal ext IRQ handler
// (radio goes to stanby mode after tx/rx operations)
void radio_irq_handler (u1_t dio) {
ostime_t now = os_getTime();
if( (readReg(RegOpMode) & OPMODE_LORA) != 0) { // LORA modem
u1_t flags = readReg(LORARegIrqFlags);
if( flags & IRQ_LORA_TXDONE_MASK ) {
// save exact tx time
LMIC.txend = now - us2osticks(43); // TXDONE FIXUP
} else if( flags & IRQ_LORA_RXDONE_MASK ) {
// save exact rx time
if(getBw(LMIC.rps) == BW125) {
now -= LORA_RXDONE_FIXUP[getSf(LMIC.rps)];
}
LMIC.rxtime = now;
// read the PDU and inform the MAC that we received something
LMIC.dataLen = (readReg(LORARegModemConfig1) & SX1272_MC1_IMPLICIT_HEADER_MODE_ON) ?
readReg(LORARegPayloadLength) : readReg(LORARegRxNbBytes);
// set FIFO read address pointer
writeReg(LORARegFifoAddrPtr, readReg(LORARegFifoRxCurrentAddr));
// now read the FIFO
readBuf(RegFifo, LMIC.frame, LMIC.dataLen);
// read rx quality parameters
LMIC.snr = readReg(LORARegPktSnrValue); // SNR [dB] * 4
LMIC.rssi = readReg(LORARegPktRssiValue) - 125 + 64; // RSSI [dBm] (-196...+63)
} else if( flags & IRQ_LORA_RXTOUT_MASK ) {
// indicate timeout
LMIC.dataLen = 0;
}
// mask all radio IRQs
writeReg(LORARegIrqFlagsMask, 0xFF);
// clear radio IRQ flags
writeReg(LORARegIrqFlags, 0xFF);
} else { // FSK modem
u1_t flags1 = readReg(FSKRegIrqFlags1);
u1_t flags2 = readReg(FSKRegIrqFlags2);
if( flags2 & IRQ_FSK2_PACKETSENT_MASK ) {
// save exact tx time
LMIC.txend = now;
} else if( flags2 & IRQ_FSK2_PAYLOADREADY_MASK ) {
// save exact rx time
LMIC.rxtime = now;
// read the PDU and inform the MAC that we received something
LMIC.dataLen = readReg(FSKRegPayloadLength);
// now read the FIFO
readBuf(RegFifo, LMIC.frame, LMIC.dataLen);
// read rx quality parameters
LMIC.snr = 0; // determine snr
LMIC.rssi = 0; // determine rssi
} else if( flags1 & IRQ_FSK1_TIMEOUT_MASK ) {
// indicate timeout
LMIC.dataLen = 0;
} else {
//while(1);
// Do not infinite loop when interrupt triggers unexpectedly
// Instead, call ASSERT, which calls hal_failed, so there is at least a
// traceable error. (MK)
ASSERT(0);
}
}
// go from stanby to sleep
opmode(OPMODE_SLEEP);
// run os job (use preset func ptr)
os_setCallback(&LMIC.osjob, LMIC.osjob.func);
}
VectorXd rigidBodyDynamics::f(VectorXd x) {
Vector3d dr, dv, da, dw;
Matrix<double,12,12> lambda, dLambda;
VectorXd vec_dLambda;
int vecsize;
if (covProp) {
vecsize = 90;
} else {
vecsize = 12;
}
VectorXd dx(vecsize);
Vector3d r = x.segment<3>(0);
Vector3d v = x.segment<3>(3);
Vector3d a = x.segment<3>(6);
Vector3d w = x.segment<3>(9);
MatrixXd Bw = getBw();
Matrix3d J = _ir.getJ();
//Nonlinear State Model \dot x = f(x)
/*
* \mathbf{\dot r} = \mathbf{v}
*/
dr = v;
/*
* \mathbf{\dot v} = 0
*/
dv = Vector3d::Zero();
/*
* \frac{d \mathbf{a}_p}{dt} =
* \frac{1}{2}\left(\mathbf{[\omega \times]} +
* \mathbf{\omega} \cdot \mathbf{\bar q} \right) \mathbf{a}_p +
* \frac{2 q_4}{1+q_4} \mathbf{\omega}
*/
double c1, c2, c3;
c1 = 0.5;
c2 = 0.125 * w.dot(a); //NEW simplification
c3 = 1 - a.dot(a)/16;
da = -c1 * w.cross(a) + c2* a + c3 * w;
/*
* \dot \mathbf{w} = -\mathbf{J}^{-1} \mathbf{\omega} \times \mathbf{J} \mathbf{\omega}
*/
dw = - J.inverse() * w.cross(J * w);
if (covProp) {
//Covariance Propagation according to Lyapunov function
//see Brown & Hwang pg 204
//Compute Linear transition matrix
Matrix<double,12,12> A = Matrix<double,12,12>::Zero();
//position derivative
A.block<3,3>(0,3) = Matrix<double,3,3>::Identity();
//mrp kinematics
A.block<3,3>(6,6) = -0.5*crossProductMat(w) + w.dot(a)/8 * Matrix3d::Identity();
A.block<3,3>(6,9) = (1-a.dot(a/16))*Matrix3d::Identity();
//angular velocity dynamics
A.block<3,3>(9,9) = - J.inverse() * crossProductMat(w) * J;
lambda = vec2symmMat(x.segment<78>(12));
dLambda = A * lambda + lambda *A.transpose() + Bw * _Q * Bw.transpose();
vec_dLambda = symmMat2Vec(dLambda);
}
//write to dx
dx.segment<3>(0) = dr;
dx.segment<3>(3) = dv;
dx.segment<3>(6) = da;
dx.segment<3>(9) = dw;
if(covProp){
dx.segment<78>(12) = vec_dLambda;
}
return dx;
}
// configure LoRa modem (cfg1, cfg2)
static void configLoraModem () {
sf_t sf = getSf(LMIC.rps);
#ifdef CFG_sx1276_radio
u1_t mc1 = 0, mc2 = 0, mc3 = 0;
switch (getBw(LMIC.rps)) {
case BW125: mc1 |= SX1276_MC1_BW_125; break;
case BW250: mc1 |= SX1276_MC1_BW_250; break;
case BW500: mc1 |= SX1276_MC1_BW_500; break;
default:
ASSERT(0);
}
switch( getCr(LMIC.rps) ) {
case CR_4_5: mc1 |= SX1276_MC1_CR_4_5; break;
case CR_4_6: mc1 |= SX1276_MC1_CR_4_6; break;
case CR_4_7: mc1 |= SX1276_MC1_CR_4_7; break;
case CR_4_8: mc1 |= SX1276_MC1_CR_4_8; break;
default:
ASSERT(0);
}
if (getIh(LMIC.rps)) {
mc1 |= SX1276_MC1_IMPLICIT_HEADER_MODE_ON;
writeReg(LORARegPayloadLength, getIh(LMIC.rps)); // required length
}
// set ModemConfig1
writeReg(LORARegModemConfig1, mc1);
mc2 = (SX1272_MC2_SF7 + ((sf-1)<<4));
if (getNocrc(LMIC.rps) == 0) {
mc2 |= SX1276_MC2_RX_PAYLOAD_CRCON;
}
writeReg(LORARegModemConfig2, mc2);
mc3 = SX1276_MC3_AGCAUTO;
if ((sf == SF11 || sf == SF12) && getBw(LMIC.rps) == BW125) {
mc3 |= SX1276_MC3_LOW_DATA_RATE_OPTIMIZE;
}
writeReg(LORARegModemConfig3, mc3);
#elif CFG_sx1272_radio
u1_t mc1 = (getBw(LMIC.rps)<<6);
switch( getCr(LMIC.rps) ) {
case CR_4_5: mc1 |= SX1272_MC1_CR_4_5; break;
case CR_4_6: mc1 |= SX1272_MC1_CR_4_6; break;
case CR_4_7: mc1 |= SX1272_MC1_CR_4_7; break;
case CR_4_8: mc1 |= SX1272_MC1_CR_4_8; break;
}
if ((sf == SF11 || sf == SF12) && getBw(LMIC.rps) == BW125) {
mc1 |= SX1272_MC1_LOW_DATA_RATE_OPTIMIZE;
}
if (getNocrc(LMIC.rps) == 0) {
mc1 |= SX1272_MC1_RX_PAYLOAD_CRCON;
}
if (getIh(LMIC.rps)) {
mc1 |= SX1272_MC1_IMPLICIT_HEADER_MODE_ON;
writeReg(LORARegPayloadLength, getIh(LMIC.rps)); // required length
}
// set ModemConfig1
writeReg(LORARegModemConfig1, mc1);
// set ModemConfig2 (sf, AgcAutoOn=1 SymbTimeoutHi=00)
writeReg(LORARegModemConfig2, (SX1272_MC2_SF7 + ((sf-1)<<4)) | 0x04);
#else
#error Missing CFG_sx1272_radio/CFG_sx1276_radio
#endif /* CFG_sx1272_radio */
}