control: add rover mode

Author: bkleiner

src/config/feature.h

@@ -6,6 +6,7 @@
 #define USE_BLACKBOX
 #define USE_ADC
 #define USE_GPS
+#define USE_SERVO
 
 #ifndef SIMULATOR
 #define USE_GYRO
@@ -21,8 +22,6 @@
 #ifndef VEHICLE_ROVER
 #define USE_MOTOR_DSHOT
 #define USE_MOTOR_PWM
-#else
-#define USE_SERVO
 #endif
 #define USE_SERIAL_4WAY
 

src/control/gestures.c

@@ -29,6 +29,16 @@ typedef enum {
 #define STICKMAX 0.7f
 #define STICKCENTER 0.2f
 
+#ifdef VEHICLE_ROVER
+#define GMACRO_TRIGGER ((state.rx.throttle - 0.5f) * 2.0f)
+#define GMACRO_LEFT (GMACRO_TRIGGER < -STICKMAX)
+#define GMACRO_RIGHT (GMACRO_TRIGGER > STICKMAX)
+#define GMACRO_XCENTER (fabsf(GMACRO_TRIGGER) < STICKCENTER)
+
+#define GMACRO_DOWN (state.rx.yaw > STICKMAX)
+#define GMACRO_UP (state.rx.yaw < -STICKMAX)
+#define GMACRO_PITCHCENTER (fabsf(state.rx.yaw) < STICKCENTER)
+#else
 #define GMACRO_LEFT (state.rx.roll < -STICKMAX || state.rx.yaw < -STICKMAX)
 #define GMACRO_RIGHT (state.rx.roll > STICKMAX || state.rx.yaw > STICKMAX)
 #define GMACRO_XCENTER (fabsf(state.rx.roll) < STICKCENTER && fabsf(state.rx.yaw) < STICKCENTER)
@@ -37,6 +47,7 @@ typedef enum {
 #define GMACRO_UP (state.rx.pitch > STICKMAX)
 
 #define GMACRO_PITCHCENTER (fabsf(state.rx.pitch) < STICKCENTER)
+#endif
 
 #define GESTURETIME_MIN 50e3
 #define GESTURETIME_MAX 500e3

src/control/imu.c

@@ -112,10 +112,8 @@ void imu_calc() {
     }
   }
 
-  if (rx_aux_on(AUX_HORIZON)) {
-    state.attitude.roll = atan2approx(state.GEstG.roll, state.GEstG.yaw);
-    state.attitude.pitch = atan2approx(state.GEstG.pitch, state.GEstG.yaw);
-  }
+  state.attitude.roll = atan2approx(state.GEstG.roll, state.GEstG.yaw);
+  state.attitude.pitch = atan2approx(state.GEstG.pitch, state.GEstG.yaw);
 }
 #endif
 
@@ -165,9 +163,7 @@ void imu_calc() {
   state.GEstG.pitch = state.GEstG.pitch * (ACC_1G / GEstGmag);
   state.GEstG.yaw = state.GEstG.yaw * (ACC_1G / GEstGmag);
 
-  if (rx_aux_on(AUX_HORIZON)) {
-    state.attitude.roll = atan2approx(state.GEstG.roll, state.GEstG.yaw);
-    state.attitude.pitch = atan2approx(state.GEstG.pitch, state.GEstG.yaw);
-  }
+  state.attitude.roll = atan2approx(state.GEstG.roll, state.GEstG.yaw);
+  state.attitude.pitch = atan2approx(state.GEstG.pitch, state.GEstG.yaw);
 }
-#endif
+#endif

src/control/multi/control.c

@@ -10,6 +10,7 @@
 #include "control/multi/pid.h"
 #include "control/multi/rates.h"
 #include "control/multi/turtle_mode.h"
+#include "control/output.h"
 #include "core/profile.h"
 #include "driver/motor.h"
 #include "driver/time.h"
