mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-23 16:25:26 +03:00
Merge remote-tracking branch 'origin/master' into sh_vtol_nav
This commit is contained in:
Pawel Spychalski (DzikuVx)
commit
859911599f
265 changed files with 6533 additions and 1672 deletions
|
|
@ -462,9 +462,9 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
|
|||
sbufWriteU16(dst, packSensorStatus());
|
||||
sbufWriteU16(dst, averageSystemLoadPercent);
|
||||
sbufWriteU8(dst, (getConfigBatteryProfile() << 4) | getConfigProfile());
|
||||
sbufWriteU8(dst, getConfigMixerProfile());
|
||||
sbufWriteU32(dst, armingFlags);
|
||||
sbufWriteData(dst, &mspBoxModeFlags, sizeof(mspBoxModeFlags));
|
||||
sbufWriteU8(dst, getConfigMixerProfile());
|
||||
}
|
||||
break;
|
||||
|
||||
|
|
@ -478,7 +478,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
|
|||
}
|
||||
for (int i = 0; i < 3; i++) {
|
||||
#ifdef USE_MAG
|
||||
sbufWriteU16(dst, mag.magADC[i]);
|
||||
sbufWriteU16(dst, lrintf(mag.magADC[i]));
|
||||
#else
|
||||
sbufWriteU16(dst, 0);
|
||||
#endif
|
||||
|
|
@ -1083,7 +1083,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
|
|||
legacyLedConfig |= ledConfig->led_function << shiftCount;
|
||||
shiftCount += 4;
|
||||
legacyLedConfig |= (ledConfig->led_overlay & 0x3F) << (shiftCount);
|
||||
shiftCount += 6;
|
||||
shiftCount += 6;
|
||||
legacyLedConfig |= (ledConfig->led_color) << (shiftCount);
|
||||
shiftCount += 4;
|
||||
legacyLedConfig |= (ledConfig->led_direction) << (shiftCount);
|
||||
|
|
@ -2585,6 +2585,29 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
|
|||
if (sbufBytesRemaining(src) > 0) {
|
||||
vtxSettingsConfigMutable()->lowPowerDisarm = sbufReadU8(src);
|
||||
}
|
||||
|
||||
// //////////////////////////////////////////////////////////
|
||||
// this code is taken from BF, it's hack for HDZERO VTX MSP frame
|
||||
// API version 1.42 - this parameter kept separate since clients may already be supplying
|
||||
if (sbufBytesRemaining(src) >= 2) {
|
||||
sbufReadU16(src); //skip pitModeFreq
|
||||
}
|
||||
|
||||
// API version 1.42 - extensions for non-encoded versions of the band, channel or frequency
|
||||
if (sbufBytesRemaining(src) >= 4) {
|
||||
uint8_t newBand = sbufReadU8(src);
|
||||
const uint8_t newChannel = sbufReadU8(src);
|
||||
vtxSettingsConfigMutable()->band = newBand;
|
||||
vtxSettingsConfigMutable()->channel = newChannel;
|
||||
}
|
||||
|
||||
/* if (sbufBytesRemaining(src) >= 4) {
|
||||
sbufRead8(src); // freq_l
|
||||
sbufRead8(src); // freq_h
|
||||
sbufRead8(src); // band count
|
||||
sbufRead8(src); // channel count
|
||||
}*/
|
||||
// //////////////////////////////////////////////////////////
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -2831,7 +2854,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
|
|||
ledConfig->led_position = legacyConfig & 0xFF;
|
||||
ledConfig->led_function = (legacyConfig >> 8) & 0xF;
|
||||
ledConfig->led_overlay = (legacyConfig >> 12) & 0x3F;
|
||||
ledConfig->led_color = (legacyConfig >> 18) & 0xF;
|
||||
ledConfig->led_color = (legacyConfig >> 18) & 0xF;
|
||||
ledConfig->led_direction = (legacyConfig >> 22) & 0x3F;
|
||||
ledConfig->led_params = (legacyConfig >> 28) & 0xF;
|
||||
|
||||
|
|
@ -3400,7 +3423,7 @@ static bool mspParameterGroupsCommand(sbuf_t *dst, sbuf_t *src)
|
|||
bool isOSDTypeSupportedBySimulator(void)
|
||||
{
|
||||
#ifdef USE_OSD
|
||||
displayPort_t *osdDisplayPort = osdGetDisplayPort();
|
||||
displayPort_t *osdDisplayPort = osdGetDisplayPort();
|
||||
return (!!osdDisplayPort && !!osdDisplayPort->vTable->readChar);
|
||||
#else
|
||||
return false;
|
||||
|
|
@ -3623,132 +3646,130 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
|
|||
|
||||
#ifdef USE_SIMULATOR
|
||||
case MSP_SIMULATOR:
|
||||
tmp_u8 = sbufReadU8(src); // Get the Simulator MSP version
|
||||
|
||||
tmp_u8 = sbufReadU8(src); // Get the Simulator MSP version
|
||||
|
||||
// Check the MSP version of simulator
|
||||
if (tmp_u8 != SIMULATOR_MSP_VERSION) {
|
||||
if (tmp_u8 != SIMULATOR_MSP_VERSION) {
|
||||
break;
|
||||
}
|
||||
|
||||
simulatorData.flags = sbufReadU8(src);
|
||||
simulatorData.flags = sbufReadU8(src);
|
||||
|
||||
if (!SIMULATOR_HAS_OPTION(HITL_ENABLE)) {
|
||||
|
||||
if (ARMING_FLAG(SIMULATOR_MODE_HITL)) { // Just once
|
||||
DISABLE_ARMING_FLAG(SIMULATOR_MODE_HITL);
|
||||
if (ARMING_FLAG(SIMULATOR_MODE_HITL)) { // Just once
|
||||
DISABLE_ARMING_FLAG(SIMULATOR_MODE_HITL);
|
||||
|
||||
#ifdef USE_BARO
|
||||
if ( requestedSensors[SENSOR_INDEX_BARO] != BARO_NONE ) {
|
||||
baroStartCalibration();
|
||||
baroStartCalibration();
|
||||
}
|
||||
#endif
|
||||
#ifdef USE_MAG
|
||||
DISABLE_STATE(COMPASS_CALIBRATED);
|
||||
compassInit();
|
||||
DISABLE_STATE(COMPASS_CALIBRATED);
|
||||
compassInit();
|
||||
#endif
|
||||
simulatorData.flags = HITL_RESET_FLAGS;
|
||||
simulatorData.flags = HITL_RESET_FLAGS;
|
||||
// Review: Many states were affected. Reboot?
|
||||
|
||||
disarm(DISARM_SWITCH); // Disarm to prevent motor output!!!
|
||||
disarm(DISARM_SWITCH); // Disarm to prevent motor output!!!
|
||||
}
|
||||
} else {
|
||||
if (!ARMING_FLAG(SIMULATOR_MODE_HITL)) { // Just once
|
||||
if (!ARMING_FLAG(SIMULATOR_MODE_HITL)) { // Just once
|
||||
#ifdef USE_BARO
|
||||
if ( requestedSensors[SENSOR_INDEX_BARO] != BARO_NONE ) {
|
||||
sensorsSet(SENSOR_BARO);
|
||||
setTaskEnabled(TASK_BARO, true);
|
||||
DISABLE_ARMING_FLAG(ARMING_DISABLED_HARDWARE_FAILURE);
|
||||
baroStartCalibration();
|
||||
baroStartCalibration();
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef USE_MAG
|
||||
if (compassConfig()->mag_hardware != MAG_NONE) {
|
||||
sensorsSet(SENSOR_MAG);
|
||||
ENABLE_STATE(COMPASS_CALIBRATED);
|
||||
DISABLE_ARMING_FLAG(ARMING_DISABLED_HARDWARE_FAILURE);
|
||||
mag.magADC[X] = 800;
|
||||
mag.magADC[Y] = 0;
|
||||
mag.magADC[Z] = 0;
|
||||
}
|
||||
if (compassConfig()->mag_hardware != MAG_NONE) {
|
||||
sensorsSet(SENSOR_MAG);
|
||||
ENABLE_STATE(COMPASS_CALIBRATED);
|
||||
DISABLE_ARMING_FLAG(ARMING_DISABLED_HARDWARE_FAILURE);
|
||||
mag.magADC[X] = 800;
|
||||
mag.magADC[Y] = 0;
|
||||
mag.magADC[Z] = 0;
|
||||
}
|
||||
#endif
|
||||
ENABLE_ARMING_FLAG(SIMULATOR_MODE_HITL);
|
||||
ENABLE_ARMING_FLAG(SIMULATOR_MODE_HITL);
|
||||
ENABLE_STATE(ACCELEROMETER_CALIBRATED);
|
||||
LOG_DEBUG(SYSTEM, "Simulator enabled");
|
||||
}
|
||||
LOG_DEBUG(SYSTEM, "Simulator enabled");
|
||||
}
|
||||
|
||||
if (dataSize >= 14) {
|
||||
if (dataSize >= 14) {
|
||||
|
||||
if (feature(FEATURE_GPS) && SIMULATOR_HAS_OPTION(HITL_HAS_NEW_GPS_DATA)) {
|
||||
gpsSol.fixType = sbufReadU8(src);
|
||||
gpsSol.hdop = gpsSol.fixType == GPS_NO_FIX ? 9999 : 100;
|
||||
gpsSol.flags.hasNewData = true;
|
||||
gpsSol.numSat = sbufReadU8(src);
|
||||
if (feature(FEATURE_GPS) && SIMULATOR_HAS_OPTION(HITL_HAS_NEW_GPS_DATA)) {
|
||||
gpsSolDRV.fixType = sbufReadU8(src);
|
||||
gpsSolDRV.hdop = gpsSolDRV.fixType == GPS_NO_FIX ? 9999 : 100;
|
||||
gpsSolDRV.numSat = sbufReadU8(src);
|
||||
|
||||
if (gpsSol.fixType != GPS_NO_FIX) {
|
||||
gpsSol.flags.validVelNE = true;
|
||||
gpsSol.flags.validVelD = true;
|
||||
gpsSol.flags.validEPE = true;
|
||||
gpsSol.flags.validTime = false;
|
||||
if (gpsSolDRV.fixType != GPS_NO_FIX) {
|
||||
gpsSolDRV.flags.validVelNE = true;
|
||||
gpsSolDRV.flags.validVelD = true;
|
||||
gpsSolDRV.flags.validEPE = true;
|
||||
gpsSolDRV.flags.validTime = false;
|
||||
|
||||
gpsSol.llh.lat = sbufReadU32(src);
|
||||
gpsSol.llh.lon = sbufReadU32(src);
|
||||
gpsSol.llh.alt = sbufReadU32(src);
|
||||
gpsSol.groundSpeed = (int16_t)sbufReadU16(src);
|
||||
gpsSol.groundCourse = (int16_t)sbufReadU16(src);
|
||||
gpsSolDRV.llh.lat = sbufReadU32(src);
|
||||
gpsSolDRV.llh.lon = sbufReadU32(src);
|
||||
gpsSolDRV.llh.alt = sbufReadU32(src);
|
||||
gpsSolDRV.groundSpeed = (int16_t)sbufReadU16(src);
|
||||
gpsSolDRV.groundCourse = (int16_t)sbufReadU16(src);
|
||||
|
||||
gpsSol.velNED[X] = (int16_t)sbufReadU16(src);
|
||||
gpsSol.velNED[Y] = (int16_t)sbufReadU16(src);
|
||||
gpsSol.velNED[Z] = (int16_t)sbufReadU16(src);
|
||||
gpsSolDRV.velNED[X] = (int16_t)sbufReadU16(src);
|
||||
gpsSolDRV.velNED[Y] = (int16_t)sbufReadU16(src);
|
||||
gpsSolDRV.velNED[Z] = (int16_t)sbufReadU16(src);
|
||||
|
||||
gpsSol.eph = 100;
|
||||
gpsSol.epv = 100;
|
||||
|
||||
ENABLE_STATE(GPS_FIX);
|
||||
} else {
|
||||
sbufAdvance(src, sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint16_t) + sizeof(uint16_t) + sizeof(uint16_t) * 3);
|
||||
}
|
||||
gpsSolDRV.eph = 100;
|
||||
gpsSolDRV.epv = 100;
|
||||
} else {
|
||||
sbufAdvance(src, sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint16_t) + sizeof(uint16_t) + sizeof(uint16_t) * 3);
|
||||
}
|
||||
// Feed data to navigation
|
||||
gpsProcessNewSolutionData();
|
||||
} else {
|
||||
sbufAdvance(src, sizeof(uint8_t) + sizeof(uint8_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint16_t) + sizeof(uint16_t) + sizeof(uint16_t) * 3);
|
||||
}
|
||||
|
||||
if (!SIMULATOR_HAS_OPTION(HITL_USE_IMU)) {
|
||||
attitude.values.roll = (int16_t)sbufReadU16(src);
|
||||
attitude.values.pitch = (int16_t)sbufReadU16(src);
|
||||
attitude.values.yaw = (int16_t)sbufReadU16(src);
|
||||
} else {
|
||||
sbufAdvance(src, sizeof(uint16_t) * XYZ_AXIS_COUNT);
|
||||
}
|
||||
gpsProcessNewDriverData();
|
||||
gpsProcessNewSolutionData(false);
|
||||
} else {
|
||||
sbufAdvance(src, sizeof(uint8_t) + sizeof(uint8_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint16_t) + sizeof(uint16_t) + sizeof(uint16_t) * 3);
|
||||
}
|
||||
|
||||
if (!SIMULATOR_HAS_OPTION(HITL_USE_IMU)) {
|
||||
attitude.values.roll = (int16_t)sbufReadU16(src);
|
||||
attitude.values.pitch = (int16_t)sbufReadU16(src);
|
||||
attitude.values.yaw = (int16_t)sbufReadU16(src);
|
||||
} else {
|
||||
sbufAdvance(src, sizeof(uint16_t) * XYZ_AXIS_COUNT);
|
||||
}
|
||||
|
||||
// Get the acceleration in 1G units
|
||||
acc.accADCf[X] = ((int16_t)sbufReadU16(src)) / 1000.0f;
|
||||
acc.accADCf[Y] = ((int16_t)sbufReadU16(src)) / 1000.0f;
|
||||
acc.accADCf[Z] = ((int16_t)sbufReadU16(src)) / 1000.0f;
|
||||
acc.accVibeSq[X] = 0.0f;
|
||||
acc.accVibeSq[Y] = 0.0f;
|
||||
acc.accVibeSq[Z] = 0.0f;
|
||||
|
||||
acc.accADCf[X] = ((int16_t)sbufReadU16(src)) / 1000.0f;
|
||||
acc.accADCf[Y] = ((int16_t)sbufReadU16(src)) / 1000.0f;
|
||||
acc.accADCf[Z] = ((int16_t)sbufReadU16(src)) / 1000.0f;
|
||||
acc.accVibeSq[X] = 0.0f;
|
||||
acc.accVibeSq[Y] = 0.0f;
|
||||
acc.accVibeSq[Z] = 0.0f;
|
||||
|
||||
// Get the angular velocity in DPS
|
||||
gyro.gyroADCf[X] = ((int16_t)sbufReadU16(src)) / 16.0f;
|
||||
gyro.gyroADCf[Y] = ((int16_t)sbufReadU16(src)) / 16.0f;
|
||||
gyro.gyroADCf[Z] = ((int16_t)sbufReadU16(src)) / 16.0f;
|
||||
gyro.gyroADCf[X] = ((int16_t)sbufReadU16(src)) / 16.0f;
|
||||
gyro.gyroADCf[Y] = ((int16_t)sbufReadU16(src)) / 16.0f;
|
||||
gyro.gyroADCf[Z] = ((int16_t)sbufReadU16(src)) / 16.0f;
|
||||
|
||||
if (sensors(SENSOR_BARO)) {
|
||||
baro.baroPressure = (int32_t)sbufReadU32(src);
|
||||
baro.baroTemperature = DEGREES_TO_CENTIDEGREES(SIMULATOR_BARO_TEMP);
|
||||
} else {
|
||||
sbufAdvance(src, sizeof(uint32_t));
|
||||
}
|
||||
if (sensors(SENSOR_BARO)) {
|
||||
baro.baroPressure = (int32_t)sbufReadU32(src);
|
||||
baro.baroTemperature = DEGREES_TO_CENTIDEGREES(SIMULATOR_BARO_TEMP);
|
||||
} else {
|
||||
sbufAdvance(src, sizeof(uint32_t));
|
||||
}
|
||||
|
||||
if (sensors(SENSOR_MAG)) {
|
||||
mag.magADC[X] = ((int16_t)sbufReadU16(src)) / 20; // 16000 / 20 = 800uT
|
||||
mag.magADC[Y] = ((int16_t)sbufReadU16(src)) / 20; //note that mag failure is simulated by setting all readings to zero
|
||||
mag.magADC[Z] = ((int16_t)sbufReadU16(src)) / 20;
|
||||
} else {
|
||||
sbufAdvance(src, sizeof(uint16_t) * XYZ_AXIS_COUNT);
|
||||
}
|
||||
if (sensors(SENSOR_MAG)) {
|
||||
mag.magADC[X] = ((int16_t)sbufReadU16(src)) / 20; // 16000 / 20 = 800uT
|
||||
mag.magADC[Y] = ((int16_t)sbufReadU16(src)) / 20; //note that mag failure is simulated by setting all readings to zero
|
||||
mag.magADC[Z] = ((int16_t)sbufReadU16(src)) / 20;
|
||||
} else {
|
||||
sbufAdvance(src, sizeof(uint16_t) * XYZ_AXIS_COUNT);
|
||||
}
|
||||
|
||||
if (SIMULATOR_HAS_OPTION(HITL_EXT_BATTERY_VOLTAGE)) {
|
||||
simulatorData.vbat = sbufReadU8(src);
|
||||
|
|
@ -3758,44 +3779,44 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
|
|||
|
||||
if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) {
|
||||
simulatorData.airSpeed = sbufReadU16(src);
|
||||
} else {
|
||||
} else {
|
||||
if (SIMULATOR_HAS_OPTION(HITL_EXTENDED_FLAGS)) {
|
||||
sbufReadU16(src);
|
||||
sbufReadU16(src);
|
||||
}
|
||||
}
|
||||
|
||||
if (SIMULATOR_HAS_OPTION(HITL_EXTENDED_FLAGS)) {
|
||||
simulatorData.flags |= ((uint16_t)sbufReadU8(src)) << 8;
|
||||
}
|
||||
} else {
|
||||
DISABLE_STATE(GPS_FIX);
|
||||
}
|
||||
}
|
||||
} else {
|
||||
DISABLE_STATE(GPS_FIX);
|
||||
}
|
||||
}
|
||||
|
||||
sbufWriteU16(dst, (uint16_t)simulatorData.input[INPUT_STABILIZED_ROLL]);
|
||||
sbufWriteU16(dst, (uint16_t)simulatorData.input[INPUT_STABILIZED_PITCH]);
|
||||
sbufWriteU16(dst, (uint16_t)simulatorData.input[INPUT_STABILIZED_YAW]);
|
||||
sbufWriteU16(dst, (uint16_t)(ARMING_FLAG(ARMED) ? simulatorData.input[INPUT_STABILIZED_THROTTLE] : -500));
|
||||
sbufWriteU16(dst, (uint16_t)simulatorData.input[INPUT_STABILIZED_ROLL]);
|
||||
sbufWriteU16(dst, (uint16_t)simulatorData.input[INPUT_STABILIZED_PITCH]);
|
||||
sbufWriteU16(dst, (uint16_t)simulatorData.input[INPUT_STABILIZED_YAW]);
|
||||
sbufWriteU16(dst, (uint16_t)(ARMING_FLAG(ARMED) ? simulatorData.input[INPUT_STABILIZED_THROTTLE] : -500));
|
||||
|
||||
simulatorData.debugIndex++;
|
||||
if (simulatorData.debugIndex == 8) {
|
||||
simulatorData.debugIndex = 0;
|
||||
}
|
||||
simulatorData.debugIndex++;
|
||||
if (simulatorData.debugIndex == 8) {
|
||||
simulatorData.debugIndex = 0;
|
||||
}
|
||||
|
||||
tmp_u8 = simulatorData.debugIndex |
|
||||
((mixerConfig()->platformType == PLATFORM_AIRPLANE) ? 128 : 0) |
|
||||
(ARMING_FLAG(ARMED) ? 64 : 0) |
|
||||
(!feature(FEATURE_OSD) ? 32: 0) |
|
||||
(!isOSDTypeSupportedBySimulator() ? 16 : 0);
|
||||
tmp_u8 = simulatorData.debugIndex |
|
||||
((mixerConfig()->platformType == PLATFORM_AIRPLANE) ? 128 : 0) |
|
||||
(ARMING_FLAG(ARMED) ? 64 : 0) |
|
||||
(!feature(FEATURE_OSD) ? 32: 0) |
|
||||
(!isOSDTypeSupportedBySimulator() ? 16 : 0);
|
||||
|
||||
sbufWriteU8(dst, tmp_u8);
|
||||
sbufWriteU32(dst, debug[simulatorData.debugIndex]);
|
||||
sbufWriteU8(dst, tmp_u8);
|
||||
sbufWriteU32(dst, debug[simulatorData.debugIndex]);
|
||||
|
||||
sbufWriteU16(dst, attitude.values.roll);
|
||||
sbufWriteU16(dst, attitude.values.pitch);
|
||||
sbufWriteU16(dst, attitude.values.yaw);
|
||||
sbufWriteU16(dst, attitude.values.roll);
|
||||
sbufWriteU16(dst, attitude.values.pitch);
|
||||
sbufWriteU16(dst, attitude.values.yaw);
|
||||
|
||||
mspWriteSimulatorOSD(dst);
|
||||
mspWriteSimulatorOSD(dst);
|
||||
|
||||
*ret = MSP_RESULT_ACK;
|
||||
break;
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue