support imu and gps over xbee (not tested)

[fpv.git] / mainboard / commands.c

diff --git a/mainboard/commands.c b/mainboard/commands.c
index 1837fda..d0ff2a1 100644 (file)
--- a/mainboard/commands.c
+++ b/mainboard/commands.c
@@ -1622,6 +1622,8 @@ struct cmd_rc_proto_timers_result {
        uint16_t servo_min;
        uint16_t servo_max;
        uint16_t power_probe;
+       uint16_t gps_pos;
+       uint16_t imu_pos;
        uint16_t autobypass;
 };
 
@@ -1634,14 +1636,18 @@ static void cmd_rc_proto_timers_parsed(void *parsed_result, void *data)
                rc_proto_timers.send_servo_min_ms = res->servo_min;
                rc_proto_timers.send_servo_max_ms = res->servo_max;
                rc_proto_timers.send_power_probe_ms = res->power_probe;
+               rc_proto_timers.send_gps_pos_ms = res->gps_pos;
+               rc_proto_timers.send_imu_pos_ms = res->imu_pos;
                rc_proto_timers.autobypass_ms = res->autobypass;
        }
 
        printf_P(PSTR("rc_proto_timers: min=%d, max=%d, "
-                       "power_probe=%d autobypass=%d\n"),
+                       "power_probe=%d gps_pos=%d imu_pos=%d autobypass=%d\n"),
                rc_proto_timers.send_servo_min_ms,
                rc_proto_timers.send_servo_max_ms,
                rc_proto_timers.send_power_probe_ms,
+               rc_proto_timers.send_gps_pos_ms,
+               rc_proto_timers.send_imu_pos_ms,
                rc_proto_timers.autobypass_ms);
 }
 
@@ -1662,12 +1668,18 @@ const parse_token_num_t PROGMEM cmd_rc_proto_timers_servo_max =
 const parse_token_num_t PROGMEM cmd_rc_proto_timers_power_probe =
        TOKEN_NUM_INITIALIZER(struct cmd_rc_proto_timers_result, power_probe,
                UINT16);
+const parse_token_num_t PROGMEM cmd_rc_proto_timers_gps_pos =
+       TOKEN_NUM_INITIALIZER(struct cmd_rc_proto_timers_result, gps_pos,
+               UINT16);
+const parse_token_num_t PROGMEM cmd_rc_proto_timers_imu_pos =
+       TOKEN_NUM_INITIALIZER(struct cmd_rc_proto_timers_result, imu_pos,
+               UINT16);
 const parse_token_num_t PROGMEM cmd_rc_proto_timers_autobypass =
        TOKEN_NUM_INITIALIZER(struct cmd_rc_proto_timers_result, autobypass,
                UINT16);
 
 const char PROGMEM help_rc_proto_timers[] = "set rc_proto_timers (servo_min, "
-       "servo_max, pow_probe, autobypass)";
+       "servo_max, pow_probe, gps_pos, imu_pos, autobypass)";
 const parse_inst_t PROGMEM cmd_rc_proto_timers = {
        .f = cmd_rc_proto_timers_parsed,  /* function to call */
        .data = NULL,      /* 2nd arg of func */
@@ -1678,6 +1690,8 @@ const parse_inst_t PROGMEM cmd_rc_proto_timers = {
                (PGM_P)&cmd_rc_proto_timers_servo_min,
                (PGM_P)&cmd_rc_proto_timers_servo_max,
                (PGM_P)&cmd_rc_proto_timers_power_probe,
+               (PGM_P)&cmd_rc_proto_timers_gps_pos,
+               (PGM_P)&cmd_rc_proto_timers_imu_pos,
                (PGM_P)&cmd_rc_proto_timers_autobypass,
                NULL,
        },
@@ -1713,7 +1727,7 @@ static void cmd_rc_proto_mode_parsed(void *parsed_result, void *data)
 {
        struct cmd_rc_proto_mode_result *res = parsed_result;
        (void)data;
-       uint8_t flags;
+       uint16_t flags;
        uint8_t on = 0;
 
        flags = rc_proto_get_mode();
@@ -1744,6 +1758,18 @@ static void cmd_rc_proto_mode_parsed(void *parsed_result, void *data)
                else
                        flags &= ~RC_PROTO_FLAGS_TX_POW_PROBE;
        }
+       else if (!strcmp_P(res->cmd, PSTR("tx_gps_pos"))) {
+               if (on == 1)
+                       flags |= RC_PROTO_FLAGS_TX_GPS_POS;
+               else
+                       flags &= ~RC_PROTO_FLAGS_TX_GPS_POS;
+       }
+       else if (!strcmp_P(res->cmd, PSTR("tx_imu_pos"))) {
+               if (on == 1)
+                       flags |= RC_PROTO_FLAGS_TX_IMU_POS;
+               else
+                       flags &= ~RC_PROTO_FLAGS_TX_IMU_POS;
+       }
        else if (!strcmp_P(res->cmd, PSTR("compute_best_pow"))) {
                if (on == 1)
                        flags |= RC_PROTO_FLAGS_COMPUTE_BEST_POW;
@@ -1774,6 +1800,10 @@ static void cmd_rc_proto_mode_parsed(void *parsed_result, void *data)
                (flags & RC_PROTO_FLAGS_TX_STATS) ? "on" : "off");
        printf_P(PSTR("rc_proto_mode tx_power_probe %s\n"),
                (flags & RC_PROTO_FLAGS_TX_POW_PROBE) ? "on" : "off");
+       printf_P(PSTR("rc_proto_mode tx_gps_pos %s\n"),
+               (flags & RC_PROTO_FLAGS_TX_GPS_POS) ? "on" : "off");
+       printf_P(PSTR("rc_proto_mode tx_imu_pos %s\n"),
+               (flags & RC_PROTO_FLAGS_TX_IMU_POS) ? "on" : "off");
        printf_P(PSTR("rc_proto_mode compute_best_pow %s\n"),
                (flags & RC_PROTO_FLAGS_COMPUTE_BEST_POW) ? "on" : "off");
 }
@@ -1784,7 +1814,8 @@ const parse_token_string_t PROGMEM cmd_rc_proto_mode_arg0 =
                                 str_rc_proto_mode_arg0);
 
 const char PROGMEM str_rc_proto_mode_cmd[] =
-       "rx_copy_spi#rx_autobypass#tx_stats#tx_power_probe#compute_best_pow";
+       "rx_copy_spi#rx_autobypass#tx_stats#tx_power_probe#"
+       "tx_gps_pos#tx_imu_pos#compute_best_pow";
 const parse_token_string_t PROGMEM cmd_rc_proto_mode_cmd =
        TOKEN_STRING_INITIALIZER(struct cmd_rc_proto_mode_result, cmd,
                str_rc_proto_mode_cmd);