@@ -202,6 +203,9 @@ void control() {
   if (flags.motortest_override) {
     motor_test_calc(motortest_usb, state.motor_mix.axis);
     motor_output_calc(state.motor_mix.axis);
+#ifndef NOMOTORS
+    output_apply_mapped_sources();
+#endif
   } else if (!flags.arm_state || flags.failsafe || (state.throttle < 0.001f)) {
     flags.on_ground = 1;
     state.throttle = 0;
@@ -219,6 +223,9 @@ void control() {
 
     motor_mixer_calc(state.motor_mix.axis);
     motor_output_calc(state.motor_mix.axis);
+#ifndef NOMOTORS
+    output_apply_mapped_sources();
+#endif
   }
 
 #ifdef MOTOR_BEEPS

src/control/multi/motor.c

@@ -47,7 +47,7 @@ static float motord(float in, int x) {
   return in + out;
 }
 
-static void motor_brushless_mixer_scale_calc(float throttle, float mix[MOTOR_PIN_MAX]) {
+static void motor_brushless_mixer_scale_calc(float throttle, float mix[MULTI_MOTOR_COUNT]) {
   // only enable once really in the air
   if (flags.on_ground || !flags.in_air) {
     return;
@@ -56,7 +56,7 @@ static void motor_brushless_mixer_scale_calc(float throttle, float mix[MOTOR_PIN
   float min = FLT_MAX;
   float max = FLT_MIN;
 
-  for (uint32_t i = 0; i < MOTOR_PIN_MAX; i++) {
+  for (uint32_t i = 0; i < MULTI_MOTOR_COUNT; i++) {
     if (mix[i] < min) {
       min = mix[i];
     }
@@ -72,18 +72,18 @@ static void motor_brushless_mixer_scale_calc(float throttle, float mix[MOTOR_PIN
   const float scaled_max = max * scale;
   const float scaled_throttle = constrain(throttle, -scaled_min, 1.0f - scaled_max);
 
-  for (uint32_t i = 0; i < MOTOR_PIN_MAX; i++) {
+  for (uint32_t i = 0; i < MULTI_MOTOR_COUNT; i++) {
     mix[i] = mix[i] * scale + scaled_throttle;
   }
 }
 
-static void motor_brushed_mixer_scale_calc(float throttle, float mix[MOTOR_PIN_MAX]) {
+static void motor_brushed_mixer_scale_calc(float throttle, float mix[MULTI_MOTOR_COUNT]) {
   // throttle reduction
   float overthrottle = 0;
   float underthrottle = 0.001f;
   static float overthrottlefilt = 0;
 
-  for (uint32_t i = 0; i < MOTOR_PIN_MAX; i++) {
+  for (uint32_t i = 0; i < MULTI_MOTOR_COUNT; i++) {
     mix[i] += throttle;
 
     if (mix[i] > overthrottle)
@@ -117,7 +117,7 @@ static void motor_brushed_mixer_scale_calc(float throttle, float mix[MOTOR_PIN_M
 
   if (overthrottle > 0) { // exceeding max motor thrust
     float temp = overthrottle;
-    for (uint32_t i = 0; i < MOTOR_PIN_MAX; i++) {
+    for (uint32_t i = 0; i < MULTI_MOTOR_COUNT; i++) {
       mix[i] -= temp;
     }
   }
@@ -126,7 +126,7 @@ static void motor_brushed_mixer_scale_calc(float throttle, float mix[MOTOR_PIN_M
   if (flags.in_air == 1) {
     float underthrottle = 0;
 
-    for (uint32_t i = 0; i < MOTOR_PIN_MAX; i++) {
+    for (uint32_t i = 0; i < MULTI_MOTOR_COUNT; i++) {
       if (mix[i] < underthrottle)
         underthrottle = mix[i];
     }
@@ -136,20 +136,20 @@ static void motor_brushed_mixer_scale_calc(float throttle, float mix[MOTOR_PIN_M
       underthrottle = -(float)MIX_THROTTLE_INCREASE_MAX;
 
     if (underthrottle < 0.0f) {
-      for (uint32_t i = 0; i < MOTOR_PIN_MAX; i++)
+      for (uint32_t i = 0; i < MULTI_MOTOR_COUNT; i++)
         mix[i] -= underthrottle;
     }
   }
 }
 
-static void motor_mixer_scale_calc(float throttle, float mix[MOTOR_PIN_MAX]) {
+static void motor_mixer_scale_calc(float throttle, float mix[MULTI_MOTOR_COUNT]) {
   if (target.brushless) {
     return motor_brushless_mixer_scale_calc(throttle, mix);
   }
   return motor_brushed_mixer_scale_calc(throttle, mix);
 }
 
-void motor_test_calc(bool motortest_usb, float mix[MOTOR_PIN_MAX]) {
+void motor_test_calc(bool motortest_usb, float mix[MULTI_MOTOR_COUNT]) {
   if (motortest_usb) {
     // set mix according to values we got via usb
     mix[MOTOR_FR] = motor_test.value[MOTOR_FR];
@@ -184,7 +184,7 @@ void motor_test_calc(bool motortest_usb, float mix[MOTOR_PIN_MAX]) {
   }
 }
 
-void motor_mixer_calc(float mix[MOTOR_PIN_MAX]) {
+void motor_mixer_calc(float mix[MULTI_MOTOR_COUNT]) {
   const float yaw = profile.motor.invert_yaw ? -state.pidoutput.yaw : state.pidoutput.yaw;
 
 #ifndef MOTOR_PLUS_CONFIGURATION
@@ -202,7 +202,7 @@ void motor_mixer_calc(float mix[MOTOR_PIN_MAX]) {
 #endif
 
   if (profile.motor.torque_boost > 0.0f) {
-    for (uint32_t i = 0; i < MOTOR_PIN_MAX; i++) {
+    for (uint32_t i = 0; i < MULTI_MOTOR_COUNT; i++) {
       mix[i] = motord(mix[i], i);
     }
   }
@@ -211,7 +211,7 @@ void motor_mixer_calc(float mix[MOTOR_PIN_MAX]) {
 }
 
 //********************************MOTOR OUTPUT***********************************************************
-void motor_output_calc(float mix[MOTOR_PIN_MAX]) {
+void motor_output_calc(float mix[MULTI_MOTOR_COUNT]) {
   state.thrsum = 0; // reset throttle sum for voltage monitoring logic in main loop
 
   // only apply digital idle if we are armed and not in motor test
@@ -222,22 +222,19 @@ void motor_output_calc(float mix[MOTOR_PIN_MAX]) {
   }
 
   // Begin for-loop to send motor commands
-  for (uint32_t i = 0; i < MOTOR_PIN_MAX; i++) {
+  for (uint32_t i = 0; i < MULTI_MOTOR_COUNT; i++) {
     if (!flags.motortest_override) {
       // use values as supplied in motor test mode
       mix[i] = constrain(mix[i], 0, 1);
       mix[i] = mapf(mix[i], 0.0f, 1.0f, motor_min_value, profile.motor.motor_limit * 0.01f);
     } else if (mix[i] <= 0.0f) {
-      // zero throttle in motor test must map to MOTOR_OFF (-1) so the dshot driver sends value 0 (stop) rather than 48 (min throttle)
+      // zero throttle in motor test must map to MOTOR_OFF so the dshot driver sends stop rather than min throttle
       mix[i] = MOTOR_OFF;
     }
 
-#ifndef NOMOTORS
-    output_set_role(OUTPUT_ROLE_MOTOR_1 + i, mix[i]);
-#endif
     state.thrsum += mix[i];
   }
 
   // calculate throttle sum for voltage monitoring logic in main loop
-  state.thrsum = state.thrsum / MOTOR_PIN_MAX;
+  state.thrsum = state.thrsum / MULTI_MOTOR_COUNT;
 }

src/control/multi/motor.h

@@ -4,7 +4,9 @@
 
 #include "core/project.h"
 
-void motor_test_calc(bool motortest_usb, float mix[MOTOR_PIN_MAX]);
-void motor_mixer_calc(float mix[MOTOR_PIN_MAX]);
+#define MULTI_MOTOR_COUNT 4
 
-void motor_output_calc(float mix[MOTOR_PIN_MAX]);
+void motor_test_calc(bool motortest_usb, float mix[MULTI_MOTOR_COUNT]);
+void motor_mixer_calc(float mix[MULTI_MOTOR_COUNT]);
+
+void motor_output_calc(float mix[MULTI_MOTOR_COUNT]);