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 taskProcessGPS(void) { // if GPS feature is enabled, gpsThread() will be called at some intervals to check for stuck // hardware, wrong baud rates, init GPS if needed, etc. Don't use SENSOR_GPS here as gpsThread() can and will // change this based on available hardware if (feature(FEATURE_GPS)) { gpsThread(); } if (sensors(SENSOR_GPS)) { updateGpsIndicator(currentTime); } }
void handleFrSkyTelemetry(void) { if (!canSendFrSkyTelemetry()) { return; } uint32_t now = millis(); if (!hasEnoughTimeLapsedSinceLastTelemetryTransmission(now)) { return; } lastCycleTime = now; cycleNum++; // Sent every 125ms sendAccel(); sendVario(); sendTelemetryTail(); if ((cycleNum % 4) == 0) { // Sent every 500ms sendBaro(); sendHeading(); sendTelemetryTail(); } if ((cycleNum % 8) == 0) { // Sent every 1s sendTemperature1(); if (feature(FEATURE_VBAT)) { sendVoltage(); sendVoltageAmp(); sendAmperage(); sendFuelLevel(); } #ifdef GPS if (sensors(SENSOR_GPS)) sendGPS(); #endif sendTelemetryTail(); } if (cycleNum == 40) { //Frame 3: Sent every 5s cycleNum = 0; sendTime(); sendTelemetryTail(); } }
static void taskUpdateRxMain(timeUs_t currentTimeUs) { processRx(currentTimeUs); isRXDataNew = true; #if !defined(BARO) && !defined(SONAR) // updateRcCommands sets rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState updateRcCommands(); #endif updateArmingStatus(); #ifdef BARO if (sensors(SENSOR_BARO)) { updateAltHoldState(); } #endif #ifdef SONAR if (sensors(SENSOR_SONAR)) { updateSonarAltHoldState(); } #endif }
void imuInit(void) // Initialize & precalculate some values here { if (cfg.gy_smrll || cfg.gy_smptc || cfg.gy_smyw) { SmoothingFactor[ROLL] = cfg.gy_smrll; SmoothingFactor[PITCH] = cfg.gy_smptc; SmoothingFactor[YAW] = cfg.gy_smyw; GyroSmoothing = true; } else GyroSmoothing = false; #ifdef MAG if (sensors(SENSOR_MAG)) Mag_init(); #endif }
void taskUpdateRxMain(void) { processRx(); isRXDataNew = true; #if !defined(BARO) && !defined(SONAR) // updateRcCommands sets rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState updateRcCommands(); #endif updateLEDs(); #ifdef BARO if (sensors(SENSOR_BARO)) { updateAltHoldState(); } #endif #ifdef SONAR if (sensors(SENSOR_SONAR)) { updateSonarAltHoldState(); } #endif }
void taskUpdateRxMain(void) { processRx(); isRXDataNew = true; #ifdef BARO // the 'annexCode' initialses rcCommand, updateAltHoldState depends on valid rcCommand data. if (haveProcessedAnnexCodeOnce) { if (sensors(SENSOR_BARO)) { updateAltHoldState(); } } #endif #ifdef SONAR // the 'annexCode' initialses rcCommand, updateAltHoldState depends on valid rcCommand data. if (haveProcessedAnnexCodeOnce) { if (sensors(SENSOR_SONAR)) { updateSonarAltHoldState(); } } #endif }
double DetectorModule::stripOccupancyPerEventBarrel() const { double rho = center().Rho()/10.; double theta = center().Theta(); double myOccupancyBarrel=(1.63e-4)+(2.56e-4)*rho-(1.92e-6)*rho*rho; double factor = fabs(sin(theta))*2; // 2 is a magic adjustment factor double dphideta = phiAperture() * etaAperture(); double minNSegments = minSegments(); int numStripsAcrossEstimate = sensors().begin()->numStripsAcrossEstimate(); double modWidth = (maxWidth() + minWidth())/2.; double occupancy = myOccupancyBarrel / factor / (90/1e3) * (dphideta / minNSegments) * (modWidth / numStripsAcrossEstimate); return occupancy; }
bool ProcessController::saveSettings(QDomDocument& doc, QDomElement& element) { if(!mProcessList) return false; element.setAttribute("hostName", sensors().at(0)->hostName()); element.setAttribute("sensorName", sensors().at(0)->name()); element.setAttribute("sensorType", sensors().at(0)->type()); element.setAttribute("version", QString::number(PROCESSHEADERVERSION)); element.setAttribute("treeViewHeader", QString::fromLatin1(mProcessList->treeView()->header()->saveState().toBase64())); element.setAttribute("showTotals", mProcessList->showTotals()?1:0); element.setAttribute("units", (int)(mProcessList->units())); element.setAttribute("ioUnits", (int)(mProcessList->processModel()->ioUnits())); element.setAttribute("ioInformation", (int)(mProcessList->processModel()->ioInformation())); element.setAttribute("showCommandLineOptions", mProcessList->processModel()->isShowCommandLineOptions()); element.setAttribute("showTooltips", mProcessList->processModel()->isShowingTooltips()); element.setAttribute("normalizeCPUUsage", mProcessList->processModel()->isNormalizedCPUUsage()); element.setAttribute("filterState", (int)(mProcessList->state())); SensorDisplay::saveSettings(doc, element); return true; }
bool LogFile::saveSettings(QDomDocument& doc, QDomElement& element) { element.setAttribute("hostName", sensors().at(0)->hostName()); element.setAttribute("sensorName", sensors().at(0)->name()); element.setAttribute("sensorType", sensors().at(0)->type()); element.setAttribute("font", monitor->font().toString()); saveColor(element, "textColor", monitor->palette().color( QPalette::Text ) ); saveColor(element, "backgroundColor", monitor->palette().color( QPalette::Base ) ); for (QStringList::Iterator it = filterRules.begin(); it != filterRules.end(); ++it) { QDomElement filter = doc.createElement("filter"); filter.setAttribute("rule", (*it)); element.appendChild(filter); } SensorDisplay::saveSettings(doc, element); return true; }
double DetectorModule::stripOccupancyPerEventEndcap() const { double rho = center().Rho()/10.; double theta = center().Theta(); double z = center().Z()/10.; double myOccupancyEndcap = (-6.20e-5)+(1.75e-4)*rho-(1.08e-6)*rho*rho+(1.50e-5)*(z); double factor=fabs(cos(theta))*2; // 2 is a magic adjustment factor double dphideta = phiAperture() * etaAperture(); double minNSegments = minSegments(); int numStripsAcrossEstimate = sensors().begin()->numStripsAcrossEstimate(); double modWidth = (maxWidth() + minWidth())/2.; double occupancy = myOccupancyEndcap / factor / (90/1e3) * (dphideta / minNSegments) * (modWidth / numStripsAcrossEstimate); return occupancy; }
/* This is equivalent to the act of lifting the robot and moving it * to a new location without the encoders registering the movement. */ void hyperJump(double new_x, double new_y, double new_o) { robot_x = new_x; robot_y = new_y; robot_o = new_o; /* this function needs a value */ /* greater than 0 to update the */ /* position, so this is how long */ /* a teleportation will take, 1 msec */ updatePosition(1); sensors(); refreshSimDisplay(TRUE); RedrawWholeMap(); }
static Node::Info tempInfo(std::string uid){ BB::Node::Sensors sensors({ BB::Node::Sensor("Temperature", BB::Node::Info::Temperature, "C") }); BB::Node::Settings settings({ BB::Node::Setting("name"), BB::Node::Setting("place"), BB::Node::Setting("low"), BB::Node::Setting("high"), BB::Node::Setting("extremelyLow"), BB::Node::Setting("extremelyHigh"), }); Node::Info info(uid, Node::Info::Temperature, sensors, settings); return info; }
/** * Read BARO and update alt/vel topic * Function is called at main loop rate, updates happen at reduced rate */ static void updateBaroTopic(uint32_t currentTime) { static navigationTimer_t baroUpdateTimer; if (updateTimer(&baroUpdateTimer, HZ2US(INAV_BARO_UPDATE_RATE), currentTime)) { float newBaroAlt = baroCalculateAltitude(); if (sensors(SENSOR_BARO) && isBaroCalibrationComplete()) { posEstimator.baro.alt = newBaroAlt; posEstimator.baro.epv = posControl.navConfig->inav.baro_epv; posEstimator.baro.lastUpdateTime = currentTime; } else { posEstimator.baro.alt = 0; posEstimator.baro.lastUpdateTime = 0; } } }
void computeIMU(void) { static float LastGyroSmooth[3] = { 0.0f, 0.0f, 0.0f }; static int16_t triywavg[4]; static uint8_t triywavgpIDX = 0; static uint32_t prevT; uint32_t curT; uint8_t axis, i; float flttmp; if (MpuSpecial) GETMPU6050(); else { gyro.temperature(&telemTemperature1); // Read out gyro temperature Gyro_getADC(); // Also feeds gyroData if (sensors(SENSOR_ACC)) ACC_getADC(); } curT = micros(); ACCDeltaTimeINS = (float)(curT - prevT) * 0.000001f; // ACCDeltaTimeINS is in seconds now ACCDeltaTimeINS = constrain(ACCDeltaTimeINS, 0.0001f, 0.5f); // Constrain to range 0,1ms - 500ms prevT = curT; if(cfg.acc_calibrated) getEstimatedAttitude(); // acc_calibrated just can turn true if acc present. if(cfg.mixerConfiguration == MULTITYPE_TRI && cfg.gy_smyw) // Moving average for yaw in tri mode { triywavg[triywavgpIDX] = (int16_t)gyroData[YAW]; triywavgpIDX++; if (triywavgpIDX == 4) triywavgpIDX = 0; flttmp = 0; for (i = 0; i < 4; i++) flttmp += triywavg[i]; gyroData[YAW] = flttmp * 0.25f; } if (GyroSmoothing) { for (axis = 0; axis < 3; axis++) { if (SmoothingFactor[axis] > 1) // Circumvent useless action { flttmp = (float)SmoothingFactor[axis]; gyroData[axis] = ((LastGyroSmooth[axis] * (flttmp - 1.0f)) + gyroData[axis]) / flttmp; LastGyroSmooth[axis] = gyroData[axis]; } } } }
void sendFRSKYTelemetry(void) { static uint32_t lastCycleTime = 0; static uint8_t cycleNum = 0; if (currentTimeMS - lastCycleTime >= CYCLETIME) { lastCycleTime = currentTimeMS; cycleNum++; // Sent every 125ms sendAccel(); sendTelemetryTail(); if ((cycleNum % 4) == 0) // Sent every 500ms { sendBaro(); sendHeading(); sendTelemetryTail(); } if ((cycleNum % 8) == 0) // Sent every 1s { sendTemperature1(); if (feature(FEATURE_VBAT)) { sendVoltage(); sendVoltageAmp(); } if (sensors(SENSOR_GPS)) sendGPS(); sendTelemetryTail(); } if (cycleNum == 40) //Frame 3: Sent every 5s { cycleNum = 0; sendTime(); sendTelemetryTail(); } } }
void GARule::randomRule() { int rInt; // random integer in case we need it more than once int j; // variable for scratch use j = teamBall(Random::randint(-1,1)); if (j == 1) myBall(Random::randbool()); // have at least some logic in the random rules; else myBall(false); // if my team doesn't have the ball, then I can't have it Thing sens[8]; for (int i = 0; i < 8; i++){ rInt = Random::randint(0,5); switch(rInt){ case(0): sens[i] = Nothing; break; case(1): sens[i] = MyBot; break; case(2): sens[i] = OtherBot; break; case(3): sens[i] = TheBall; break; case(4): sens[i] = Net; break; case(5): sens[i] = Wall; break; }//end switch }//end for sensors(sens); fire(Random::randbool()); move(Random::randbool()); rInt = Random::randint(0,2); if (rInt == 0) turn(BotRotation::Left); else if (rInt == 1) turn(BotRotation::Right); else turn(BotRotation::None); // generates rules with more than one action, but should work anyway since // only one action will occur }
void sendTelemetry(void) { if (mcfg.telemetry_softserial == TELEMETRY_UART && !f.ARMED) return; if (serialTotalBytesWaiting(core.telemport) != 0) return; if (millis() - lastCycleTime >= CYCLETIME) { lastCycleTime = millis(); cycleNum++; // Sent every 125ms sendAccel(); sendTelemetryTail(); if ((cycleNum % 4) == 0) { // Sent every 500ms sendBaro(); sendHeading(); sendTelemetryTail(); } if ((cycleNum % 8) == 0) { // Sent every 1s sendTemperature1(); if (feature(FEATURE_VBAT)) { sendVoltage(); sendVoltageAmp(); } if (sensors(SENSOR_GPS)) sendGPS(); sendTelemetryTail(); } if (cycleNum == 40) { //Frame 3: Sent every 5s cycleNum = 0; sendTime(); sendTelemetryTail(); } } }
void RobotMove::GoToPose(Pose newPose){ xDestination = newPose.getX(); yDestination = newPose.getY(); //Vector worldCoordinateVector(xDestination + (*position).GetXPos(), yDestination + (*position).GetYPos()); //Vector localCoordinateVector(worldCoordinateVector.getDistance(), dtor(worldCoordinateVector.getAngle()) + (*position).GetYaw(), false); //xDestination = localCoordinateVector.getXIntensity();//x + (*position).GetXPos(); //yDestination = localCoordinateVector.getYIntensity();//y + (*position).GetYPos(); SensorScan sensors(&(*laser), &(*position), &(*worldView), sensorFunction, 1000.0); double distance = closenessCutOff; while (distance >= closenessCutOff) { //cout << "Setup"; (*robot).Read(); sensors.ReadSensors(); Vector sensorVector = sensors.VectorSum(); sensorVector.invertVector(); //sensorVector.debugIntensity(); Vector avoidVector(sensorVector.getDistance(), dtor(sensorVector.getAngle()) + (*position).GetYaw() , false); Vector driveVector((*position).GetXPos(), (*position).GetYPos(), xDestination, yDestination); distance = driveVector.getDistance(); TargetFunction(driveVector); //TargetFunction(driveVector); //cout << endl << "AvoidVector : "; //avoidVector.debug();avoidVector.debugIntensity(); //cout << endl << "Drive Vector : "; //cout << closenessCutOff << endl; //driveVector.debug(); //driveVector.debugIntensity(); //cout << endl; driveVector.add(avoidVector); //cout << endl << "Resulting Vector : " << endl; //driveVector.debug();driveVector.debugIntensity(); DriveToVector(driveVector, &(*position)); } Vector directionVector(.1, newPose.getAngle(), true); //while (abs((*position).GetYaw() - dtor(newPose.getAngle())) > closenessCutOff) { // DriveToVector(directionVector, &(*position)); //} (*position).SetSpeed(0,0); }
static bool processBinaryModeRequest(uint8_t address) { switch (address) { #ifdef USE_GPS case 0x8A: if (sensors(SENSOR_GPS)) { hottPrepareGPSResponse(&hottGPSMessage); hottQueueSendResponse((uint8_t *)&hottGPSMessage, sizeof(hottGPSMessage)); return true; } break; #endif case 0x8E: hottPrepareEAMResponse(&hottEAMMessage); hottQueueSendResponse((uint8_t *)&hottEAMMessage, sizeof(hottEAMMessage)); return true; } return false; }
static void cliStatus(char *cmdline) { uint8_t i; uint32_t mask; printf("System Uptime: %d seconds, Voltage: %d * 0.1V (%dS battery)\r\n", millis() / 1000, vbat, batteryCellCount); mask = sensorsMask(); uartPrint("Detected sensors: "); for (i = 0; ; i++) { if (sensorNames[i] == NULL) break; if (mask & (1 << i)) printf("%s ", sensorNames[i]); } if (sensors(SENSOR_ACC)) printf("ACCHW: %s", accNames[accHardware]); uartPrint("\r\n"); printf("Cycle Time: %d, I2C Errors: %d\r\n", cycleTime, i2cGetErrorCounter()); }
static void processBinaryModeRequest(uint8_t address) { #ifdef HOTT_DEBUG static uint8_t hottBinaryRequests = 0; static uint8_t hottGPSRequests = 0; static uint8_t hottEAMRequests = 0; #endif switch (address) { #ifdef GPS case 0x8A: #ifdef HOTT_DEBUG hottGPSRequests++; #endif if (sensors(SENSOR_GPS)) { hottSendGPSResponse(); } break; #endif case 0x8E: #ifdef HOTT_DEBUG hottEAMRequests++; #endif hottSendEAMResponse(); break; } #ifdef HOTT_DEBUG hottBinaryRequests++; debug[0] = hottBinaryRequests; #ifdef GPS debug[1] = hottGPSRequests; #endif debug[2] = hottEAMRequests; #endif }
void simUpdate (void) { unsigned long msec, t; if( rotation_velocity != 0 || translation_velocity != 0 ) { stepEndTime = getTime(); msec = stepEndTime - stepStartTime; t = 1000*msec/timeScale; if (t >= timeIncrement) { if (t > 3 * timeIncrement) { fprintf(stderr,"Missed Simulations steps for %ld msec\n",msec); t = 2 * timeIncrement; } updatePosition(t); sensors(); stepStartTime = stepEndTime; } } else { UpdateElapsedTime(); RedrawTime(); } }
/* Calculate measured acceleration in body frame cm/s/s */ static void imuUpdateMeasuredAcceleration(void) { int axis; /* Convert acceleration to cm/s/s */ for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) { imuAccelInBodyFrame.A[axis] = accADC[axis] * (GRAVITY_CMSS / acc.acc_1G); imuMeasuredGravityBF.A[axis] = imuAccelInBodyFrame.A[axis]; } #ifdef GPS /** Centrifugal force compensation on a fixed-wing aircraft * a_c_body = omega x vel_tangential_body * assumption: tangential velocity only along body x-axis * assumption: GPS velocity equal to body x-axis velocity */ if (STATE(FIXED_WING) && sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 5) { imuMeasuredGravityBF.A[Y] -= gpsSol.groundSpeed * imuMeasuredRotationBF.A[Z]; imuMeasuredGravityBF.A[Z] += gpsSol.groundSpeed * imuMeasuredRotationBF.A[Y]; } #endif }
static FAST_CODE_NOINLINE void subTaskPidSubprocesses(timeUs_t currentTimeUs) { uint32_t startTime = 0; if (debugMode == DEBUG_PIDLOOP) { startTime = micros(); } #ifdef USE_MAG if (sensors(SENSOR_MAG)) { updateMagHold(); } #endif #ifdef USE_BLACKBOX if (!cliMode && blackboxConfig()->device) { blackboxUpdate(currentTimeUs); } #else UNUSED(currentTimeUs); #endif DEBUG_SET(DEBUG_PIDLOOP, 3, micros() - startTime); }
static void cliStatus(char *cmdline) { (void)cmdline; uint8_t i; uint32_t mask; printf("System Uptime: %d seconds, Voltage: %d * 0.1V (%dS battery)\r\n", millis() / 1000, vbat, batteryCellCount); mask = sensorsMask(); printf("Hardware: %s @ %dMHz, detected sensors: ", hwNames[hw_revision], (SystemCoreClock / 1000000)); for (i = 0; ; i++) { if (sensorNames[i] == NULL) break; if (mask & (1 << i)) printf("%s ", sensorNames[i]); } if (sensors(SENSOR_ACC)) printf("ACCHW: %s", accNames[accHardware]); cliPrint("\r\n"); printf("Cycle Time: %d, I2C Errors: %d, config size: %d\r\n", cycleTime, i2cGetErrorCounter(), sizeof(master_t)); }
void computeIMU(void) { static int16_t gyroYawSmooth = 0; Gyro_getADC(); if (sensors(SENSOR_ACC)) { ACC_getADC(); getEstimatedAttitude(); } else { accADC[X] = 0; accADC[Y] = 0; accADC[Z] = 0; } if (mcfg.mixerConfiguration == MULTITYPE_TRI) { gyroData[YAW] = (gyroYawSmooth * 2 + gyroADC[YAW]) / 3; gyroYawSmooth = gyroData[YAW]; } else { gyroData[YAW] = gyroADC[YAW]; } gyroData[ROLL] = gyroADC[ROLL]; gyroData[PITCH] = gyroADC[PITCH]; }
void computeIMU(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerConfiguration) { static int16_t gyroYawSmooth = 0; gyroGetADC(); if (sensors(SENSOR_ACC)) { updateAccelerationReadings(accelerometerTrims); getEstimatedAttitude(); } else { accADC[X] = 0; accADC[Y] = 0; accADC[Z] = 0; } gyroData[FD_ROLL] = gyroADC[FD_ROLL]; gyroData[FD_PITCH] = gyroADC[FD_PITCH]; if (mixerConfiguration == MULTITYPE_TRI) { gyroData[FD_YAW] = (gyroYawSmooth * 2 + gyroADC[FD_YAW]) / 3; gyroYawSmooth = gyroData[FD_YAW]; } else { gyroData[FD_YAW] = gyroADC[FD_YAW]; } }
void handleSmartPortTelemetry(void) { uint32_t smartPortLastServiceTime = millis(); if (!smartPortTelemetryEnabled) { return; } if (!canSendSmartPortTelemetry()) { return; } while (serialRxBytesWaiting(smartPortSerialPort) > 0) { uint8_t c = serialRead(smartPortSerialPort); smartPortDataReceive(c); } uint32_t now = millis(); // if timed out, reconfigure the UART back to normal so the GUI or CLI works if ((now - smartPortLastRequestTime) > SMARTPORT_NOT_CONNECTED_TIMEOUT_MS) { smartPortState = SPSTATE_TIMEDOUT; return; } while (smartPortHasRequest) { // Ensure we won't get stuck in the loop if there happens to be nothing available to send in a timely manner - dump the slot if we loop in there for too long. if ((millis() - smartPortLastServiceTime) > SMARTPORT_SERVICE_TIMEOUT_MS) { smartPortHasRequest = 0; return; } // we can send back any data we want, our table keeps track of the order and frequency of each data type we send uint16_t id = frSkyDataIdTable[smartPortIdCnt]; if (id == 0) { // end of table reached, loop back smartPortIdCnt = 0; id = frSkyDataIdTable[smartPortIdCnt]; } smartPortIdCnt++; int32_t tmpi; static uint8_t t1Cnt = 0; switch(id) { #ifdef GPS case FSSP_DATAID_SPEED : if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) { uint32_t tmpui = (GPS_speed * 36 + 36 / 2) / 100; smartPortSendPackage(id, tmpui); // given in 0.1 m/s, provide in KM/H smartPortHasRequest = 0; } break; #endif case FSSP_DATAID_VFAS : if (feature(FEATURE_VBAT)) { uint16_t vfasVoltage; if (telemetryConfig->frsky_vfas_cell_voltage) { vfasVoltage = vbat / batteryCellCount; } else { vfasVoltage = vbat; } smartPortSendPackage(id, vfasVoltage * 10); // given in 0.1V, convert to volts smartPortHasRequest = 0; } break; case FSSP_DATAID_CURRENT : if (feature(FEATURE_CURRENT_METER)) { smartPortSendPackage(id, amperage / 10); // given in 10mA steps, unknown requested unit smartPortHasRequest = 0; } break; //case FSSP_DATAID_RPM : case FSSP_DATAID_ALTITUDE : if (sensors(SENSOR_BARO)) { smartPortSendPackage(id, BaroAlt); // unknown given unit, requested 100 = 1 meter smartPortHasRequest = 0; } break; case FSSP_DATAID_FUEL : if (feature(FEATURE_CURRENT_METER)) { smartPortSendPackage(id, mAhDrawn); // given in mAh, unknown requested unit smartPortHasRequest = 0; } break; //case FSSP_DATAID_ADC1 : //case FSSP_DATAID_ADC2 : #ifdef GPS case FSSP_DATAID_LATLONG : if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) { uint32_t tmpui = 0; // the same ID is sent twice, one for longitude, one for latitude // the MSB of the sent uint32_t helps FrSky keep track // the even/odd bit of our counter helps us keep track if (smartPortIdCnt & 1) { tmpui = abs(GPS_coord[LON]); // now we have unsigned value and one bit to spare tmpui = (tmpui + tmpui / 2) / 25 | 0x80000000; // 6/100 = 1.5/25, division by power of 2 is fast if (GPS_coord[LON] < 0) tmpui |= 0x40000000; } else { tmpui = abs(GPS_coord[LAT]); // now we have unsigned value and one bit to spare tmpui = (tmpui + tmpui / 2) / 25; // 6/100 = 1.5/25, division by power of 2 is fast if (GPS_coord[LAT] < 0) tmpui |= 0x40000000; } smartPortSendPackage(id, tmpui); smartPortHasRequest = 0; } break; #endif //case FSSP_DATAID_CAP_USED : case FSSP_DATAID_VARIO : if (sensors(SENSOR_BARO)) { smartPortSendPackage(id, vario); // unknown given unit but requested in 100 = 1m/s smartPortHasRequest = 0; } break; case FSSP_DATAID_HEADING : smartPortSendPackage(id, attitude.values.yaw * 10); // given in 10*deg, requested in 10000 = 100 deg smartPortHasRequest = 0; break; case FSSP_DATAID_ACCX : smartPortSendPackage(id, accSmooth[X] / 44); // unknown input and unknown output unit // we can only show 00.00 format, another digit won't display right on Taranis // dividing by roughly 44 will give acceleration in G units smartPortHasRequest = 0; break; case FSSP_DATAID_ACCY : smartPortSendPackage(id, accSmooth[Y] / 44); smartPortHasRequest = 0; break; case FSSP_DATAID_ACCZ : smartPortSendPackage(id, accSmooth[Z] / 44); smartPortHasRequest = 0; break; case FSSP_DATAID_T1 : // we send all the flags as decimal digits for easy reading // the t1Cnt simply allows the telemetry view to show at least some changes t1Cnt++; if (t1Cnt >= 4) { t1Cnt = 1; } tmpi = t1Cnt * 10000; // start off with at least one digit so the most significant 0 won't be cut off // the Taranis seems to be able to fit 5 digits on the screen // the Taranis seems to consider this number a signed 16 bit integer if (ARMING_FLAG(OK_TO_ARM)) tmpi += 1; if (ARMING_FLAG(PREVENT_ARMING)) tmpi += 2; if (ARMING_FLAG(ARMED)) tmpi += 4; if (FLIGHT_MODE(ANGLE_MODE)) tmpi += 10; if (FLIGHT_MODE(HORIZON_MODE)) tmpi += 20; if (FLIGHT_MODE(UNUSED_MODE)) tmpi += 40; if (FLIGHT_MODE(PASSTHRU_MODE)) tmpi += 40; if (FLIGHT_MODE(MAG_MODE)) tmpi += 100; if (FLIGHT_MODE(BARO_MODE)) tmpi += 200; if (FLIGHT_MODE(SONAR_MODE)) tmpi += 400; if (FLIGHT_MODE(GPS_HOLD_MODE)) tmpi += 1000; if (FLIGHT_MODE(GPS_HOME_MODE)) tmpi += 2000; if (FLIGHT_MODE(HEADFREE_MODE)) tmpi += 4000; smartPortSendPackage(id, (uint32_t)tmpi); smartPortHasRequest = 0; break; case FSSP_DATAID_T2 : if (sensors(SENSOR_GPS)) { #ifdef GPS // provide GPS lock status smartPortSendPackage(id, (STATE(GPS_FIX) ? 1000 : 0) + (STATE(GPS_FIX_HOME) ? 2000 : 0) + GPS_numSat); smartPortHasRequest = 0; #endif } else if (feature(FEATURE_GPS)) { smartPortSendPackage(id, 0); smartPortHasRequest = 0; } break; #ifdef GPS case FSSP_DATAID_GPS_ALT : if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) { smartPortSendPackage(id, GPS_altitude * 100); // given in 0.1m , requested in 10 = 1m (should be in mm, probably a bug in opentx, tested on 2.0.1.7) smartPortHasRequest = 0; } break; #endif case FSSP_DATAID_A4 : if (feature(FEATURE_VBAT)) { smartPortSendPackage(id, vbat * 10 / batteryCellCount ); // given in 0.1V, convert to volts smartPortHasRequest = 0; } break; default: break; // if nothing is sent, smartPortHasRequest isn't cleared, we already incremented the counter, just loop back to the start } } }
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 } }