Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-25 17:25:18 +03:00
Code Issues Wiki Activity

reverted unrelated changes

Browse source
This commit is contained in:
Roman Lut 2022-07-30 05:17:54 +03:00
parent a8860e4285
commit c36154172e
13 changed files with 70 additions and 57 deletions
Download patch file Download diff file Expand all files Collapse all files

45
src/main/fc/fc_msp.c
View file

@ -21,7 +21,7 @@
#include <string.h>
#include <math.h>
#include "common/log.h"
#include "common/log.h" //for MSP_SIMULATOR
#include "platform.h"
#include "blackbox/blackbox.h"
@ -100,8 +100,8 @@
#include "msp/msp_serial.h"
#include "navigation/navigation.h"
#include "navigation/navigation_private.h"
#include "navigation/navigation_pos_estimator_private.h"
#include "navigation/navigation_private.h" //for MSP_SIMULATOR
#include "navigation/navigation_pos_estimator_private.h" //for MSP_SIMULATOR
#include "rx/rx.h"
#include "rx/msp.h"
@ -607,14 +607,12 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
break;
case MSP_ALTITUDE:
sbufWriteU32(dst, lrintf(getEstimatedActualPosition(Z)));
sbufWriteU16(dst, lrintf(getEstimatedActualVelocity(Z)));
#if defined(USE_BARO)
sbufWriteU32(dst, baro.BaroAlt);
sbufWriteU32(dst, baro.BaroMslAlt);
sbufWriteU32(dst, baro.baroPressure);
sbufWriteU32(dst, baroGetLatestAltitude());
#else
sbufWriteU32(dst, 0);
sbufWriteU32(dst, 0);
sbufWriteU32(dst, 0);
#endif
break;
@ -2480,8 +2478,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
#ifdef USE_GPS
case MSP_SET_RAW_GPS:
if (dataSize == 14) {
gpsSol.fixType = sbufReadU8(src);
if (gpsSol.fixType) {
if (sbufReadU8(src)) {
ENABLE_STATE(GPS_FIX);
} else {
DISABLE_STATE(GPS_FIX);
@ -3188,6 +3185,7 @@ static bool mspParameterGroupsCommand(sbuf_t *dst, sbuf_t *src)
return true;
}
#ifdef USE_SIMULATOR
bool isOSDTypeSupportedBySimulator(void)
{
displayPort_t *osdDisplayPort = osdGetDisplayPort();
@ -3320,12 +3318,11 @@ void mspWriteSimulatorOSD(sbuf_t *dst)
sbufWriteU8(dst, 255);
}
}
#endif
bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResult_e *ret)
{
uint8_t tmp_u8;
// uint16_t tmp_u16;
const unsigned int dataSize = sbufBytesRemaining(src);
switch (cmdMSP) {
@ -3423,6 +3420,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
*ret = MSP_RESULT_ACK;
break;
#ifdef USE_SIMULATOR
case MSP_SIMULATOR:
tmp_u8 = sbufReadU8(src); //MSP_SIMULATOR version
if (tmp_u8 != 2) break;
@ -3551,27 +3549,6 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
sbufWriteU16(dst, (uint16_t)simulatorData.INPUT_STABILIZED_YAW);
sbufWriteU16(dst, (uint16_t)(ARMING_FLAG(ARMED) ? simulatorData.INPUT_STABILIZED_THROTTLE : -500));
/*
debug[0] = mag.magADC[X];
debug[1] = mag.magADC[Y];
debug[2] = mag.magADC[Z];
//debug[3] = sensors(SENSOR_MAG) ? 1 : 0;
//debug[4] = STATE(COMPASS_CALIBRATED) ? 1 : 0;
fpVector3_t vMag;
fpVector3_t magBF;
magBF.x = mag.magADC[X];
magBF.y = mag.magADC[Y];
magBF.z = mag.magADC[Z];
quaternionRotateVectorInv(&vMag, &magBF, &orientation); // BF -> EF
debug[3] = vMag.x;
debug[4] = vMag.y;
debug[5] = vMag.z;
debug[6] = getvCorrectedMagNorth().x*1000;
debug[7] = getvCorrectedMagNorth().y*1000;
*/
simulatorData.debugIndex++;
if (simulatorData.debugIndex == 8) {
simulatorData.debugIndex = 0;
@ -3593,6 +3570,8 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
*ret = MSP_RESULT_ACK;
break;
#endif
default:
// Not handled
return false;

38
src/main/flight/imu.c
View file

@ -600,12 +600,37 @@ static void imuCalculateEstimatedAttitude(float dT)
imuUpdateEulerAngles();
}
#ifdef HIL
void imuHILUpdate(void)
{
/* Set attitude */
attitude.values.roll = hilToFC.rollAngle;
attitude.values.pitch = hilToFC.pitchAngle;
attitude.values.yaw = hilToFC.yawAngle;
/* Compute rotation quaternion for future use */
imuComputeQuaternionFromRPY(attitude.values.roll, attitude.values.pitch, attitude.values.yaw);
/* Fake accADC readings */
accADCf[X] = hilToFC.bodyAccel[X] / GRAVITY_CMSS;
accADCf[Y] = hilToFC.bodyAccel[Y] / GRAVITY_CMSS;
accADCf[Z] = hilToFC.bodyAccel[Z] / GRAVITY_CMSS;
}
#endif
void imuUpdateAccelerometer(void)
{
#ifdef HIL
if (sensors(SENSOR_ACC) && !hilActive) {
accUpdate();
isAccelUpdatedAtLeastOnce = true;
}
#else
if (sensors(SENSOR_ACC)) {
accUpdate();
isAccelUpdatedAtLeastOnce = true;
}
#endif
}
void imuCheckVibrationLevels(void)
@ -630,10 +655,23 @@ void imuUpdateAttitude(timeUs_t currentTimeUs)
previousIMUUpdateTimeUs = currentTimeUs;
if (sensors(SENSOR_ACC) && isAccelUpdatedAtLeastOnce) {
#ifdef HIL
if (!hilActive) {
gyroGetMeasuredRotationRate(&imuMeasuredRotationBF); // Calculate gyro rate in body frame in rad/s
accGetMeasuredAcceleration(&imuMeasuredAccelBF); // Calculate accel in body frame in cm/s/s
imuCheckVibrationLevels();
imuCalculateEstimatedAttitude(dT); // Update attitude estimate
}
else {
imuHILUpdate();
imuUpdateMeasuredAcceleration();
}
#else
gyroGetMeasuredRotationRate(&imuMeasuredRotationBF); // Calculate gyro rate in body frame in rad/s
accGetMeasuredAcceleration(&imuMeasuredAccelBF); // Calculate accel in body frame in cm/s/s
imuCheckVibrationLevels();
imuCalculateEstimatedAttitude(dT); // Update attitude estimate
#endif
} else {
acc.accADCf[X] = 0.0f;
acc.accADCf[Y] = 0.0f;

3
src/main/flight/servos.c
View file

@ -246,7 +246,6 @@ void writeServos(void)
void servoMixer(float dT)
{
int16_t input[INPUT_SOURCE_COUNT]; // Range [-500:+500]
if (FLIGHT_MODE(MANUAL_MODE)) {
input[INPUT_STABILIZED_ROLL] = rcCommand[ROLL];
input[INPUT_STABILIZED_PITCH] = rcCommand[PITCH];
@ -572,9 +571,11 @@ void processContinuousServoAutotrim(const float dT)
}
void processServoAutotrim(const float dT) {
#ifdef USE_SIMULATOR
if (ARMING_FLAG(SIMULATOR_MODE)) {
return;
}
#endif
if (feature(FEATURE_FW_AUTOTRIM)) {
processContinuousServoAutotrim(dT);
} else {

2
src/main/io/osd_dji_hd.c
View file

@ -537,8 +537,10 @@ static char * osdArmingDisabledReasonMessage(void)
FALLTHROUGH;
case ARMED:
FALLTHROUGH;
#ifdef USE_SIMULATOR
case SIMULATOR_MODE:
FALLTHROUGH;
#endif
case WAS_EVER_ARMED:
break;
}

3
src/main/navigation/navigation_pos_estimator.c
View file

@ -629,7 +629,7 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
}
DEBUG_SET(DEBUG_ALTITUDE, 0, posEstimator.est.pos.z); // Position estimate
DEBUG_SET(DEBUG_ALTITUDE, 2, imuMeasuredAccelBF.z); // Baro altitude
DEBUG_SET(DEBUG_ALTITUDE, 2, posEstimator.baro.alt); // Baro altitude
DEBUG_SET(DEBUG_ALTITUDE, 4, posEstimator.gps.pos.z); // GPS altitude
DEBUG_SET(DEBUG_ALTITUDE, 6, accGetVibrationLevel()); // Vibration level
DEBUG_SET(DEBUG_ALTITUDE, 1, posEstimator.est.vel.z); // Vertical speed estimate
@ -858,7 +858,6 @@ void initializePositionEstimator(void)
pt1FilterInit(&posEstimator.surface.avgFilter, INAV_SURFACE_AVERAGE_HZ, 0.0f);
}
/**
* Update estimator
* Update rate: loop rate (>100Hz)

1
src/main/sensors/acceleration.c
View file

@ -552,7 +552,6 @@ void accUpdate(void)
// Filter acceleration
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
acc.accADCf[axis] = accSoftLpfFilterApplyFn(accSoftLpfFilter[axis], acc.accADCf[axis]);
DEBUG_SET(DEBUG_ACC, axis, acc.accADCf[axis] * 1000);
}
if (accelerometerConfig()->acc_notch_hz) {

2
src/main/sensors/barometer.c
View file

@ -332,7 +332,7 @@ int32_t baroCalculateAltitude(void)
}
#endif
// calculates height from ground via baro readings
baro.BaroMslAlt = pressureToAltitude(baro.baroPressure) - baroGroundAltitude;
baro.BaroAlt = pressureToAltitude(baro.baroPressure) - baroGroundAltitude;
}
return baro.BaroAlt;

2
src/main/sensors/battery.c
View file

@ -291,7 +291,7 @@ static void updateBatteryVoltage(timeUs_t timeDelta, bool justConnected)
break;
}
#ifdef USE_SIMULATOR
if (!ARMING_FLAG(SIMULATOR_MODE)) {
if (ARMING_FLAG(SIMULATOR_MODE)) {
if (simulatorData.flags & SIMU_SIMULATE_BATTERY) {
vbat = 1260;
batteryFullVoltage = 1210;

9
src/main/target/HGLRCF722/target.c
View file

@ -41,13 +41,8 @@ timerHardware_t timerHardware[] = {
DEF_TIM(TIM3, CH2, PB5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S2 UP1-2 D(1, 5, 5)
DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3 UP1-2 D(1, 7, 5)
DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 UP1-2 D(1, 2, 5)
// DEF_TIM(TIM2, CH1, PA15, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S5 UP1-7 D(1, 5, 3) - clash with S2
// DEF_TIM(TIM2, CH2, PB3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6 UP1-7 D(1, 6, 3)
DEF_TIM(TIM2, CH3, PB10, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S5 UP1-7 D(1, 5, 3) - clash with S2
DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6 UP1-7 D(1, 6, 3)
DEF_TIM(TIM2, CH1, PA15, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S5 UP1-7 D(1, 5, 3) - clash with S2
DEF_TIM(TIM2, CH2, PB3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6 UP1-7 D(1, 6, 3)
DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 0, 0), // S7 UP1-6 D(1, 0, 2)
DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 0, 0), // S8 UP1-6 D(1, 3, 2)