Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with HTTPS or Subversion.

Download ZIP
Browse files

Adjust throttle when tilted.

  • Loading branch information...
commit 35af86132c83cac7c635f7fa6123673c4f93ead4 1 parent 686f6db
@balrog-kun authored
Showing with 17 additions and 4 deletions.
  1. +12 −4 pilot.c
  2. +5 −0 trig.h
View
16 pilot.c
@@ -258,7 +258,7 @@ static int constants_cnt = 0;
static void control_update(void) {
int16_t cur_pitch, cur_roll, cur_yaw, raw_pitch, raw_roll;
- int16_t dest_pitch, dest_roll, dest_yaw, base_throttle;
+ int16_t dest_pitch, dest_roll, dest_yaw, base_throttle, z;
static uint8_t yaw_deadband_pos = 0x80;
static uint8_t roll_deadband_pos = 0x80;
@@ -359,10 +359,18 @@ static void control_update(void) {
neutral_yaw += ((int16_t) co_right << 2) - (128 << 2);
dest_yaw = neutral_yaw;
- /* TODO: divide by cos(angle from neutral), which we can probably
- * approximate with (dest-cur_pitch / 90 + dest-cur_roll / 90) for now
- */
base_throttle = co_throttle << 7;
+ /* Adjust throttle for the current tilt so we don't lose altitude
+ * whenever accelerating. */
+ z = rotate_z(cur_yaw, -cur_pitch, -cur_roll);
+ if (z < 0) {
+ if (base_throttle > 0x3000)
+ base_throttle = 0x3000; /* XXX */
+ } else {
+ if (z < (1 << 14))
+ z = 1 << 14;
+ base_throttle = ((uint32_t) base_throttle << 15) / z;
+ }
dest_pitch = -(cur_pitch + dest_pitch) / 1;
dest_roll = -(cur_roll + dest_roll) / 1;
View
5 trig.h
@@ -70,3 +70,8 @@ static inline uint32_t hypot3(int16_t v[3]) {
return (int32_t) v[0] * v[0] + (int32_t) v[1] * v[1] +
(int32_t) v[2] * v[2];
}
+
+/* Rotate_rev { 0, 0, 1 } and return z */
+static inline int16_t rotate_z(int16_t y, int32_t p, int32_t r) {
+ return ((int32_t) cos_32_l(-p) * cos_32_l(-r) + 0x3fff) >> 15;
+}
Please sign in to comment.
Something went wrong with that request. Please try again.