diff options
| -rw-r--r-- | altoslib/AltosState.java | 12 | ||||
| -rw-r--r-- | src/core/ao_flight.c | 2 | ||||
| -rw-r--r-- | src/core/ao_kalman.c | 2 |
3 files changed, 4 insertions, 12 deletions
diff --git a/altoslib/AltosState.java b/altoslib/AltosState.java index 134aeb4e..08dafbb4 100644 --- a/altoslib/AltosState.java +++ b/altoslib/AltosState.java @@ -949,14 +949,8 @@ public class AltosState implements Cloneable { } void update_accel() { - double ground = ground_accel; - - if (ground == AltosLib.MISSING) - ground = ground_accel_avg; if (accel == AltosLib.MISSING) return; - if (ground == AltosLib.MISSING) - return; if (accel_plus_g == AltosLib.MISSING) return; if (accel_minus_g == AltosLib.MISSING) @@ -964,7 +958,7 @@ public class AltosState implements Cloneable { double counts_per_g = (accel_minus_g - accel_plus_g) / 2.0; double counts_per_mss = counts_per_g / 9.80665; - acceleration.set_measured((ground - accel) / counts_per_mss, time); + acceleration.set_measured((accel_plus_g - accel) / counts_per_mss, time); } public void set_accel_g(double accel_plus_g, double accel_minus_g) { @@ -976,10 +970,8 @@ public class AltosState implements Cloneable { } public void set_ground_accel(double ground_accel) { - if (ground_accel != AltosLib.MISSING) { + if (ground_accel != AltosLib.MISSING) this.ground_accel = ground_accel; - update_accel(); - } } public void set_accel(double accel) { diff --git a/src/core/ao_flight.c b/src/core/ao_flight.c index 08302140..702c3403 100644 --- a/src/core/ao_flight.c +++ b/src/core/ao_flight.c @@ -401,7 +401,7 @@ ao_flight_dump(void) #if HAS_ACCEL int16_t accel; - accel = ((ao_ground_accel - ao_sample_accel) * ao_accel_scale) >> 16; + accel = ((ao_config.accel_plus_g - ao_sample_accel) * ao_accel_scale) >> 16; #endif printf ("sample:\n"); diff --git a/src/core/ao_kalman.c b/src/core/ao_kalman.c index 7fd4f889..9aea1f14 100644 --- a/src/core/ao_kalman.c +++ b/src/core/ao_kalman.c @@ -166,7 +166,7 @@ ao_kalman_err_accel(void) { int32_t accel; - accel = (ao_ground_accel - ao_sample_accel) * ao_accel_scale; + accel = (ao_config.accel_plus_g - ao_sample_accel) * ao_accel_scale; /* Can't use ao_accel here as it is the pre-prediction value still */ ao_error_a = (accel - ao_k_accel) >> 16; |