diff --git a/conf/modules/e_identification_fr.xml b/conf/modules/e_identification_fr.xml index 2208d5172a..304d169488 100644 --- a/conf/modules/e_identification_fr.xml +++ b/conf/modules/e_identification_fr.xml @@ -6,11 +6,18 @@ + + + + + + +
- + diff --git a/sw/airborne/modules/e_identification_fr/e_identification_fr.c b/sw/airborne/modules/e_identification_fr/e_identification_fr.c index 841f092279..5452c23afa 100644 --- a/sw/airborne/modules/e_identification_fr/e_identification_fr.c +++ b/sw/airborne/modules/e_identification_fr/e_identification_fr.c @@ -38,32 +38,37 @@ static struct uart_periph *dev = &(E_ID_DEV); +bool e_identification_started = false; static int put_lat(uint8_t *buf) { int32_t lat = stateGetPositionLla_i()->lat / 1e2; + /// warning! strong dependency over GCC! + int32_t belat = __builtin_bswap32(lat); buf[0] = E_ID_LAT; - buf[1] = sizeof(lat); - memcpy(buf + 2, &lat, sizeof(lat)); - return sizeof(lat) + 2; + buf[1] = sizeof(belat); + memcpy(buf + 2, &belat, sizeof(belat)); + return sizeof(belat) + 2; } static int put_lon(uint8_t *buf) { int32_t lon = stateGetPositionLla_i()->lon / 1e2; + int32_t belon = __builtin_bswap32(lon); buf[0] = E_ID_LON; - buf[1] = sizeof(lon); - memcpy(buf + 2, &lon, sizeof(lon)); - return sizeof(lon) + 2; + buf[1] = sizeof(belon); + memcpy(buf + 2, &belon, sizeof(belon)); + return sizeof(belon) + 2; } static int put_alt(uint8_t *buf) { int16_t alt = gps.hmsl / 1e3; + int16_t bealt = __builtin_bswap16(alt); buf[0] = E_ID_HMSL; - buf[1] = sizeof(alt); - memcpy(buf + 2, &alt, sizeof(alt)); - return sizeof(alt) + 2; + buf[1] = sizeof(bealt); + memcpy(buf + 2, &bealt, sizeof(bealt)); + return sizeof(bealt) + 2; } static int put_horizontal_speed(uint8_t *buf) @@ -84,10 +89,11 @@ static int put_route(uint8_t *buf) } else { route = (uint16_t)(360 + route_normalized); } + uint16_t beroute = __builtin_bswap16(route); buf[0] = E_ID_ROUTE; - buf[1] = sizeof(route); - memcpy(buf + 2, &route, sizeof(route)); - return sizeof(route) + 2; + buf[1] = sizeof(beroute); + memcpy(buf + 2, &beroute, sizeof(beroute)); + return sizeof(beroute) + 2; } static int put_lat_lon_home(uint8_t *buf) @@ -116,17 +122,19 @@ static int put_lat_lon_home(uint8_t *buf) int32_t lat_home = ptr_home_lla_i->lat / 1e2; int32_t lon_home = ptr_home_lla_i->lon / 1e2; + int32_t belat_home = __builtin_bswap32(lat_home); + int32_t belon_home = __builtin_bswap32(lon_home); int offset = 0; buf[offset++] = E_ID_LAT_TO; - buf[offset++] = sizeof(lat_home); - memcpy(buf + offset, &lat_home, sizeof(lat_home)); - offset += sizeof(lat_home); + buf[offset++] = sizeof(belat_home); + memcpy(buf + offset, &belat_home, sizeof(belat_home)); + offset += sizeof(belat_home); buf[offset++] = E_ID_LON_TO; - buf[offset++] = sizeof(lon_home); - memcpy(buf + offset, &lon_home, sizeof(lon_home)); - offset += sizeof(lon_home); + buf[offset++] = sizeof(belon_home); + memcpy(buf + offset, &belon_home, sizeof(belon_home)); + offset += sizeof(belon_home); return offset; } @@ -138,8 +146,17 @@ void e_identification_fr_init() void e_identification_fr_periodic() { - - if (gps.fix) { +#if defined(FIXEDWING_FIRMWARE) + if (autopilot.launch) { +#elif defined(ROTORCRAFT_FIRMWARE) + if (autopilot_in_flight()) { +#else +#error "firmware unsupported!" +#endif + e_identification_started = true; + } + + if (GpsFixValid() && e_identification_started) { uint8_t buf[MAX_BUF_LEN]; buf[0] = 0X99; //PPRZ_STX buf[1] = 0; //filled later with message length diff --git a/sw/airborne/modules/e_identification_fr/e_identification_fr.h b/sw/airborne/modules/e_identification_fr/e_identification_fr.h index 55a241c681..cb23b56152 100644 --- a/sw/airborne/modules/e_identification_fr/e_identification_fr.h +++ b/sw/airborne/modules/e_identification_fr/e_identification_fr.h @@ -26,6 +26,8 @@ #ifndef E_IDENTIFICATION_FR_H #define E_IDENTIFICATION_FR_H +#include "stdbool.h" + enum e_id_type { // 0 reserved for future use E_ID_PROTOCOL_VERSION = 1, @@ -45,8 +47,7 @@ enum e_id_type { extern void e_identification_fr_init(void); extern void e_identification_fr_periodic(void); -#define E_Identification_Fr_Start() (e_identification_fr_e_identification_fr_periodic_status = MODULES_START) -#define E_Identification_Fr_Stop() (e_identification_fr_e_identification_fr_periodic_status = MODULES_STOP) +extern bool e_identification_started; #endif