diff --git a/conf/airframes/CDW/TwoSeas.xml b/conf/airframes/CDW/TwoSeas.xml
new file mode 100644
index 0000000000..9a7bb0699b
--- /dev/null
+++ b/conf/airframes/CDW/TwoSeas.xml
@@ -0,0 +1,268 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/airframes/CDW/conf_two_seas.xml b/conf/airframes/CDW/conf_two_seas.xml
new file mode 100644
index 0000000000..99eac6a9a5
--- /dev/null
+++ b/conf/airframes/CDW/conf_two_seas.xml
@@ -0,0 +1,12 @@
+
+
+
diff --git a/conf/telemetry/default_fixedwing.xml b/conf/telemetry/default_fixedwing.xml
index bd4bdd0633..81c04cfb01 100644
--- a/conf/telemetry/default_fixedwing.xml
+++ b/conf/telemetry/default_fixedwing.xml
@@ -28,6 +28,8 @@
+
+
diff --git a/conf/telemetry/default_fixedwing_imu.xml b/conf/telemetry/default_fixedwing_imu.xml
index 8783c77634..06206b55cf 100644
--- a/conf/telemetry/default_fixedwing_imu.xml
+++ b/conf/telemetry/default_fixedwing_imu.xml
@@ -30,6 +30,8 @@
+
+
diff --git a/conf/telemetry/default_fixedwing_imu_9k6.xml b/conf/telemetry/default_fixedwing_imu_9k6.xml
index a11857ca06..69ebd379b1 100644
--- a/conf/telemetry/default_fixedwing_imu_9k6.xml
+++ b/conf/telemetry/default_fixedwing_imu_9k6.xml
@@ -28,6 +28,8 @@
+
+
diff --git a/sw/airborne/link_mcu_usart.c b/sw/airborne/link_mcu_usart.c
index 3afdfb58a1..d4b6a999c6 100644
--- a/sw/airborne/link_mcu_usart.c
+++ b/sw/airborne/link_mcu_usart.c
@@ -244,11 +244,50 @@ struct link_mcu_msg link_mcu_from_fbw_msg;
inline void parse_mavpilot_msg( void );
+
+#ifdef AP
+#include "subsystems/datalink/telemetry.h"
+
+#define RC_OK 0
+#define RC_LOST 1
+#define RC_REALLY_LOST 2
+
+
+static void send_commands(void) {
+ DOWNLINK_SEND_COMMANDS(DefaultChannel, DefaultDevice, COMMANDS_NB, ap_state->commands);
+}
+
+
+static void send_fbw_status(void) {
+ uint8_t rc_status = 0;
+ uint8_t fbw_status = 0;
+ if (fbw_state->status & _BV(STATUS_MODE_AUTO))
+ fbw_status = FBW_MODE_AUTO;
+ if (fbw_state->status & _BV(STATUS_MODE_FAILSAFE))
+ fbw_status = FBW_MODE_FAILSAFE;
+ if (fbw_state->status & _BV(STATUS_RADIO_REALLY_LOST))
+ rc_status = RC_REALLY_LOST;
+ else if (fbw_state->status & _BV(RC_OK))
+ rc_status = RC_OK;
+ else
+ rc_status = RC_LOST;
+ DOWNLINK_SEND_FBW_STATUS(DefaultChannel, DefaultDevice,
+ &(rc_status), &(fbw_state->ppm_cpt), &(fbw_status), &(fbw_state->vsupply), &(fbw_state->current));
+}
+#endif
+
void link_mcu_init( void )
{
intermcu_data.status = LINK_MCU_UNINIT;
intermcu_data.msg_available = FALSE;
intermcu_data.error_cnt = 0;
+#ifdef AP
+ #if PERIODIC_TELEMETRY
+ // If FBW has not telemetry, then AP can send some of the info
+ register_periodic_telemetry(DefaultPeriodic, "COMMANDS", send_commands);
+ register_periodic_telemetry(DefaultPeriodic, "FBW_STATUS", send_fbw_status);
+#endif
+#endif
}
void parse_mavpilot_msg( void )