void imuUpdateGyroAndAttitude(void)
{
/* Calculate dT */
static uint32_t previousIMUUpdateTime;
uint32_t currentTime = micros();
float dT = (currentTime - previousIMUUpdateTime) * 1e-6;
previousIMUUpdateTime = currentTime;
/* Update gyroscope */
gyroUpdate();
if (sensors(SENSOR_ACC) && isAccelUpdatedAtLeastOnce) {
#ifdef HIL
if (!hilActive) {
imuUpdateMeasuredRotationRate(); // Calculate gyro rate in body frame in rad/s
imuUpdateMeasuredAcceleration(); // Calculate accel in body frame in cm/s/s
imuCalculateEstimatedAttitude(dT); // Update attitude estimate
}
else {
imuHILUpdate();
imuUpdateMeasuredAcceleration();
}
#else
imuUpdateMeasuredRotationRate(); // Calculate gyro rate in body frame in rad/s
imuUpdateMeasuredAcceleration(); // Calculate accel in body frame in cm/s/s
imuCalculateEstimatedAttitude(dT); // Update attitude estimate
#endif
} else {
accADC[X] = 0;
accADC[Y] = 0;
accADC[Z] = 0;
}
}
// Function for loop trigger
void taskMainPidLoopCheck(void)
{
static bool runTaskMainSubprocesses;
static uint8_t pidUpdateCountdown;
cycleTime = getTaskDeltaTime(TASK_SELF);
if (debugMode == DEBUG_CYCLETIME) {
debug[0] = cycleTime;
debug[1] = averageSystemLoadPercent;
}
if (runTaskMainSubprocesses) {
subTaskMainSubprocesses();
runTaskMainSubprocesses = false;
}
gyroUpdate();
if (pidUpdateCountdown) {
pidUpdateCountdown--;
} else {
pidUpdateCountdown = setPidUpdateCountDown();
subTaskPidController();
subTaskMotorUpdate();
runTaskMainSubprocesses = true;
}
}
// Function for loop trigger
FAST_CODE void taskMainPidLoop(timeUs_t currentTimeUs)
{
static uint32_t pidUpdateCounter = 0;
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_GYROPID_SYNC)
if (lockMainPID() != 0) return;
#endif
// DEBUG_PIDLOOP, timings for:
// 0 - gyroUpdate()
// 1 - subTaskPidController()
// 2 - subTaskMotorUpdate()
// 3 - subTaskPidSubprocesses()
gyroUpdate(currentTimeUs);
DEBUG_SET(DEBUG_PIDLOOP, 0, micros() - currentTimeUs);
if (pidUpdateCounter++ % pidConfig()->pid_process_denom == 0) {
subTaskRcCommand(currentTimeUs);
subTaskPidController(currentTimeUs);
subTaskMotorUpdate(currentTimeUs);
subTaskPidSubprocesses(currentTimeUs);
}
if (debugMode == DEBUG_CYCLETIME) {
debug[0] = getTaskDeltaTime(TASK_SELF);
debug[1] = averageSystemLoadPercent;
}
}
void FalconBoard::qParamSensoresInerciales(QByteArray paquete)
{
qPacketParamSensoresInerciales_t *qinerciales;
qinerciales = (qPacketParamSensoresInerciales_t *)(paquete.data() + 4);
qinerciales->gx *= 180.0 / pi;
emit gyroUpdate(qinerciales->gx, qinerciales->gy, qinerciales->gz);
emit accUpdate(qinerciales->accx,qinerciales->accy, qinerciales->accz);
emit magUpdate(qinerciales->magx, qinerciales->magy, qinerciales->magz);
//qDebug() << qinerciales->gx << "," << qinerciales->gy << "," << qinerciales->gz;
}
void imuUpdateGyroAndAttitude(void)
{
gyroUpdate();
if (sensors(SENSOR_ACC) && isAccelUpdatedAtLeastOnce) {
imuCalculateEstimatedAttitude();
} else {
accADC[X] = 0;
accADC[Y] = 0;
accADC[Z] = 0;
}
}
void imuUpdate(rollAndPitchTrims_t *accelerometerTrims)
{
gyroUpdate();
if (sensors(SENSOR_ACC)) {
updateAccelerationReadings(accelerometerTrims); // TODO rename to accelerometerUpdate and rename many other 'Acceleration' references to be 'Accelerometer'
imuCalculateEstimatedAttitude();
} else {
accADC[X] = 0;
accADC[Y] = 0;
accADC[Z] = 0;
}
}
void taskGyro(void)
{
gyroUpdate();
gyroUpdateAt += US_FROM_HZ(gyro.sampleFrequencyHz);
gyroReadyCounter++;
gyroDeltaUs = getTaskDeltaTime(TASK_SELF);
if (debugMode == DEBUG_CYCLETIME) {
debug[0] = gyroDeltaUs;
debug[1] = averageSystemLoadPercent;
}
if (debugMode == DEBUG_GYRO_SYNC) {
debug[0] = gyroDeltaUs;
}
}
// Function for loop trigger
void taskMainPidLoop(timeUs_t currentTimeUs)
{
static bool runTaskMainSubprocesses;
static uint8_t pidUpdateCountdown;
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_GYROPID_SYNC)
if(lockMainPID() != 0) return;
#endif
if (debugMode == DEBUG_CYCLETIME) {
debug[0] = getTaskDeltaTime(TASK_SELF);
debug[1] = averageSystemLoadPercent;
}
if (runTaskMainSubprocesses) {
subTaskMainSubprocesses(currentTimeUs);
runTaskMainSubprocesses = false;
}
// DEBUG_PIDLOOP, timings for:
// 0 - gyroUpdate()
// 1 - pidController()
// 2 - subTaskMainSubprocesses()
// 3 - subTaskMotorUpdate()
uint32_t startTime = 0;
if (debugMode == DEBUG_PIDLOOP) {startTime = micros();}
gyroUpdate();
DEBUG_SET(DEBUG_PIDLOOP, 0, micros() - startTime);
if (pidUpdateCountdown) {
pidUpdateCountdown--;
} else {
pidUpdateCountdown = setPidUpdateCountDown();
subTaskPidController(currentTimeUs);
subTaskMotorUpdate();
runTaskMainSubprocesses = true;
}
}
uint8 bottomBoardISR() {
uint32 data, i;
uint32 timerLoadPeriod;
uint8 checksum;
long val;
TimerIntClear(TIMER1_BASE, TIMER_TIMB_TIMEOUT);
// Multiplex SPI (bottomboard/radio)
if (MSPSPIState == MSPSPI_STATE_XMIT_BYTE) {
radioIntDisable();
SPISelectDeviceISR(SPI_MSP430);
timerLoadPeriod = MSP430_SPI_DESELECT_PERIOD;
} else if (MSPSPIState == MSPSPI_STATE_DESELECT_SPI) {
SPIDeselectISR();
radioIntEnable();
if (bootloaderSet && MSP430MessageIdx == 0) {
// Deselect but then set the boot-loader to operate.
msp430BSLInit();
bootloaderSet = FALSE;
} else {
timerLoadPeriod = MSP430_SPI_BYTE_PERIOD;
}
}
// Set delay
MAP_TimerLoadSet(TIMER1_BASE, TIMER_B, timerLoadPeriod);
MAP_TimerEnable(TIMER1_BASE, TIMER_B);
// Transmit message to bottomboard
if (MSPSPIState == MSPSPI_STATE_XMIT_BYTE) {
// Pack new message at the beginning of each cycle
if (MSP430MessageIdx == 0) {
// Pack code
for (i = 0; i < MSP430_CODE_LENGTH; i++) {
MSP430MessageOut[i] = MSP430Code[i];
}
// Build and pack payload
switch (msp430SystemGetCommandMessage()) {
case MSP430_CMD_COMMAND_NORMAL:
blinkySystemBuildMessage(&MSP430MessageOut[MSP430_CMD_SYSTEM_LED_IDX]);
buttonsBuildMessage(&MSP430MessageOut[MSP430_CMD_BUTTONS_IDX]);
ledsBuildMessage(&MSP430MessageOut[MSP430_CMD_LED_IDX]);
break;
case MSP430_CMD_COMMAND_REPROGRAM:
bootloaderSet = TRUE;
break;
default:
// Do nothing for shutdown or reset
break;
}
msp430SystemCommandBuildMessage(&MSP430MessageOut[MSP430_CMD_COMMAND_IDX]);
MSP430MessageOut[MSP430_CMD_CHECKSUM_IDX] = messageChecksum(MSP430MessageOut, MSP430_CODE_LENGTH, MSP430_MSG_LENGTH);
}
// Shift receive buffer
shiftBuffer(MSP430MessageIn, MSP430_MSG_LENGTH);
// Put data into the transmit buffer
val = MAP_SSIDataPutNonBlocking(SSI0_BASE, (uint32)MSP430MessageOut[MSP430MessageIdx]);
if (val == 0) {
SPIBusError_SSIDataPutNonBlocking = 1;
}
// Retrieve the received byte
MAP_SSIDataGet(SSI0_BASE, &data);
// Put byte the the end of the receive buffer
MSP430MessageIn[MSP430_MSG_LENGTH - 1] = data;
// Check to see if we have a complete message, with correct code and checksum
if (MSP430MessageIdx == 0) {
// Checkcode
for (i = 0; i < MSP430_CODE_LENGTH; i++) {
if (MSP430MessageIn[i] != MSP430Code[i])
break;
}
if (i == MSP430_CODE_LENGTH) {
// Checksum
checksum = messageChecksum(MSP430MessageIn, MSP430_CODE_LENGTH, MSP430_MSG_LENGTH);
if (checksum == MSP430MessageIn[MSP430_MSG_LENGTH - 1]) {
// Process incoming message
bumpSensorsUpdate(MSP430MessageIn[MSP430_MSG_BUMPER_IDX]);
accelerometerUpdate(&MSP430MessageIn[MSP430_MSG_ACCEL_START_IDX]);
gyroUpdate(&MSP430MessageIn[MSP430_MSG_GYRO_START_IDX]);
systemBatteryVoltageUpdate(MSP430MessageIn[MSP430_MSG_VBAT_IDX]);
systemUSBVoltageUpdate(MSP430MessageIn[MSP430_MSG_VUSB_IDX]);
systemPowerButtonUpdate(MSP430MessageIn[MSP430_MSG_POWER_BUTTON_IDX]);
systemMSPVersionUpdate(MSP430MessageIn[MSP430_MSG_VERSION_IDX]);
reflectiveSensorsUpdate(&MSP430MessageIn[MSP430_MSG_REFLECT_START_IDX]);
if((MSP430MessageIn[MSP430_MSG_VERSION_IDX] != 0) && (!systemMSP430CommsValid)) {
systemMSP430CommsValid = TRUE;
}
} else {
checksumFailure++;
// Fail checksum
if (showError) {cprintf("Bsum ");}
}
} else {
// Fail checkcode
if (showError) {cprintf("Bcod ");}
}
// Toggle MSP boards, regardless of result
if (expand0En) {
MSPSelect = MSP_EXPAND0_BOARD_SEL;
}
}
// Increment index, wrap back to 0 if it exceeds the range
MSP430MessageIdx++;
if (MSP430MessageIdx >= MSP430_MSG_LENGTH) {
MSP430MessageIdx = 0;
}
// Toggle states
MSPSPIState = MSPSPI_STATE_DESELECT_SPI;
} else if (MSPSPIState == MSPSPI_STATE_DESELECT_SPI) {
// Toggle states
MSPSPIState = MSPSPI_STATE_XMIT_BYTE;
}
return 0;
}
