From 765d09a52714aca736398c28b8a0f704dc36d6c3 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Wed, 5 Nov 2014 22:21:32 +0100 Subject: [PATCH] [telemetry] fixing and cleaning a few stuff --- sw/airborne/boards/ardrone/navdata.c | 6 ++-- sw/airborne/firmwares/beth/overo_test_uart.c | 12 +++---- sw/airborne/firmwares/fixedwing/autopilot.c | 6 ++-- sw/airborne/firmwares/fixedwing/datalink.c | 2 +- sw/airborne/firmwares/fixedwing/main_ap.c | 22 ++----------- sw/airborne/firmwares/fixedwing/nav.c | 6 ++-- sw/airborne/firmwares/fixedwing/nav.h | 4 +-- sw/airborne/firmwares/rotorcraft/main.c | 14 ++------- sw/airborne/fms/udp_transport.h | 6 ++-- sw/airborne/mcu_periph/i2c.c | 2 +- sw/airborne/modules/digital_cam/hackhd.h | 2 +- sw/airborne/subsystems/datalink/downlink.c | 31 ++++++++++++++++--- sw/airborne/subsystems/datalink/downlink.h | 15 ++++++--- .../subsystems/datalink/ivy_transport.c | 4 +-- .../subsystems/datalink/pprz_transport.c | 5 +-- .../subsystems/datalink/pprzlog_transport.c | 1 + sw/airborne/subsystems/datalink/xbee.c | 5 +-- sw/simulator/nps/nps_ivy_common.c | 1 + 18 files changed, 76 insertions(+), 68 deletions(-) diff --git a/sw/airborne/boards/ardrone/navdata.c b/sw/airborne/boards/ardrone/navdata.c index 8556b84793..4825fa28f1 100644 --- a/sw/airborne/boards/ardrone/navdata.c +++ b/sw/airborne/boards/ardrone/navdata.c @@ -154,7 +154,7 @@ static void send_navdata(struct transport_tx *trans, struct device *dev) { &nav_port.checksum_errors); } -static void send_fliter_status(void) { +static void send_filter_status(struct transport_tx *trans, struct device *dev) { uint8_t mde = 3; if (ahrs.status == AHRS_UNINIT) mde = 2; if (imu_lost) mde = 5; @@ -231,7 +231,7 @@ bool_t navdata_init() #if PERIODIC_TELEMETRY register_periodic_telemetry(DefaultPeriodic, "ARDRONE_NAVDATA", send_navdata); - register_periodic_telemetry(DefaultPeriodic, "STATE_FILTER_STATUS", send_fliter_status); + register_periodic_telemetry(DefaultPeriodic, "STATE_FILTER_STATUS", send_filter_status); #endif return TRUE; @@ -324,7 +324,7 @@ static void mag_freeze_check(void) { uint8_t mde = 5; uint16_t val = 0; - pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &mde, &val); + DOWNLINK_SEND_STATE_FILTER_STATUS(DefaultChannel, DefaultDevice, &mde, &val); // stop acquisition uint8_t cmd=0x02; diff --git a/sw/airborne/firmwares/beth/overo_test_uart.c b/sw/airborne/firmwares/beth/overo_test_uart.c index e7ad7d3eb6..1cceade1cc 100644 --- a/sw/airborne/firmwares/beth/overo_test_uart.c +++ b/sw/airborne/firmwares/beth/overo_test_uart.c @@ -146,9 +146,9 @@ static void main_periodic(int my_sig_num) { #include "subsystems/datalink/downlink.h" #if 0 -uint8_t downlink_nb_ovrn; -uint16_t downlink_nb_bytes; -uint16_t downlink_nb_msgs; +uint8_t downlink.nb_ovrn; +uint16_t downlink.nb_bytes; +uint16_t downlink.nb_msgs; #define __Transport(dev, _x) dev##_x #define _Transport(dev, _x) __Transport(dev, _x) @@ -161,11 +161,11 @@ uint16_t downlink_nb_msgs; #define DownlinkPutUint8ByAddr(_chan, _x) Transport(_chan, PutUint8ByAddr(_x)) #define DownlinkPutUint8Array(_chan, _n, _x) Transport(_chan, PutUint8Array(_n, _x)) -#define DownlinkOverrun(_chan) downlink_nb_ovrn++; -#define DownlinkCountBytes(_chan, _n) downlink_nb_bytes += _n; +#define DownlinkOverrun(_chan) downlink.nb_ovrn++; +#define DownlinkCountBytes(_chan, _n) downlink.nb_bytes += _n; #define DownlinkStartMessage(_chan, _name, msg_id, payload_len) { \ - downlink_nb_msgs++; \ + downlink.nb_msgs++; \ Transport(_chan, Header(DownlinkIDsSize(_chan, payload_len))); \ Transport(_chan, PutUint8(AC_ID)); \ Transport(_chan, PutNamedUint8(_name, msg_id)); \ diff --git a/sw/airborne/firmwares/fixedwing/autopilot.c b/sw/airborne/firmwares/fixedwing/autopilot.c index 1ecb3213e5..505a3709ae 100644 --- a/sw/airborne/firmwares/fixedwing/autopilot.c +++ b/sw/airborne/firmwares/fixedwing/autopilot.c @@ -138,10 +138,10 @@ static void send_airspeed(struct transport_tx *trans __attribute__((unused)), st static void send_downlink(struct transport_tx *trans, struct device *dev) { static uint16_t last; - uint16_t rate = (downlink_nb_bytes - last) / PERIOD_DOWNLINK_Ap_0; - last = downlink_nb_bytes; + uint16_t rate = (downlink.nb_bytes - last) / PERIOD_DOWNLINK_Ap_0; + last = downlink.nb_bytes; pprz_msg_send_DOWNLINK(trans, dev, AC_ID, - &downlink_nb_ovrn, &rate, &downlink_nb_msgs); + &downlink.nb_ovrn, &rate, &downlink.nb_msgs); } void autopilot_init(void) { diff --git a/sw/airborne/firmwares/fixedwing/datalink.c b/sw/airborne/firmwares/fixedwing/datalink.c index ecc1af5c73..b37fd5b324 100644 --- a/sw/airborne/firmwares/fixedwing/datalink.c +++ b/sw/airborne/firmwares/fixedwing/datalink.c @@ -139,7 +139,7 @@ void dl_parse_msg(void) { DOWNLINK_SEND_WP_MOVED(DefaultChannel, DefaultDevice, &wp_id, &utm.east, &utm.north, &a, &nav_utm_zone0); } else if (msg_id == DL_BLOCK && DL_BLOCK_ac_id(dl_buffer) == AC_ID) { nav_goto_block(DL_BLOCK_block_id(dl_buffer)); - SEND_NAVIGATION(DefaultChannel, DefaultDevice); + SEND_NAVIGATION(&(DefaultChannel).trans_tx, &(DefaultDevice).device); } else #endif /** NAV */ #ifdef WIND_INFO diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c index c575369ba8..fc9665d5df 100644 --- a/sw/airborne/firmwares/fixedwing/main_ap.c +++ b/sw/airborne/firmwares/fixedwing/main_ap.c @@ -73,12 +73,9 @@ PRINT_CONFIG_MSG_VALUE("USE_BARO_BOARD is TRUE, reading onboard baro: ", BARO_BO // datalink & telemetry #include "subsystems/datalink/datalink.h" +#include "subsystems/datalink/downlink.h" #include "subsystems/datalink/telemetry.h" #include "subsystems/settings.h" -#include "subsystems/datalink/pprz_transport.h" -#include "subsystems/datalink/xbee.h" -#include "subsystems/datalink/w5100.h" -#include "subsystems/datalink/ivy_transport.h" // modules & settings #include "generated/modules.h" @@ -244,20 +241,7 @@ void init_ap( void ) { /** - start interrupt task */ mcu_int_enable(); -#if defined DATALINK -#if DATALINK == PPRZ - pprz_transport_init(); -#endif -#if DATALINK == XBEE - xbee_init(); -#endif -#if DATALINK == W5100 - w5100_init(); -#endif -#endif /* DATALINK */ -#if SITL - ivy_transport_init(); -#endif + downlink_init(); #if defined AEROCOMM_DATA_PIN IO0DIR |= _BV(AEROCOMM_DATA_PIN); @@ -513,7 +497,7 @@ void navigation_task( void ) { #endif #ifndef PERIOD_NAVIGATION_Ap_0 // If not sent periodically (in default 0 mode) - SEND_NAVIGATION(DefaultChannel, DefaultDevice); + SEND_NAVIGATION(&(DefaultChannel).trans_tx, &(DefaultDevice).device); #endif /* The nav task computes only nav_altitude. However, we are interested diff --git a/sw/airborne/firmwares/fixedwing/nav.c b/sw/airborne/firmwares/fixedwing/nav.c index a8a21a60a9..fc160002f3 100644 --- a/sw/airborne/firmwares/fixedwing/nav.c +++ b/sw/airborne/firmwares/fixedwing/nav.c @@ -457,18 +457,18 @@ static void send_nav_ref(struct transport_tx *trans, struct device *dev) { } static void send_nav(struct transport_tx *trans, struct device *dev) { - SEND_NAVIGATION(DefaultChannel, DefaultDevice); + SEND_NAVIGATION(trans, dev); } static void send_wp_moved(struct transport_tx *trans, struct device *dev) { static uint8_t i; i++; if (i >= nb_waypoint) i = 0; - DownlinkSendWp(DefaultChannel, DefaultDevice, i); + DownlinkSendWp(trans, dev, i); } bool_t DownlinkSendWpNr(uint8_t _wp) { - DownlinkSendWp(DefaultChannel, DefaultDevice, _wp); + DownlinkSendWp(&(DefaultChannel).trans_tx, &(DefaultDevice).device, _wp); return FALSE; } diff --git a/sw/airborne/firmwares/fixedwing/nav.h b/sw/airborne/firmwares/fixedwing/nav.h index 457a17784d..4cbbfa4d3c 100644 --- a/sw/airborne/firmwares/fixedwing/nav.h +++ b/sw/airborne/firmwares/fixedwing/nav.h @@ -206,7 +206,7 @@ bool_t nav_approaching_xy(float x, float y, float from_x, float from_y, float ap struct EnuCoor_f* pos = stateGetPositionEnu_f(); \ float dist_wp = sqrtf(dist2_to_wp); \ float dist_home = sqrtf(dist2_to_home); \ - DOWNLINK_SEND_NAVIGATION(_trans, _dev, &nav_block, &nav_stage, &(pos->x), &(pos->y), &dist_wp, &dist_home, &_circle_count, &nav_oval_count); \ + pprz_msg_send_NAVIGATION(_trans, _dev, AC_ID, &nav_block, &nav_stage, &(pos->x), &(pos->y), &dist_wp, &dist_home, &_circle_count, &nav_oval_count); \ } extern bool_t DownlinkSendWpNr(uint8_t _wp); @@ -214,7 +214,7 @@ extern bool_t DownlinkSendWpNr(uint8_t _wp); #define DownlinkSendWp(_trans, _dev, i) { \ float x = nav_utm_east0 + waypoints[i].x; \ float y = nav_utm_north0 + waypoints[i].y; \ - DOWNLINK_SEND_WP_MOVED(_trans, _dev, &i, &x, &y, &(waypoints[i].a),&nav_utm_zone0); \ + pprz_msg_send_WP_MOVED(_trans, _dev, AC_ID, &i, &x, &y, &(waypoints[i].a),&nav_utm_zone0); \ } #endif /* NAV_H */ diff --git a/sw/airborne/firmwares/rotorcraft/main.c b/sw/airborne/firmwares/rotorcraft/main.c index f790c34096..9d1efe8893 100644 --- a/sw/airborne/firmwares/rotorcraft/main.c +++ b/sw/airborne/firmwares/rotorcraft/main.c @@ -41,10 +41,8 @@ #include "subsystems/datalink/telemetry.h" #include "subsystems/datalink/datalink.h" +#include "subsystems/datalink/downlink.h" #include "subsystems/settings.h" -#include "subsystems/datalink/pprz_transport.h" -#include "subsystems/datalink/xbee.h" -#include "subsystems/datalink/ivy_transport.h" #include "subsystems/commands.h" #include "subsystems/actuators.h" @@ -173,15 +171,7 @@ STATIC_INLINE void main_init( void ) { mcu_int_enable(); -#if DATALINK == PPRZ - pprz_transport_init(); -#endif -#if DATALINK == XBEE - xbee_init(); -#endif -#if SITL - ivy_transport_init(); -#endif + downlink_init(); // register the timers for the periodic functions main_periodic_tid = sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL); diff --git a/sw/airborne/fms/udp_transport.h b/sw/airborne/fms/udp_transport.h index 2de1a8ef9f..1d73e771e8 100644 --- a/sw/airborne/fms/udp_transport.h +++ b/sw/airborne/fms/udp_transport.h @@ -35,10 +35,10 @@ extern uint8_t udpt_ck_a, udpt_ck_b; if (udpt_tx_buf_idx) { \ int len; \ len = network_write(network, updt_tx_buf, udpt_tx_buf_idx); \ - downlink_nb_bytes += udpt_tx_buf_idx; \ - downlink_nb_msgs++; \ + downlink.nb_bytes += udpt_tx_buf_idx; \ + downlink.nb_msgs++; \ if (len != udpt_tx_buf_idx) \ - downlink_nb_ovrn++; \ + downlink.nb_ovrn++; \ udpt_tx_buf_idx = 0; \ } \ } diff --git a/sw/airborne/mcu_periph/i2c.c b/sw/airborne/mcu_periph/i2c.c index 0dd40c0311..b6eb09f751 100644 --- a/sw/airborne/mcu_periph/i2c.c +++ b/sw/airborne/mcu_periph/i2c.c @@ -190,7 +190,7 @@ static void send_i2c3_err(struct transport_tx *trans, struct device *dev) { #endif /* USE_I2C3 */ #if PERIODIC_TELEMETRY -static void send_i2c_err(struct transport_tx *trans, struct device *dev) { +static void send_i2c_err(struct transport_tx *trans __attribute__((unused)), struct device *dev __attribute__((unused))) { static uint8_t _i2c_nb_cnt = 0; switch (_i2c_nb_cnt) { case 0: diff --git a/sw/airborne/modules/digital_cam/hackhd.h b/sw/airborne/modules/digital_cam/hackhd.h index cfe0f2415e..93ba55f71c 100644 --- a/sw/airborne/modules/digital_cam/hackhd.h +++ b/sw/airborne/modules/digital_cam/hackhd.h @@ -58,7 +58,7 @@ enum hackhd_status { struct HackHD { enum hackhd_status status; uint32_t timer; - uint32_t photo_nr; + int16_t photo_nr; uint32_t autoshoot; struct EnuCoor_f last_shot_pos; uint32_t log_delay; diff --git a/sw/airborne/subsystems/datalink/downlink.c b/sw/airborne/subsystems/datalink/downlink.c index ac628f2ce4..c4fbbe041c 100644 --- a/sw/airborne/subsystems/datalink/downlink.c +++ b/sw/airborne/subsystems/datalink/downlink.c @@ -26,8 +26,31 @@ */ -#include "std.h" +#include "subsystems/datalink/downlink.h" + +struct downlink downlink; + +void downlink_init(void) +{ + downlink.nb_ovrn = 0; + downlink.nb_bytes = 0; + downlink.nb_msgs = 0; + +#if defined DATALINK +#if DATALINK == PPRZ + pprz_transport_init(); +#endif +#if DATALINK == XBEE + xbee_init(); +#endif +#if DATALINK == W5100 + w5100_init(); +#endif +#endif + +#if SITL + ivy_transport_init(); +#endif + +} -uint8_t downlink_nb_ovrn; -uint16_t downlink_nb_bytes; -uint16_t downlink_nb_msgs; diff --git a/sw/airborne/subsystems/datalink/downlink.h b/sw/airborne/subsystems/datalink/downlink.h index 61bb1c4ab0..4837c6b28a 100644 --- a/sw/airborne/subsystems/datalink/downlink.h +++ b/sw/airborne/subsystems/datalink/downlink.h @@ -93,9 +93,16 @@ #define DefaultDevice DOWNLINK_DEVICE #endif -/** Counter of messages not sent because of unavailibity of the output buffer*/ -extern uint8_t downlink_nb_ovrn; -extern uint16_t downlink_nb_bytes; -extern uint16_t downlink_nb_msgs; +/** Downlink structure */ +struct downlink { + uint8_t nb_ovrn; ///< Counter of messages not sent because of unavailibity of the output buffer + uint16_t nb_bytes; ///< Number of bytes send over telemetry + uint16_t nb_msgs; ///< Number of messages send over telemetry +}; + +extern struct downlink downlink; + +// Init function +extern void downlink_init(void); #endif /* DOWNLINK_H */ diff --git a/sw/airborne/subsystems/datalink/ivy_transport.c b/sw/airborne/subsystems/datalink/ivy_transport.c index 2a458a7361..caddf42e27 100644 --- a/sw/airborne/subsystems/datalink/ivy_transport.c +++ b/sw/airborne/subsystems/datalink/ivy_transport.c @@ -140,12 +140,12 @@ static void end_message(struct ivy_transport *trans, struct device *dev __attrib static void overrun(struct ivy_transport *trans __attribute__((unused)), struct device *dev __attribute__((unused))) { - downlink_nb_ovrn++; + downlink.nb_ovrn++; } static void count_bytes(struct ivy_transport *trans __attribute__((unused)), struct device *dev __attribute__((unused)), uint8_t bytes) { - downlink_nb_bytes += bytes; + downlink.nb_bytes += bytes; } static int check_available_space(struct ivy_transport *trans __attribute__((unused)), struct device *dev __attribute__((unused)), uint8_t bytes __attribute__((unused))) diff --git a/sw/airborne/subsystems/datalink/pprz_transport.c b/sw/airborne/subsystems/datalink/pprz_transport.c index 81b160412b..ca75470714 100644 --- a/sw/airborne/subsystems/datalink/pprz_transport.c +++ b/sw/airborne/subsystems/datalink/pprz_transport.c @@ -77,6 +77,7 @@ static uint8_t size_of(struct pprz_transport *trans __attribute__((unused)), uin static void start_message(struct pprz_transport *trans, struct device *dev, uint8_t payload_len) { + downlink.nb_msgs++; dev->transmit(dev->periph, STX); const uint8_t msg_len = size_of(trans, payload_len); dev->transmit(dev->periph, msg_len); @@ -93,12 +94,12 @@ static void end_message(struct pprz_transport *trans, struct device *dev) static void overrun(struct pprz_transport *trans __attribute__((unused)), struct device *dev __attribute__((unused))) { - downlink_nb_ovrn++; + downlink.nb_ovrn++; } static void count_bytes(struct pprz_transport *trans __attribute__((unused)), struct device *dev __attribute__((unused)), uint8_t bytes) { - downlink_nb_bytes += bytes; + downlink.nb_bytes += bytes; } static int check_available_space(struct pprz_transport *trans __attribute__((unused)), struct device *dev, uint8_t bytes) diff --git a/sw/airborne/subsystems/datalink/pprzlog_transport.c b/sw/airborne/subsystems/datalink/pprzlog_transport.c index 3b92e94e44..4f17133100 100644 --- a/sw/airborne/subsystems/datalink/pprzlog_transport.c +++ b/sw/airborne/subsystems/datalink/pprzlog_transport.c @@ -75,6 +75,7 @@ static uint8_t size_of(struct pprzlog_transport *trans __attribute__((unused)), static void start_message(struct pprzlog_transport *trans, struct device *dev, uint8_t payload_len) { + downlink.nb_msgs++; dev->transmit(dev->periph, STX_LOG); const uint8_t msg_len = size_of(trans, payload_len); trans->ck = 0; diff --git a/sw/airborne/subsystems/datalink/xbee.c b/sw/airborne/subsystems/datalink/xbee.c index dda3eeceea..dd7bfed7c5 100644 --- a/sw/airborne/subsystems/datalink/xbee.c +++ b/sw/airborne/subsystems/datalink/xbee.c @@ -81,6 +81,7 @@ static uint8_t size_of(struct xbee_transport *trans __attribute__((unused)), uin static void start_message(struct xbee_transport *trans, struct device *dev, uint8_t payload_len) { + downlink.nb_msgs++; dev->transmit(dev->periph, XBEE_START); const uint16_t len = payload_len + XBEE_API_OVERHEAD; dev->transmit(dev->periph, (len >> 8)); @@ -99,12 +100,12 @@ static void end_message(struct xbee_transport *trans, struct device *dev) static void overrun(struct xbee_transport *trans __attribute__((unused)), struct device *dev __attribute__((unused))) { - downlink_nb_ovrn++; + downlink.nb_ovrn++; } static void count_bytes(struct xbee_transport *trans __attribute__((unused)), struct device *dev __attribute__((unused)), uint8_t bytes) { - downlink_nb_bytes += bytes; + downlink.nb_bytes += bytes; } static int check_available_space(struct xbee_transport *trans __attribute__((unused)), struct device *dev, uint8_t bytes) diff --git a/sw/simulator/nps/nps_ivy_common.c b/sw/simulator/nps/nps_ivy_common.c index 32e5b65eb8..5ef6f19d7b 100644 --- a/sw/simulator/nps/nps_ivy_common.c +++ b/sw/simulator/nps/nps_ivy_common.c @@ -1,6 +1,7 @@ #include "nps_ivy.h" #include +#include #include #include