diff --git a/sw/airborne/csc/csc_airspeed.c b/sw/airborne/csc/csc_airspeed.c new file mode 100644 index 0000000000..6991ef2d27 --- /dev/null +++ b/sw/airborne/csc/csc_airspeed.c @@ -0,0 +1,9 @@ +#include "csc_airspeed.h" +#include "csc_ap_link.h" + +float estimator_airspeed; + +void csc_airspeed_periodic(void) +{ + csc_ap_link_send_airspeed(estimator_airspeed, 0); +} diff --git a/sw/airborne/csc/csc_airspeed.h b/sw/airborne/csc/csc_airspeed.h new file mode 100644 index 0000000000..5020ecd4c1 --- /dev/null +++ b/sw/airborne/csc/csc_airspeed.h @@ -0,0 +1 @@ +void csc_airspeed_periodic(void); diff --git a/sw/airborne/csc/csc_ap_link.c b/sw/airborne/csc/csc_ap_link.c index 33aa11b588..f974f5c473 100644 --- a/sw/airborne/csc/csc_ap_link.c +++ b/sw/airborne/csc/csc_ap_link.c @@ -47,6 +47,18 @@ void csc_ap_link_send_vane(float *vane_angles) } +void csc_ap_link_send_airspeed(float airspeed1, float airspeed2) +{ + + struct CscAirspeedMsg msg; + + msg.airspeed1 = airspeed1; + msg.airspeed2 = airspeed2; + + csc_ap_send_msg(CSC_AIRSPEED_MSG_ID, (const uint8_t *) &msg, sizeof(msg)); +} + + // Generic function for sending can messages void can_write_csc(uint8_t board_id, uint8_t msg_id, const uint8_t *buf, uint8_t len) { diff --git a/sw/airborne/csc/csc_ap_link.h b/sw/airborne/csc/csc_ap_link.h index cfcc660ee8..6a4461e233 100644 --- a/sw/airborne/csc/csc_ap_link.h +++ b/sw/airborne/csc/csc_ap_link.h @@ -15,6 +15,7 @@ void can_write_csc(uint8_t board_id, uint8_t msg_id, const uint8_t *buf, uint8_t void csc_ap_link_send_status(uint32_t loops, uint32_t msgs); void csc_ap_link_send_adc(float adc1, float adc2); void csc_ap_link_send_vane(float *vane_angle); +void csc_ap_link_send_airspeed(float airspeed1, float airspeed2); void csc_ap_link_set_servo_cmd_cb(void (* cb)(struct CscServoCmd *cmd)); void csc_ap_link_set_motor_cmd_cb(void (* cb)(struct CscMotorMsg *msg)); void csc_ap_link_set_prop_cmd_cb(void (* cb)(struct CscPropCmd *cmd, int idx)); diff --git a/sw/airborne/csc/csc_msg_def.h b/sw/airborne/csc/csc_msg_def.h index e3767fa2bd..144c346bc8 100644 --- a/sw/airborne/csc/csc_msg_def.h +++ b/sw/airborne/csc/csc_msg_def.h @@ -19,6 +19,7 @@ #define CSC_GPS_ACC_ID 9 #define CSC_PROP2_CMD_ID 10 #define CSC_VANE_MSG_ID 11 +#define CSC_AIRSPEED_MSG_ID 12 /* Received from the autopilot */ @@ -79,6 +80,11 @@ struct CscVaneMsg { float vane_angle2; } __attribute__((packed)); +struct CscAirspeedMsg { + float airspeed1; + float airspeed2; +} __attribute__((packed)); + #define CSC_RC_SCALE 20 #define CSC_RC_OFFSET 2*(MAX_PPRZ/CSC_RC_SCALE) /* Sorry this is a bit arbitrary. - mmt */ diff --git a/sw/airborne/csc/mercury_csc_main.c b/sw/airborne/csc/mercury_csc_main.c index 1c670fb5d3..3c40505d8e 100644 --- a/sw/airborne/csc/mercury_csc_main.c +++ b/sw/airborne/csc/mercury_csc_main.c @@ -50,6 +50,7 @@ #include "periodic.h" #include "downlink.h" #include "pwm_input.h" +#include "csc_airspeed.h" #include "csc_adc.h" #include "csc_rc_spektrum.h" @@ -173,6 +174,7 @@ static void csc_main_periodic( void ) { #ifdef USE_AIRSPEED airspeed_update(); + csc_airspeed_periodic(); #endif }