diff --git a/Makefile b/Makefile index 9e279d38f..ae23a74b4 100644 --- a/Makefile +++ b/Makefile @@ -82,6 +82,7 @@ COMMON_SRC = startup_stm32f10x_md_gcc.S \ telemetry_common.c \ telemetry_frsky.c \ telemetry_hott.c \ + telemetry_msp.c \ serial_common.c \ serial_cli.c \ serial_msp.c \ diff --git a/src/serial_msp.c b/src/serial_msp.c index 160387914..62d986aa9 100755 --- a/src/serial_msp.c +++ b/src/serial_msp.c @@ -715,3 +715,54 @@ void mspProcess(void) } } } + +void sendMspTelemetry(void) +{ + static int state = -1; + + switch (state) { + default: + cmdMSP = MSP_BOXNAMES; + evaluateCommand(); + case 0: + cmdMSP = MSP_STATUS; + evaluateCommand(); + cmdMSP = MSP_IDENT; + evaluateCommand(); + state++; + break; + case 1: + cmdMSP = MSP_RAW_IMU; + evaluateCommand(); + state++; + break; + case 2: + cmdMSP = MSP_DEBUG; + evaluateCommand(); + cmdMSP = MSP_ALTITUDE; + evaluateCommand(); + state++; + break; + case 3: + cmdMSP = MSP_RAW_GPS; + evaluateCommand(); + cmdMSP = MSP_RC; + evaluateCommand(); + state++; + break; + case 4: + cmdMSP = MSP_MOTOR_PINS; + evaluateCommand(); + cmdMSP = MSP_ATTITUDE; + evaluateCommand(); + state++; + break; + case 5: + cmdMSP = MSP_SERVO; + evaluateCommand(); + state=0; + break; + } + + +} diff --git a/src/serial_msp.h b/src/serial_msp.h index c4d93dc9c..29a88b29f 100644 --- a/src/serial_msp.h +++ b/src/serial_msp.h @@ -2,4 +2,4 @@ void mspProcess(void); void mspInit(void); - +void sendMspTelemetry(void); diff --git a/src/telemetry_common.c b/src/telemetry_common.c index c3f8e4071..19f36a597 100644 --- a/src/telemetry_common.c +++ b/src/telemetry_common.c @@ -6,6 +6,7 @@ #include "telemetry_frsky.h" #include "telemetry_hott.h" +#include "telemetry_msp.h" #include "telemetry_common.h" @@ -21,6 +22,10 @@ bool isTelemetryProviderHoTT(void) return masterConfig.telemetry_provider == TELEMETRY_PROVIDER_HOTT; } +bool isTelemetryProviderMSP(void) +{ + return masterConfig.telemetry_provider == TELEMETRY_PROVIDER_MSP; +} bool canUseTelemetryWithCurrentConfiguration(void) { if (!feature(FEATURE_TELEMETRY)) { @@ -145,4 +150,7 @@ void handleTelemetry(void) if (isTelemetryProviderHoTT()) { handleHoTTTelemetry(); } + if (isTelemetryProviderMSP()) { + handleMSPTelemetry(); + } } diff --git a/src/telemetry_common.h b/src/telemetry_common.h index 887b37d99..7c30d6d22 100644 --- a/src/telemetry_common.h +++ b/src/telemetry_common.h @@ -11,7 +11,8 @@ typedef enum { TELEMETRY_PROVIDER_FRSKY = 0, TELEMETRY_PROVIDER_HOTT, - TELEMETRY_PROVIDER_MAX = TELEMETRY_PROVIDER_HOTT + TELEMETRY_PROVIDER_MSP, + TELEMETRY_PROVIDER_MAX = TELEMETRY_PROVIDER_MSP } TelemetryProvider; typedef enum { diff --git a/src/telemetry_msp.c b/src/telemetry_msp.c new file mode 100644 index 000000000..6c96ccf2f --- /dev/null +++ b/src/telemetry_msp.c @@ -0,0 +1,13 @@ +/* + * telemetry_MSP.c + * + * Created on: 22 Apr 2014 + * Author: trey marc + */ + +#include "serial_msp.h" + +void handleMSPTelemetry(void) +{ + sendMspTelemetry(); +} diff --git a/src/telemetry_msp.h b/src/telemetry_msp.h new file mode 100644 index 000000000..6616966d4 --- /dev/null +++ b/src/telemetry_msp.h @@ -0,0 +1,14 @@ +/* + * telemetry_MSP.h + * + * Created on: 22 Apr 2014 + * Author: trey marc + */ + +#ifndef TELEMETRY_MSP_H_ +#define TELEMETRY_MSP_H_ + +void handleMSPTelemetry(void); + + +#endif /* TELEMETRY_MSP_H_ */