Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-16 04:45:24 +03:00
Code Issues Wiki Activity

Merge pull request #7250 from mikeller/add_small_angle_to_flip_arrow

Browse source 
Added 'small_angle' indicator to OSD flip arrow.
This commit is contained in:
Michael Keller 2018-12-22 12:15:53 +13:00 • committed by GitHub
commit f827c33cd9
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
5 changed files with 41 additions and 20 deletions
Download patch file Download diff file Expand all files Collapse all files

3
src/main/fc/core.c
View file

@ -519,8 +519,9 @@ void updateMagHold(void)
if (dif >= +180) if (dif >= +180)
dif -= 360; dif -= 360;
dif *= -GET_DIRECTION(rcControlsConfig()->yaw_control_reversed); dif *= -GET_DIRECTION(rcControlsConfig()->yaw_control_reversed);
if (STATE(SMALL_ANGLE)) if (STATE(SMALL_ANGLE)) {
rcCommand[YAW] -= dif * currentPidProfile->pid[PID_MAG].P / 30; // 18 deg rcCommand[YAW] -= dif * currentPidProfile->pid[PID_MAG].P / 30; // 18 deg
}
} else } else
magHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw); magHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
} }

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

@ -157,7 +157,8 @@ void imuConfigure(uint16_t throttle_correction_angle, uint8_t throttle_correctio
{ {
imuRuntimeConfig.dcm_kp = imuConfig()->dcm_kp / 10000.0f; imuRuntimeConfig.dcm_kp = imuConfig()->dcm_kp / 10000.0f;
imuRuntimeConfig.dcm_ki = imuConfig()->dcm_ki / 10000.0f; imuRuntimeConfig.dcm_ki = imuConfig()->dcm_ki / 10000.0f;
imuRuntimeConfig.small_angle = imuConfig()->small_angle;
smallAngleCosZ = cos_approx(degreesToRadians(imuConfig()->small_angle));
fc_acc = calculateAccZLowPassFilterRCTimeConstant(5.0f); // Set to fix value fc_acc = calculateAccZLowPassFilterRCTimeConstant(5.0f); // Set to fix value
throttleAngleScale = calculateThrottleAngleScale(throttle_correction_angle); throttleAngleScale = calculateThrottleAngleScale(throttle_correction_angle);
@ -167,7 +168,6 @@ void imuConfigure(uint16_t throttle_correction_angle, uint8_t throttle_correctio
void imuInit(void) void imuInit(void)
{ {
smallAngleCosZ = cos_approx(degreesToRadians(imuRuntimeConfig.small_angle));
accVelScale = 9.80665f * acc.dev.acc_1G_rec / 10000.0f; accVelScale = 9.80665f * acc.dev.acc_1G_rec / 10000.0f;
#ifdef USE_GPS #ifdef USE_GPS

1
src/main/flight/imu.h
View file

@ -67,7 +67,6 @@ PG_DECLARE(imuConfig_t, imuConfig);
typedef struct imuRuntimeConfig_s { typedef struct imuRuntimeConfig_s {
float dcm_ki; float dcm_ki;
float dcm_kp; float dcm_kp;
uint8_t small_angle;
} imuRuntimeConfig_t; } imuRuntimeConfig_t;
void imuConfigure(uint16_t throttle_correction_angle, uint8_t throttle_correction_value); void imuConfigure(uint16_t throttle_correction_angle, uint8_t throttle_correction_value);

52
src/main/io/osd.c
View file

@ -528,7 +528,6 @@ void changeOsdProfileIndex(uint8_t profileIndex)
} }
#endif #endif
static bool osdDrawSingleElement(uint8_t item) static bool osdDrawSingleElement(uint8_t item)
{ {
if (!VISIBLE(osdConfig()->item_pos[item]) || BLINK(item)) { if (!VISIBLE(osdConfig()->item_pos[item]) || BLINK(item)) {
@ -542,25 +541,46 @@ static bool osdDrawSingleElement(uint8_t item)
switch (item) { switch (item) {
case OSD_FLIP_ARROW: case OSD_FLIP_ARROW:
{ {
const int angleR = attitude.values.roll / 10; int rollAngle = attitude.values.roll / 10;
const int angleP = attitude.values.pitch / 10; // still gotta update all angleR and angleP pointers. const int pitchAngle = attitude.values.pitch / 10;
if (isFlipOverAfterCrashActive()) { if (abs(rollAngle) > 90) {
if (angleP > 0 && ((angleR > 175 && angleR < 180) || (angleR > -180 && angleR < -175))) { rollAngle = (rollAngle < 0 ? -180 : 180) - rollAngle;
buff[0] = SYM_ARROW_SOUTH; }
} else if (angleP > 0 && angleR > 0 && angleR < 175) {
buff[0] = (SYM_ARROW_EAST + 2); if ((isFlipOverAfterCrashActive() || (!ARMING_FLAG(ARMED) && !STATE(SMALL_ANGLE))) && !((imuConfig()->small_angle < 180) && STATE(SMALL_ANGLE)) && (rollAngle || pitchAngle)) {
} else if (angleP > 0 && angleR < 0 && angleR > -175) { if (abs(pitchAngle) < 2 * abs(rollAngle) && abs(rollAngle) < 2 * abs(pitchAngle)) {
buff[0] = (SYM_ARROW_WEST + 2); if (pitchAngle > 0) {
} else if (angleP <= 0 && ((angleR > 175 && angleR < 180) || (angleR > -180 && angleR < -175))) { if (rollAngle > 0) {
buff[0] = SYM_ARROW_NORTH; buff[0] = SYM_ARROW_WEST + 2;
} else if (angleP <= 0 && angleR > 0 && angleR < 175) { } else {
buff[0] = (SYM_ARROW_NORTH + 2); buff[0] = SYM_ARROW_EAST - 2;
} else if (angleP <= 0 && angleR < 0 && angleR > -175) { }
buff[0] = (SYM_ARROW_SOUTH + 2); } else {
if (rollAngle > 0) {
buff[0] = SYM_ARROW_WEST - 2;
} else {
buff[0] = SYM_ARROW_EAST + 2;
}
}
} else {
if (abs(pitchAngle) > abs(rollAngle)) {
if (pitchAngle > 0) {
buff[0] = SYM_ARROW_SOUTH;
} else {
buff[0] = SYM_ARROW_NORTH;
}
} else {
if (rollAngle > 0) {
buff[0] = SYM_ARROW_WEST;
} else {
buff[0] = SYM_ARROW_EAST;
}
}
} }
} else { } else {
buff[0] = ' '; buff[0] = ' ';
} }
buff[1] = '\0'; buff[1] = '\0';
break; break;
} }

1
src/test/unit/osd_unittest.cc
View file

@ -89,6 +89,7 @@ extern "C" {
PG_REGISTER(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 0); PG_REGISTER(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 0);
PG_REGISTER(pilotConfig_t, pilotConfig, PG_PILOT_CONFIG, 0); PG_REGISTER(pilotConfig_t, pilotConfig, PG_PILOT_CONFIG, 0);
PG_REGISTER(gpsRescueConfig_t, gpsRescueConfig, PG_GPS_RESCUE, 0); PG_REGISTER(gpsRescueConfig_t, gpsRescueConfig, PG_GPS_RESCUE, 0);
PG_REGISTER(imuConfig_t, imuConfig, PG_IMU_CONFIG, 0);
timeUs_t simulationTime = 0; timeUs_t simulationTime = 0;
batteryState_e simulationBatteryState; batteryState_e simulationBatteryState;