void annexCode() { // this code is excetuted at each loop and won't interfere with control loop if it lasts less than 650 microseconds
static uint32_t calibratedAccTime;
uint16_t tmp,tmp2;
uint8_t axis,prop1,prop2;
#define BREAKPOINT 1500
// PITCH & ROLL only dynamic PID adjustemnt, depending on throttle value
if (rcData[THROTTLE]<BREAKPOINT) {
prop2 = 100;
} else {
if (rcData[THROTTLE]<2000) {
prop2 = 100 - (uint16_t)conf.dynThrPID*(rcData[THROTTLE]-BREAKPOINT)/(2000-BREAKPOINT);
} else {
prop2 = 100 - conf.dynThrPID;
}
}
for(axis=0;axis<3;axis++) {
tmp = min(abs(rcData[axis]-conf.MIDRC),500);
if(axis!=2) { //ROLL & PITCH
tmp2 = tmp/100;
rcCommand[axis] = lookupPitchRollRC[tmp2] + (tmp-tmp2*100) * (lookupPitchRollRC[tmp2+1]-lookupPitchRollRC[tmp2]) / 100;
prop1 = 100-(uint16_t)conf.rollPitchRate*tmp/500;
prop1 = (uint16_t)prop1*prop2/100;
} else { // YAW
rcCommand[axis] = tmp;
prop1 = 100-(uint16_t)conf.yawRate*tmp/500;
}
dynP8[axis] = (uint16_t)conf.P8[axis]*prop1/100;
dynD8[axis] = (uint16_t)conf.D8[axis]*prop1/100;
if (rcData[axis]<conf.MIDRC) rcCommand[axis] = -rcCommand[axis];
}
tmp = constrain(rcData[THROTTLE],conf.MINCHECK,2000);
tmp = (uint32_t)(tmp-conf.MINCHECK)*1000/(2000-conf.MINCHECK); // [MINCHECK;2000] -> [0;1000]
tmp2 = tmp/100;
rcCommand[THROTTLE] = lookupThrottleRC[tmp2] + (tmp-tmp2*100) * (lookupThrottleRC[tmp2+1]-lookupThrottleRC[tmp2]) / 100; // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE]
if ( (calibratingA>0 && ACC ) || (calibratingG>0) ) { // Calibration phasis
//LEDPIN_TOGGLE; TBD
} else {
//if (f.ACC_CALIBRATED) {LEDPIN_OFF;} TBD
//if (f.ARMED) {LEDPIN_ON;} TBD
}
if ( currentTime > calibratedAccTime ) {
if (! f.SMALL_ANGLES_25) {
// the multi uses ACC and is not calibrated or is too much inclinated
f.ACC_CALIBRATED = 0;
//LEDPIN_TOGGLE; TBD
calibratedAccTime = currentTime + 500000;
} else {
f.ACC_CALIBRATED = 1;
}
}
serialCom();
}
void annexCode(void)
{
static uint32_t calibratedAccTime;
int32_t tmp, tmp2;
int32_t axis, prop1, prop2;
static uint8_t buzzerFreq; // delay between buzzer ring
// vbat shit
static uint8_t vbatTimer = 0;
static int32_t vbatRaw = 0;
static int32_t amperageRaw = 0;
static int64_t mAhdrawnRaw = 0;
static int32_t vbatCycleTime = 0;
// PITCH & ROLL only dynamic PID adjustemnt, depending on throttle value
if (rcData[THROTTLE] < cfg.tpa_breakpoint) {
prop2 = 100;
} else {
if (rcData[THROTTLE] < 2000) {
prop2 = 100 - (uint16_t)cfg.dynThrPID * (rcData[THROTTLE] - cfg.tpa_breakpoint) / (2000 - cfg.tpa_breakpoint);
} else {
prop2 = 100 - cfg.dynThrPID;
}
}
for (axis = 0; axis < 3; axis++) {
tmp = min(abs(rcData[axis] - mcfg.midrc), 500);
if (axis != 2) { // ROLL & PITCH
if (cfg.deadband) {
if (tmp > cfg.deadband) {
tmp -= cfg.deadband;
} else {
tmp = 0;
}
}
tmp2 = tmp / 100;
rcCommand[axis] = lookupPitchRollRC[tmp2] + (tmp - tmp2 * 100) * (lookupPitchRollRC[tmp2 + 1] - lookupPitchRollRC[tmp2]) / 100;
prop1 = 100 - (uint16_t)cfg.rollPitchRate * tmp / 500;
prop1 = (uint16_t)prop1 * prop2 / 100;
} else { // YAW
if (cfg.yawdeadband) {
if (tmp > cfg.yawdeadband) {
tmp -= cfg.yawdeadband;
} else {
tmp = 0;
}
}
rcCommand[axis] = tmp * -mcfg.yaw_control_direction;
prop1 = 100 - (uint16_t)cfg.yawRate * abs(tmp) / 500;
}
dynP8[axis] = (uint16_t)cfg.P8[axis] * prop1 / 100;
dynI8[axis] = (uint16_t)cfg.I8[axis] * prop1 / 100;
dynD8[axis] = (uint16_t)cfg.D8[axis] * prop1 / 100;
if (rcData[axis] < mcfg.midrc)
rcCommand[axis] = -rcCommand[axis];
}
tmp = constrain(rcData[THROTTLE], mcfg.mincheck, 2000);
tmp = (uint32_t)(tmp - mcfg.mincheck) * 1000 / (2000 - mcfg.mincheck); // [MINCHECK;2000] -> [0;1000]
tmp2 = tmp / 100;
rcCommand[THROTTLE] = lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100; // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE]
if (f.HEADFREE_MODE) {
float radDiff = (heading - headFreeModeHold) * M_PI / 180.0f;
float cosDiff = cosf(radDiff);
float sinDiff = sinf(radDiff);
int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff;
rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff;
rcCommand[PITCH] = rcCommand_PITCH;
}
if (feature(FEATURE_VBAT)) {
vbatCycleTime += cycleTime;
if (!(++vbatTimer % VBATFREQ)) {
vbatRaw -= vbatRaw / 8;
vbatRaw += adcGetChannel(ADC_BATTERY);
vbat = batteryAdcToVoltage(vbatRaw / 8);
if (mcfg.power_adc_channel > 0) {
amperageRaw -= amperageRaw / 8;
amperageRaw += adcGetChannel(ADC_EXTERNAL_CURRENT);
amperage = currentSensorToCentiamps(amperageRaw / 8);
mAhdrawnRaw += (amperage * vbatCycleTime) / 1000;
mAhdrawn = mAhdrawnRaw / (3600 * 100);
vbatCycleTime = 0;
}
}
if ((vbat > batteryWarningVoltage) || (vbat < mcfg.vbatmincellvoltage)) { // VBAT ok, buzzer off
buzzerFreq = 0;
} else
buzzerFreq = 4; // low battery
}
buzzer(buzzerFreq); // external buzzer routine that handles buzzer events globally now
if ((calibratingA > 0 && sensors(SENSOR_ACC)) || (calibratingG > 0)) { // Calibration phasis
LED0_TOGGLE;
} else {
if (f.ACC_CALIBRATED)
LED0_OFF;
if (f.ARMED)
LED0_ON;
#ifndef CJMCU
checkTelemetryState();
#endif
}
#ifdef LEDRING
if (feature(FEATURE_LED_RING)) {
static uint32_t LEDTime;
if ((int32_t)(currentTime - LEDTime) >= 0) {
LEDTime = currentTime + 50000;
ledringState();
}
}
#endif
if ((int32_t)(currentTime - calibratedAccTime) >= 0) {
if (!f.SMALL_ANGLE) {
f.ACC_CALIBRATED = 0; // the multi uses ACC and is not calibrated or is too much inclinated
LED0_TOGGLE;
calibratedAccTime = currentTime + 500000;
} else {
f.ACC_CALIBRATED = 1;
}
}
serialCom();
#ifndef CJMCU
if (!cliMode && feature(FEATURE_TELEMETRY)) {
handleTelemetry();
}
#endif
if (sensors(SENSOR_GPS)) {
static uint32_t GPSLEDTime;
if ((int32_t)(currentTime - GPSLEDTime) >= 0 && (GPS_numSat >= 5)) {
GPSLEDTime = currentTime + 150000;
LED1_TOGGLE;
}
}
// Read out gyro temperature. can use it for something somewhere. maybe get MCU temperature instead? lots of fun possibilities.
if (gyro.temperature)
gyro.temperature(&telemTemperature1);
else {
// TODO MCU temp
}
}
// this code is executed at each loop and won't interfere with control loop if it lasts less than 650 microseconds
void annexCode(void)
{
static uint32_t calibratedAccTime;
uint16_t tmp, tmp2;
static uint8_t buzzerFreq; //delay between buzzer ring
static uint8_t vbatTimer = 0;
uint8_t axis, prop1, prop2;
static uint8_t ind = 0;
uint16_t vbatRaw = 0;
static uint16_t vbatRawArray[8];
uint8_t i;
// PITCH & ROLL only dynamic PID adjustemnt, depending on throttle value
if (rcData[THROTTLE] < BREAKPOINT) {
prop2 = 100;
} else {
if (rcData[THROTTLE] < 2000) {
prop2 = 100 - (uint16_t) cfg.dynThrPID * (rcData[THROTTLE] - BREAKPOINT) / (2000 - BREAKPOINT);
} else {
prop2 = 100 - cfg.dynThrPID;
}
}
for (axis = 0; axis < 3; axis++) {
tmp = min(abs(rcData[axis] - cfg.midrc), 500);
if (axis != 2) { // ROLL & PITCH
if (cfg.deadband) {
if (tmp > cfg.deadband) {
tmp -= cfg.deadband;
} else {
tmp = 0;
}
}
tmp2 = tmp / 100;
rcCommand[axis] = lookupPitchRollRC[tmp2] + (tmp - tmp2 * 100) * (lookupPitchRollRC[tmp2 + 1] - lookupPitchRollRC[tmp2]) / 100;
prop1 = 100 - (uint16_t) cfg.rollPitchRate * tmp / 500;
prop1 = (uint16_t) prop1 *prop2 / 100;
} else { // YAW
if (cfg.yawdeadband) {
if (tmp > cfg.yawdeadband) {
tmp -= cfg.yawdeadband;
} else {
tmp = 0;
}
}
rcCommand[axis] = tmp;
prop1 = 100 - (uint16_t) cfg.yawRate * tmp / 500;
}
dynP8[axis] = (uint16_t) cfg.P8[axis] * prop1 / 100;
dynD8[axis] = (uint16_t) cfg.D8[axis] * prop1 / 100;
if (rcData[axis] < cfg.midrc)
rcCommand[axis] = -rcCommand[axis];
}
tmp = constrain(rcData[THROTTLE], cfg.mincheck, 2000);
tmp = (uint32_t) (tmp - cfg.mincheck) * 1000 / (2000 - cfg.mincheck); // [MINCHECK;2000] -> [0;1000]
tmp2 = tmp / 100;
rcCommand[THROTTLE] = lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100; // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE]
if(f.HEADFREE_MODE) {
float radDiff = (heading - headFreeModeHold) * M_PI / 180.0f;
float cosDiff = cosf(radDiff);
float sinDiff = sinf(radDiff);
int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff;
rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff;
rcCommand[PITCH] = rcCommand_PITCH;
}
if (feature(FEATURE_VBAT)) {
if (!(++vbatTimer % VBATFREQ)) {
vbatRawArray[(ind++) % 8] = adcGetBattery();
for (i = 0; i < 8; i++)
vbatRaw += vbatRawArray[i];
vbat = batteryAdcToVoltage(vbatRaw / 8);
}
if ((vbat > batteryWarningVoltage) || (vbat < cfg.vbatmincellvoltage)) { // VBAT ok, buzzer off
buzzerFreq = 0;
} else
buzzerFreq = 4; // low battery
}
buzzer(buzzerFreq); // external buzzer routine that handles buzzer events globally now
if ((calibratingA > 0 && sensors(SENSOR_ACC)) || (calibratingG > 0)) { // Calibration phasis
LED0_TOGGLE;
} else {
if (f.ACC_CALIBRATED)
LED0_OFF;
if (f.ARMED)
LED0_ON;
// This will switch to/from 9600 or 115200 baud depending on state. Of course, it should only do it on changes.
if (feature(FEATURE_TELEMETRY))
initTelemetry(f.ARMED);
}
#ifdef LEDRING
if (feature(FEATURE_LED_RING)) {
static uint32_t LEDTime;
if ((int32_t)(currentTime - LEDTime) >= 0) {
LEDTime = currentTime + 50000;
ledringState();
}
}
#endif
if ((int32_t)(currentTime - calibratedAccTime) >= 0) {
if (!f.SMALL_ANGLES_25) {
f.ACC_CALIBRATED = 0; // the multi uses ACC and is not calibrated or is too much inclinated
LED0_TOGGLE;
calibratedAccTime = currentTime + 500000;
} else {
f.ACC_CALIBRATED = 1;
}
}
serialCom();
if (sensors(SENSOR_GPS)) {
static uint32_t GPSLEDTime;
if ((int32_t)(currentTime - GPSLEDTime) >= 0 && (GPS_numSat >= 5)) {
GPSLEDTime = currentTime + 150000;
LED1_TOGGLE;
}
}
// Read out gyro temperature. can use it for something somewhere. maybe get MCU temperature instead? lots of fun possibilities.
if (gyro.temperature)
gyro.temperature(&telemTemperature1);
else {
// TODO MCU temp
}
}
int main(void)
{
uint32_t currentTime;
// High Speed Telemetry Test Code Begin
char numberString[12];
// High Speed Telemetry Test Code End
RCC_GetClocksFreq(&clocks);
USB_Interrupts_Config();
Set_USBClock();
USB_Init();
// Wait until device configured
//while(bDeviceState != CONFIGURED);
testInit();
LED0_ON;
systemReady = true;
//nrf_tx_mode_no_aa(addr,5,40);
nrf_rx_mode_dual(addr,5,40);
{
uint8_t status = nrf_read_reg(NRF_STATUS);
nrf_write_reg(NRF_WRITE_REG|NRF_STATUS,status); // clear IRQ flags
nrf_write_reg(NRF_FLUSH_RX, 0xff);
nrf_write_reg(NRF_FLUSH_TX, 0xff);
}
while (1)
{
uint8_t buf[64];
static uint8_t last_tx_done = 0;
if(ring_buf_pop(nrf_rx_buffer,buf,32)){
// get data from the adapter
switch(buf[0]){
case SET_ATT:
break;
case SET_MOTOR:
break;
case SET_MODE:
report_mode = buf[1];
break;
}
last_tx_done = 1;
}
if(tx_done){
tx_done = 0;
// report ACK success
last_tx_done = 1;
}
if(ring_buf_pop(nrf_tx_buffer,buf,32)){
if(last_tx_done){
last_tx_done = 0;
nrf_ack_packet(0, buf, 32);
}
}
if (frame_50Hz)
{
int16_t motor_val[4];
frame_50Hz = false;
currentTime = micros();
deltaTime50Hz = currentTime - previous50HzTime;
previous50HzTime = currentTime;
//memcpy(buf, accelSummedSamples100Hz, 12);
//memcpy(buf+12, gyroSummedSamples100Hz, 12);
//memcpy(buf+24, magSumed, 6);
if(report_mode == DT_ATT){
buf[0] = DT_ATT;
memcpy(buf + 1, &sensors.attitude200Hz[0], 12);
memcpy(buf + 13, &executionTime200Hz, 4);
motor_val[0] = motor[0];
motor_val[1] = motor[1];
motor_val[2] = motor[2];
motor_val[3] = motor[3];
memcpy(buf + 17, motor_val, 8);
usb_send_data(buf , 64);
executionTime50Hz = micros() - currentTime;
}else if(report_mode == DT_SENSOR){
buf[0] = DT_SENSOR;
memcpy(buf + 1, gyroSummedSamples100Hz, 12);
memcpy(buf + 13, accelSummedSamples100Hz, 12);
memcpy(buf + 25, magSumed, 6);
}
//nrf_tx_packet(buf,16);
//if(nrf_rx_packet(buf,16) == NRF_RX_OK)
//{
// LED0_TOGGLE;
//}
ring_buf_push(nrf_tx_buffer, buf, 32);
}
if(frame_10Hz)
{
frame_10Hz = false;
magSumed[XAXIS] = magSum[XAXIS];
magSumed[YAXIS] = magSum[YAXIS];
magSumed[ZAXIS] = magSum[ZAXIS];
magSum[XAXIS] = 0;
magSum[YAXIS] = 0;
magSum[ZAXIS] = 0;
newMagData = true;
}
if (frame_100Hz)
{
frame_100Hz = false;
computeAxisCommands(dt100Hz);
mixTable();
writeServos();
writeMotors();
}
if (frame_200Hz)
{
frame_200Hz = false;
currentTime = micros();
deltaTime200Hz = currentTime - previous200HzTime;
previous200HzTime = currentTime;
dt200Hz = (float)deltaTime200Hz * 0.000001f; // For integrations in 200 Hz loop
#if defined(USE_MADGWICK_AHRS) | defined(USE_MARG_AHRS)
sensors.accel200Hz[XAXIS] = -((float)accelSummedSamples200Hz[XAXIS] / 5.0f - accelRTBias[XAXIS] - sensorConfig.accelBias[XAXIS]) * sensorConfig.accelScaleFactor[XAXIS];
sensors.accel200Hz[YAXIS] = -((float)accelSummedSamples200Hz[YAXIS] / 5.0f - accelRTBias[YAXIS] - sensorConfig.accelBias[YAXIS]) * sensorConfig.accelScaleFactor[YAXIS];
sensors.accel200Hz[ZAXIS] = -((float)accelSummedSamples200Hz[ZAXIS] / 5.0f - accelRTBias[ZAXIS] - sensorConfig.accelBias[ZAXIS]) * sensorConfig.accelScaleFactor[ZAXIS];
sensors.accel200Hz[XAXIS] = computeFourthOrder200Hz(sensors.accel200Hz[XAXIS], &fourthOrder200Hz[AX_FILTER]);
sensors.accel200Hz[YAXIS] = computeFourthOrder200Hz(sensors.accel200Hz[YAXIS], &fourthOrder200Hz[AY_FILTER]);
sensors.accel200Hz[ZAXIS] = computeFourthOrder200Hz(sensors.accel200Hz[ZAXIS], &fourthOrder200Hz[AZ_FILTER]);
computeGyroTCBias();
sensors.gyro200Hz[ROLL ] = ((float)gyroSummedSamples200Hz[ROLL] / 5.0f - gyroRTBias[ROLL ] - gyroTCBias[ROLL ]) * GYRO_SCALE_FACTOR;
sensors.gyro200Hz[PITCH] = -((float)gyroSummedSamples200Hz[PITCH] / 5.0f - gyroRTBias[PITCH] - gyroTCBias[PITCH]) * GYRO_SCALE_FACTOR;
sensors.gyro200Hz[YAW ] = -((float)gyroSummedSamples200Hz[YAW] / 5.0f - gyroRTBias[YAW ] - gyroTCBias[YAW ]) * GYRO_SCALE_FACTOR;
#endif
#if defined(USE_MADGWICK_AHRS)
MadgwickAHRSupdate( sensors.gyro200Hz[ROLL], sensors.gyro200Hz[PITCH], sensors.gyro200Hz[YAW],
sensors.accel200Hz[XAXIS], sensors.accel200Hz[YAXIS], sensors.accel200Hz[ZAXIS],
sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS],
sensorConfig.accelCutoff,
newMagData,
dt200Hz );
newMagData = false;
q0q0 = q0 * q0;
q1q1 = q1 * q1;
q2q2 = q2 * q2;
q3q3 = q3 * q3;
sensors.attitude200Hz[ROLL ] = atan2f( 2.0f * (q0 * q1 + q2 * q3), q0q0 - q1q1 - q2q2 + q3q3 );
sensors.attitude200Hz[PITCH] = -asinf( 2.0f * (q1 * q3 - q0 * q2) );
sensors.attitude200Hz[YAW ] = atan2f( 2.0f * (q1 * q2 + q0 * q3), q0q0 + q1q1 - q2q2 - q3q3 );
#endif
#if defined(USE_MARG_AHRS)
MargAHRSupdate( sensors.gyro200Hz[ROLL], sensors.gyro200Hz[PITCH], sensors.gyro200Hz[YAW],
sensors.accel200Hz[XAXIS], sensors.accel200Hz[YAXIS], sensors.accel200Hz[ZAXIS],
sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS],
sensorConfig.accelCutoff,
newMagData,
dt200Hz );
newMagData = false;
q0q0 = q0 * q0;
q1q1 = q1 * q1;
q2q2 = q2 * q2;
q3q3 = q3 * q3;
sensors.attitude200Hz[ROLL ] = atan2f( 2.0f * (q0 * q1 + q2 * q3), q0q0 - q1q1 - q2q2 + q3q3 );
sensors.attitude200Hz[PITCH] = -asinf( 2.0f * (q1 * q3 - q0 * q2) );
sensors.attitude200Hz[YAW ] = atan2f( 2.0f * (q1 * q2 + q0 * q3), q0q0 + q1q1 - q2q2 - q3q3 );
#endif
executionTime200Hz = micros() - currentTime;
}
}
systemInit();
systemReady = true;
while (1)
{
///////////////////////////////
if (frame_50Hz)
{
frame_50Hz = false;
currentTime = micros();
deltaTime50Hz = currentTime - previous50HzTime;
previous50HzTime = currentTime;
processFlightCommands();
executionTime50Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_10Hz)
{
LED0_TOGGLE;
frame_10Hz = false;
currentTime = micros();
deltaTime10Hz = currentTime - previous10HzTime;
previous10HzTime = currentTime;
sensors.mag10Hz[XAXIS] = -((float)magSum[XAXIS] / 5.0f * magScaleFactor[XAXIS] - sensorConfig.magBias[XAXIS]);
sensors.mag10Hz[YAXIS] = (float)magSum[YAXIS] / 5.0f * magScaleFactor[YAXIS] - sensorConfig.magBias[YAXIS];
sensors.mag10Hz[ZAXIS] = -((float)magSum[ZAXIS] / 5.0f * magScaleFactor[ZAXIS] - sensorConfig.magBias[ZAXIS]);
magSum[XAXIS] = 0;
magSum[YAXIS] = 0;
magSum[ZAXIS] = 0;
newMagData = true;
pressureAverage = pressureSum / 10;
pressureSum = 0;
calculateTemperature();
calculatePressureAltitude();
sensors.pressureAltitude10Hz = pressureAlt;
serialCom();
if ( EKF_Initialized == false ) EKF_Init( sensors.accel100Hz[XAXIS], sensors.accel100Hz[YAXIS], sensors.accel100Hz[ZAXIS],
sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS] );
executionTime10Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_500Hz)
{
frame_500Hz = false;
currentTime = micros();
deltaTime500Hz = currentTime - previous500HzTime;
previous500HzTime = currentTime;
dt500Hz = (float)deltaTime500Hz * 0.000001f; // For integrations in 500 Hz loop
computeGyroTCBias();
sensors.gyro500Hz[ROLL ] = ((float)gyroSummedSamples500Hz[ROLL] / 2.0f - gyroRTBias[ROLL ] - gyroTCBias[ROLL ]) * GYRO_SCALE_FACTOR;
sensors.gyro500Hz[PITCH] = -((float)gyroSummedSamples500Hz[PITCH] / 2.0f - gyroRTBias[PITCH] - gyroTCBias[PITCH]) * GYRO_SCALE_FACTOR;
sensors.gyro500Hz[YAW ] = -((float)gyroSummedSamples500Hz[YAW] / 2.0f - gyroRTBias[YAW ] - gyroTCBias[YAW ]) * GYRO_SCALE_FACTOR;
#if defined(USE_CHR6DM_AHRS)
if ( EKF_Initialized == true ) EKF_Predict( sensors.gyro500Hz[ROLL], sensors.gyro500Hz[PITCH], sensors.gyro500Hz[YAW],
dt500Hz );
sensors.attitude200Hz[ROLL ] = gEstimatedStates.phi;
sensors.attitude200Hz[PITCH] = gEstimatedStates.theta;
sensors.attitude200Hz[YAW ] = gEstimatedStates.psi;
#endif
executionTime500Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_200Hz)
{
frame_200Hz = false;
currentTime = micros();
deltaTime200Hz = currentTime - previous200HzTime;
previous200HzTime = currentTime;
dt200Hz = (float)deltaTime200Hz * 0.000001f; // For integrations in 200 Hz loop
#if defined(USE_MADGWICK_AHRS) | defined(USE_MARG_AHRS)
sensors.accel200Hz[XAXIS] = -((float)accelSummedSamples200Hz[XAXIS] / 5.0f - accelRTBias[XAXIS] - sensorConfig.accelBias[XAXIS]) * sensorConfig.accelScaleFactor[XAXIS];
sensors.accel200Hz[YAXIS] = -((float)accelSummedSamples200Hz[YAXIS] / 5.0f - accelRTBias[YAXIS] - sensorConfig.accelBias[YAXIS]) * sensorConfig.accelScaleFactor[YAXIS];
sensors.accel200Hz[ZAXIS] = -((float)accelSummedSamples200Hz[ZAXIS] / 5.0f - accelRTBias[ZAXIS] - sensorConfig.accelBias[ZAXIS]) * sensorConfig.accelScaleFactor[ZAXIS];
sensors.accel200Hz[XAXIS] = computeFourthOrder200Hz(sensors.accel200Hz[XAXIS], &fourthOrder200Hz[AX_FILTER]);
sensors.accel200Hz[YAXIS] = computeFourthOrder200Hz(sensors.accel200Hz[YAXIS], &fourthOrder200Hz[AY_FILTER]);
sensors.accel200Hz[ZAXIS] = computeFourthOrder200Hz(sensors.accel200Hz[ZAXIS], &fourthOrder200Hz[AZ_FILTER]);
computeGyroTCBias();
sensors.gyro200Hz[ROLL ] = ((float)gyroSummedSamples200Hz[ROLL] / 5.0f - gyroRTBias[ROLL ] - gyroTCBias[ROLL ]) * GYRO_SCALE_FACTOR;
sensors.gyro200Hz[PITCH] = -((float)gyroSummedSamples200Hz[PITCH] / 5.0f - gyroRTBias[PITCH] - gyroTCBias[PITCH]) * GYRO_SCALE_FACTOR;
sensors.gyro200Hz[YAW ] = -((float)gyroSummedSamples200Hz[YAW] / 5.0f - gyroRTBias[YAW ] - gyroTCBias[YAW ]) * GYRO_SCALE_FACTOR;
#endif
#if defined(USE_MADGWICK_AHRS)
MadgwickAHRSupdate( sensors.gyro200Hz[ROLL], sensors.gyro200Hz[PITCH], sensors.gyro200Hz[YAW],
sensors.accel200Hz[XAXIS], sensors.accel200Hz[YAXIS], sensors.accel200Hz[ZAXIS],
sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS],
sensorConfig.accelCutoff,
newMagData,
dt200Hz );
newMagData = false;
q0q0 = q0 * q0;
q1q1 = q1 * q1;
q2q2 = q2 * q2;
q3q3 = q3 * q3;
sensors.attitude200Hz[ROLL ] = atan2f( 2.0f * (q0 * q1 + q2 * q3), q0q0 - q1q1 - q2q2 + q3q3 );
sensors.attitude200Hz[PITCH] = -asinf( 2.0f * (q1 * q3 - q0 * q2) );
sensors.attitude200Hz[YAW ] = atan2f( 2.0f * (q1 * q2 + q0 * q3), q0q0 + q1q1 - q2q2 - q3q3 );
#endif
#if defined(USE_MARG_AHRS)
MargAHRSupdate( sensors.gyro200Hz[ROLL], sensors.gyro200Hz[PITCH], sensors.gyro200Hz[YAW],
sensors.accel200Hz[XAXIS], sensors.accel200Hz[YAXIS], sensors.accel200Hz[ZAXIS],
sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS],
sensorConfig.accelCutoff,
newMagData,
dt200Hz );
newMagData = false;
q0q0 = q0 * q0;
q1q1 = q1 * q1;
q2q2 = q2 * q2;
q3q3 = q3 * q3;
sensors.attitude200Hz[ROLL ] = atan2f( 2.0f * (q0 * q1 + q2 * q3), q0q0 - q1q1 - q2q2 + q3q3 );
sensors.attitude200Hz[PITCH] = -asinf( 2.0f * (q1 * q3 - q0 * q2) );
sensors.attitude200Hz[YAW ] = atan2f( 2.0f * (q1 * q2 + q0 * q3), q0q0 + q1q1 - q2q2 - q3q3 );
#endif
executionTime200Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_100Hz)
{
frame_100Hz = false;
currentTime = micros();
deltaTime100Hz = currentTime - previous100HzTime;
previous100HzTime = currentTime;
dt100Hz = (float)deltaTime100Hz * 0.000001f; // For integrations in 100 Hz loop
sensors.accel100Hz[XAXIS] = -((float)accelSummedSamples100Hz[XAXIS] / 10.0f - accelRTBias[XAXIS] - sensorConfig.accelBias[XAXIS]) * sensorConfig.accelScaleFactor[XAXIS];
sensors.accel100Hz[YAXIS] = -((float)accelSummedSamples100Hz[YAXIS] / 10.0f - accelRTBias[YAXIS] - sensorConfig.accelBias[YAXIS]) * sensorConfig.accelScaleFactor[YAXIS];
sensors.accel100Hz[ZAXIS] = -((float)accelSummedSamples100Hz[ZAXIS] / 10.0f - accelRTBias[ZAXIS] - sensorConfig.accelBias[ZAXIS]) * sensorConfig.accelScaleFactor[ZAXIS];
sensors.accel100Hz[XAXIS] = computeFourthOrder100Hz(sensors.accel100Hz[XAXIS], &fourthOrder100Hz[AX_FILTER]);
sensors.accel100Hz[YAXIS] = computeFourthOrder100Hz(sensors.accel100Hz[YAXIS], &fourthOrder100Hz[AY_FILTER]);
sensors.accel100Hz[ZAXIS] = computeFourthOrder100Hz(sensors.accel100Hz[ZAXIS], &fourthOrder100Hz[AZ_FILTER]);
computeGyroTCBias();
sensors.gyro100Hz[ROLL ] = ((float)gyroSummedSamples100Hz[ROLL] / 10.0f - gyroRTBias[ROLL ] - gyroTCBias[ROLL ]) * GYRO_SCALE_FACTOR;
sensors.gyro100Hz[PITCH] = -((float)gyroSummedSamples100Hz[PITCH] / 10.0f - gyroRTBias[PITCH] - gyroTCBias[PITCH]) * GYRO_SCALE_FACTOR;
sensors.gyro100Hz[YAW ] = -((float)gyroSummedSamples100Hz[YAW] / 10.0f - gyroRTBias[YAW ] - gyroTCBias[YAW ]) * GYRO_SCALE_FACTOR;
#if defined(USE_CHR6DM_AHRS)
if ( EKF_Initialized == true ) EKF_Update( sensors.accel100Hz[XAXIS], sensors.accel100Hz[YAXIS], sensors.accel100Hz[ZAXIS],
sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS],
sensorConfig.accelCutoff,
newMagData );
newMagData = false;
sensors.attitude200Hz[ROLL ] = gEstimatedStates.phi;
sensors.attitude200Hz[PITCH] = gEstimatedStates.theta;
sensors.attitude200Hz[YAW ] = gEstimatedStates.psi;
#endif
computeAxisCommands(dt100Hz);
mixTable();
writeServos();
writeMotors();
// High Speed Telemetry Test Code Begin
if ( highSpeedAccelTelemEnabled == true )
{
// 100 Hz Accels
ftoa(sensors.accel100Hz[XAXIS], numberString); uartPrint(numberString); uartPrint(",");
ftoa(sensors.accel100Hz[YAXIS], numberString); uartPrint(numberString); uartPrint(",");
ftoa(sensors.accel100Hz[ZAXIS], numberString); uartPrint(numberString); uartPrint("\n");
}
if ( highSpeedGyroTelemEnabled == true )
{
// 100 Hz Gyros
ftoa(sensors.gyro100Hz[ROLL ], numberString); uartPrint(numberString); uartPrint(",");
ftoa(sensors.gyro100Hz[PITCH], numberString); uartPrint(numberString); uartPrint(",");
ftoa(sensors.gyro100Hz[YAW ], numberString); uartPrint(numberString); uartPrint("\n");
}
if ( highSpeedRollRateTelemEnabled == true )
{
// Roll Rate, Roll Rate Command
ftoa(sensors.gyro100Hz[ROLL], numberString); uartPrint(numberString); uartPrint(",");
ftoa(rxCommand[ROLL], numberString); uartPrint(numberString); uartPrint("\n");
}
if ( highSpeedPitchRateTelemEnabled == true )
{
// Pitch Rate, Pitch Rate Command
ftoa(sensors.gyro100Hz[PITCH], numberString); uartPrint(numberString); uartPrint(",");
ftoa(rxCommand[PITCH], numberString); uartPrint(numberString); uartPrint("\n");
}
if ( highSpeedYawRateTelemEnabled == true )
{
// Yaw Rate, Yaw Rate Command
ftoa(sensors.gyro100Hz[YAW], numberString); uartPrint(numberString); uartPrint(",");
ftoa(rxCommand[YAW], numberString); uartPrint(numberString); uartPrint("\n");
}
if ( highSpeedAttitudeTelemEnabled == true )
{
// 200 Hz Attitudes
ftoa(sensors.attitude200Hz[ROLL ], numberString); uartPrint(numberString); uartPrint(",");
ftoa(sensors.attitude200Hz[PITCH], numberString); uartPrint(numberString); uartPrint(",");
ftoa(sensors.attitude200Hz[YAW ], numberString); uartPrint(numberString); uartPrint("\n");
}
// High Speed Telemetry Test Code End
executionTime100Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_5Hz)
{
frame_5Hz = false;
currentTime = micros();
deltaTime5Hz = currentTime - previous5HzTime;
previous5HzTime = currentTime;
executionTime5Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_1Hz)
{
frame_1Hz = false;
currentTime = micros();
deltaTime1Hz = currentTime - previous1HzTime;
previous1HzTime = currentTime;
executionTime1Hz = micros() - currentTime;
}
////////////////////////////////
}
}
int main(void)
{
uint32_t currentTime;
// High Speed Telemetry Test Code Begin
char numberString[12];
// High Speed Telemetry Test Code End
systemInit();
systemReady = true;
while (1)
{
///////////////////////////////
if (frame_50Hz)
{
frame_50Hz = false;
currentTime = micros();
deltaTime50Hz = currentTime - previous50HzTime;
previous50HzTime = currentTime;
processFlightCommands();
executionTime50Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_10Hz)
{
frame_10Hz = false;
currentTime = micros();
deltaTime10Hz = currentTime - previous10HzTime;
previous10HzTime = currentTime;
sensors.mag10Hz[XAXIS] = -((float)magSum[XAXIS] / 5.0f * magScaleFactor[XAXIS] - sensorConfig.magBias[XAXIS]);
sensors.mag10Hz[YAXIS] = (float)magSum[YAXIS] / 5.0f * magScaleFactor[YAXIS] - sensorConfig.magBias[YAXIS];
sensors.mag10Hz[ZAXIS] = -((float)magSum[ZAXIS] / 5.0f * magScaleFactor[ZAXIS] - sensorConfig.magBias[ZAXIS]);
magSum[XAXIS] = 0;
magSum[YAXIS] = 0;
magSum[ZAXIS] = 0;
newMagData = true;
pressureAverage = pressureSum / 10;
pressureSum = 0;
calculateTemperature();
calculatePressureAltitude();
sensors.pressureAltitude10Hz = pressureAlt;
serialCom();
if ( EKF_Initialized == false ) EKF_Init( sensors.accel100Hz[XAXIS], sensors.accel100Hz[YAXIS], sensors.accel100Hz[ZAXIS],
sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS] );
executionTime10Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_500Hz)
{
frame_500Hz = false;
currentTime = micros();
deltaTime500Hz = currentTime - previous500HzTime;
previous500HzTime = currentTime;
dt500Hz = (float)deltaTime500Hz * 0.000001f; // For integrations in 500 Hz loop
computeGyroTCBias();
sensors.gyro500Hz[ROLL ] = ((float)gyroSummedSamples500Hz[ROLL] / 2.0f - gyroRTBias[ROLL ] - gyroTCBias[ROLL ]) * GYRO_SCALE_FACTOR;
sensors.gyro500Hz[PITCH] = -((float)gyroSummedSamples500Hz[PITCH] / 2.0f - gyroRTBias[PITCH] - gyroTCBias[PITCH]) * GYRO_SCALE_FACTOR;
sensors.gyro500Hz[YAW ] = -((float)gyroSummedSamples500Hz[YAW] / 2.0f - gyroRTBias[YAW ] - gyroTCBias[YAW ]) * GYRO_SCALE_FACTOR;
#if defined(USE_CHR6DM_AHRS)
if ( EKF_Initialized == true ) EKF_Predict( sensors.gyro500Hz[ROLL], sensors.gyro500Hz[PITCH], sensors.gyro500Hz[YAW],
dt500Hz );
sensors.attitude200Hz[ROLL ] = gEstimatedStates.phi;
sensors.attitude200Hz[PITCH] = gEstimatedStates.theta;
sensors.attitude200Hz[YAW ] = gEstimatedStates.psi;
#endif
executionTime500Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_200Hz)
{
frame_200Hz = false;
currentTime = micros();
deltaTime200Hz = currentTime - previous200HzTime;
previous200HzTime = currentTime;
dt200Hz = (float)deltaTime200Hz * 0.000001f; // For integrations in 200 Hz loop
#if defined(USE_MADGWICK_AHRS) | defined(USE_MARG_AHRS)
sensors.accel200Hz[XAXIS] = -((float)accelSummedSamples200Hz[XAXIS] / 5.0f - accelRTBias[XAXIS] - sensorConfig.accelBias[XAXIS]) * sensorConfig.accelScaleFactor[XAXIS];
sensors.accel200Hz[YAXIS] = -((float)accelSummedSamples200Hz[YAXIS] / 5.0f - accelRTBias[YAXIS] - sensorConfig.accelBias[YAXIS]) * sensorConfig.accelScaleFactor[YAXIS];
sensors.accel200Hz[ZAXIS] = -((float)accelSummedSamples200Hz[ZAXIS] / 5.0f - accelRTBias[ZAXIS] - sensorConfig.accelBias[ZAXIS]) * sensorConfig.accelScaleFactor[ZAXIS];
sensors.accel200Hz[XAXIS] = computeFourthOrder200Hz(sensors.accel200Hz[XAXIS], &fourthOrder200Hz[AX_FILTER]);
sensors.accel200Hz[YAXIS] = computeFourthOrder200Hz(sensors.accel200Hz[YAXIS], &fourthOrder200Hz[AY_FILTER]);
sensors.accel200Hz[ZAXIS] = computeFourthOrder200Hz(sensors.accel200Hz[ZAXIS], &fourthOrder200Hz[AZ_FILTER]);
computeGyroTCBias();
sensors.gyro200Hz[ROLL ] = ((float)gyroSummedSamples200Hz[ROLL] / 5.0f - gyroRTBias[ROLL ] - gyroTCBias[ROLL ]) * GYRO_SCALE_FACTOR;
sensors.gyro200Hz[PITCH] = -((float)gyroSummedSamples200Hz[PITCH] / 5.0f - gyroRTBias[PITCH] - gyroTCBias[PITCH]) * GYRO_SCALE_FACTOR;
sensors.gyro200Hz[YAW ] = -((float)gyroSummedSamples200Hz[YAW] / 5.0f - gyroRTBias[YAW ] - gyroTCBias[YAW ]) * GYRO_SCALE_FACTOR;
#endif
#if defined(USE_MADGWICK_AHRS)
MadgwickAHRSupdate( sensors.gyro200Hz[ROLL], sensors.gyro200Hz[PITCH], sensors.gyro200Hz[YAW],
sensors.accel200Hz[XAXIS], sensors.accel200Hz[YAXIS], sensors.accel200Hz[ZAXIS],
sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS],
sensorConfig.accelCutoff,
newMagData,
dt200Hz );
newMagData = false;
q0q0 = q0 * q0;
q1q1 = q1 * q1;
q2q2 = q2 * q2;
q3q3 = q3 * q3;
sensors.attitude200Hz[ROLL ] = atan2f( 2.0f * (q0 * q1 + q2 * q3), q0q0 - q1q1 - q2q2 + q3q3 );
sensors.attitude200Hz[PITCH] = -asinf( 2.0f * (q1 * q3 - q0 * q2) );
sensors.attitude200Hz[YAW ] = atan2f( 2.0f * (q1 * q2 + q0 * q3), q0q0 + q1q1 - q2q2 - q3q3 );
#endif
#if defined(USE_MARG_AHRS)
MargAHRSupdate( sensors.gyro200Hz[ROLL], sensors.gyro200Hz[PITCH], sensors.gyro200Hz[YAW],
sensors.accel200Hz[XAXIS], sensors.accel200Hz[YAXIS], sensors.accel200Hz[ZAXIS],
sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS],
sensorConfig.accelCutoff,
newMagData,
dt200Hz );
newMagData = false;
q0q0 = q0 * q0;
q1q1 = q1 * q1;
q2q2 = q2 * q2;
q3q3 = q3 * q3;
sensors.attitude200Hz[ROLL ] = atan2f( 2.0f * (q0 * q1 + q2 * q3), q0q0 - q1q1 - q2q2 + q3q3 );
sensors.attitude200Hz[PITCH] = -asinf( 2.0f * (q1 * q3 - q0 * q2) );
sensors.attitude200Hz[YAW ] = atan2f( 2.0f * (q1 * q2 + q0 * q3), q0q0 + q1q1 - q2q2 - q3q3 );
#endif
executionTime200Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_100Hz)
{
frame_100Hz = false;
currentTime = micros();
deltaTime100Hz = currentTime - previous100HzTime;
previous100HzTime = currentTime;
dt100Hz = (float)deltaTime100Hz * 0.000001f; // For integrations in 100 Hz loop
sensors.accel100Hz[XAXIS] = -((float)accelSummedSamples100Hz[XAXIS] / 10.0f - accelRTBias[XAXIS] - sensorConfig.accelBias[XAXIS]) * sensorConfig.accelScaleFactor[XAXIS];
sensors.accel100Hz[YAXIS] = -((float)accelSummedSamples100Hz[YAXIS] / 10.0f - accelRTBias[YAXIS] - sensorConfig.accelBias[YAXIS]) * sensorConfig.accelScaleFactor[YAXIS];
sensors.accel100Hz[ZAXIS] = -((float)accelSummedSamples100Hz[ZAXIS] / 10.0f - accelRTBias[ZAXIS] - sensorConfig.accelBias[ZAXIS]) * sensorConfig.accelScaleFactor[ZAXIS];
sensors.accel100Hz[XAXIS] = computeFourthOrder100Hz(sensors.accel100Hz[XAXIS], &fourthOrder100Hz[AX_FILTER]);
sensors.accel100Hz[YAXIS] = computeFourthOrder100Hz(sensors.accel100Hz[YAXIS], &fourthOrder100Hz[AY_FILTER]);
sensors.accel100Hz[ZAXIS] = computeFourthOrder100Hz(sensors.accel100Hz[ZAXIS], &fourthOrder100Hz[AZ_FILTER]);
computeGyroTCBias();
sensors.gyro100Hz[ROLL ] = ((float)gyroSummedSamples100Hz[ROLL] / 10.0f - gyroRTBias[ROLL ] - gyroTCBias[ROLL ]) * GYRO_SCALE_FACTOR;
sensors.gyro100Hz[PITCH] = -((float)gyroSummedSamples100Hz[PITCH] / 10.0f - gyroRTBias[PITCH] - gyroTCBias[PITCH]) * GYRO_SCALE_FACTOR;
sensors.gyro100Hz[YAW ] = -((float)gyroSummedSamples100Hz[YAW] / 10.0f - gyroRTBias[YAW ] - gyroTCBias[YAW ]) * GYRO_SCALE_FACTOR;
#if defined(USE_CHR6DM_AHRS)
if ( EKF_Initialized == true ) EKF_Update( sensors.accel100Hz[XAXIS], sensors.accel100Hz[YAXIS], sensors.accel100Hz[ZAXIS],
sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS],
sensorConfig.accelCutoff,
newMagData );
newMagData = false;
sensors.attitude200Hz[ROLL ] = gEstimatedStates.phi;
sensors.attitude200Hz[PITCH] = gEstimatedStates.theta;
sensors.attitude200Hz[YAW ] = gEstimatedStates.psi;
#endif
computeAxisCommands(dt100Hz);
mixTable();
writeServos();
writeMotors();
// High Speed Telemetry Test Code Begin
if ( highSpeedAccelTelemEnabled == true )
{
// 100 Hz Accels
ftoa(sensors.accel100Hz[XAXIS], numberString); uartPrint(numberString); uartPrint(",");
ftoa(sensors.accel100Hz[YAXIS], numberString); uartPrint(numberString); uartPrint(",");
ftoa(sensors.accel100Hz[ZAXIS], numberString); uartPrint(numberString); uartPrint("\n");
}
if ( highSpeedGyroTelemEnabled == true )
{
// 100 Hz Gyros
ftoa(sensors.gyro100Hz[ROLL ], numberString); uartPrint(numberString); uartPrint(",");
ftoa(sensors.gyro100Hz[PITCH], numberString); uartPrint(numberString); uartPrint(",");
ftoa(sensors.gyro100Hz[YAW ], numberString); uartPrint(numberString); uartPrint("\n");
}
if ( highSpeedRollRateTelemEnabled == true )
{
// Roll Rate, Roll Rate Command
ftoa(sensors.gyro100Hz[ROLL], numberString); uartPrint(numberString); uartPrint(",");
ftoa(rxCommand[ROLL], numberString); uartPrint(numberString); uartPrint("\n");
}
if ( highSpeedPitchRateTelemEnabled == true )
{
// Pitch Rate, Pitch Rate Command
ftoa(sensors.gyro100Hz[PITCH], numberString); uartPrint(numberString); uartPrint(",");
ftoa(rxCommand[PITCH], numberString); uartPrint(numberString); uartPrint("\n");
}
if ( highSpeedYawRateTelemEnabled == true )
{
// Yaw Rate, Yaw Rate Command
ftoa(sensors.gyro100Hz[YAW], numberString); uartPrint(numberString); uartPrint(",");
ftoa(rxCommand[YAW], numberString); uartPrint(numberString); uartPrint("\n");
}
if ( highSpeedAttitudeTelemEnabled == true )
{
// 200 Hz Attitudes
ftoa(sensors.attitude200Hz[ROLL ], numberString); uartPrint(numberString); uartPrint(",");
ftoa(sensors.attitude200Hz[PITCH], numberString); uartPrint(numberString); uartPrint(",");
ftoa(sensors.attitude200Hz[YAW ], numberString); uartPrint(numberString); uartPrint("\n");
}
// High Speed Telemetry Test Code End
executionTime100Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_5Hz)
{
frame_5Hz = false;
currentTime = micros();
deltaTime5Hz = currentTime - previous5HzTime;
previous5HzTime = currentTime;
executionTime5Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_1Hz)
{
frame_1Hz = false;
currentTime = micros();
deltaTime1Hz = currentTime - previous1HzTime;
previous1HzTime = currentTime;
executionTime1Hz = micros() - currentTime;
}
////////////////////////////////
}
}
void MultiWii_loop () {
static uint8_t rcDelayCommand; // this indicates the number of time (multiple of RC measurement at 50Hz) the sticks must be maintained to run or switch off motors
static uint8_t rcSticks; // this hold sticks position for command combos
uint8_t axis,i;
int16_t error= 0;
int16_t delta;
int16_t PTerm = 0,ITerm = 0,DTerm, PTermACC, ITermACC;
static int16_t lastGyro[2] = {0,0};
static int16_t delta1[2],delta2[2];
static int16_t errorGyroI[2] = {0,0};
static int32_t errorGyroI_YAW =0;
static uint32_t rcTime = 0;
static int16_t initialThrottleHold;
static uint32_t timestamp_fixated = 0;
int16_t rc;
int32_t prop = 0;
if (currentTime > rcTime ) { // 50Hz: PPM frequency of the RC, no change happen within 20ms
rcTime = currentTime + 20000;
computeRC();
#if !defined(TEST)
serialCom();
// ------------------ STICKS COMMAND HANDLER --------------------
// checking sticks positions
uint8_t stTmp = 0;
for(i=0;i<4;i++) {
stTmp >>= 2;
if(rcData[i] > MINCHECK) stTmp |= 0x80; // check for MIN
if(rcData[i] < MAXCHECK) stTmp |= 0x40; // check for MAX
//Serial.print("i: "); Serial.println((int)i); Serial.print("rcData[i]: ");Serial.println(rcData[i]);
}
if(stTmp == rcSticks) {
if(rcDelayCommand<250) rcDelayCommand++;
} else rcDelayCommand = 0;
rcSticks = stTmp;
// perform actions
if (rcData[THROTTLE] <= MINCHECK) { // THROTTLE at minimum
errorGyroI[ROLL] = 0; errorGyroI[PITCH] = 0; errorGyroI_YAW = 0; // reset I sum
}
// calibrate online
if(rcDelayCommand == 20) {
if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) { // throttle=low, yaw=low, pitch=min, roll=center =>GYRO calibration
calibratingG=512;
}
else if (rcSticks == THR_HI + YAW_LO + PIT_LO + ROL_CE) { // throttle=max, yaw=low, pitch=min, roll=center => ACC calibration
calibratingA=512;
}
rcDelayCommand = 0;
}
#endif
} /* end currentTime > rcTime */