diff --git a/libraries/AP_Devo_Telem/AP_Devo_Telem.cpp b/libraries/AP_Devo_Telem/AP_Devo_Telem.cpp index 94668dcbe4..bb3eb2d5fb 100644 --- a/libraries/AP_Devo_Telem/AP_Devo_Telem.cpp +++ b/libraries/AP_Devo_Telem/AP_Devo_Telem.cpp @@ -33,13 +33,6 @@ extern const AP_HAL::HAL& hal; - -//constructor -AP_DEVO_Telem::AP_DEVO_Telem() -{ - devoPacket.header = DEVOM_SYNC_BYTE; -} - void AP_DEVO_Telem::init() { const AP_SerialManager& serial_manager = AP::serialmanager(); @@ -84,6 +77,19 @@ void AP_DEVO_Telem::send_frames() return; } + struct PACKED { + uint8_t header; ///< 0xAA for a valid packet + int32_t lon; + int32_t lat; + int32_t alt; + int16_t speed; + int16_t temp; + int16_t volt; + uint8_t checksum8; + } devoPacket{}; + + devoPacket.header = DEVOM_SYNC_BYTE; + const AP_AHRS &_ahrs = AP::ahrs(); const AP_GPS &gps = AP::gps(); Location loc; @@ -100,11 +106,6 @@ void AP_DEVO_Telem::send_frames() float alt; _ahrs.get_relative_position_D_home(alt); devoPacket.alt = alt * -100.0f; // coordinates was in NED, so it needs to change sign. Protocol requires in cm! - } else { - devoPacket.lat = 0; - devoPacket.lon = 0; - devoPacket.speed = 0; - devoPacket.alt=0; } @@ -112,8 +113,9 @@ void AP_DEVO_Telem::send_frames() devoPacket.volt = roundf(AP::battery().voltage() * 10.0f); devoPacket.temp = gcs().custom_mode(); // Send mode as temperature - devoPacket.checksum8 = 0; // Init Checksum with zero Byte - + // emit the packet to the port byte-by-byte, calculating checksum + // as we go. Note we are stepping backwards through the structure + // - presumably to get endianness correct on the entries! uint8_t *b = (uint8_t *)&devoPacket; for (uint8_t i = sizeof(devoPacket)-1; i !=0; i--) { _port->write(b, 1); diff --git a/libraries/AP_Devo_Telem/AP_Devo_Telem.h b/libraries/AP_Devo_Telem/AP_Devo_Telem.h index 1e34c7c39e..1dfdc6e7f0 100644 --- a/libraries/AP_Devo_Telem/AP_Devo_Telem.h +++ b/libraries/AP_Devo_Telem/AP_Devo_Telem.h @@ -21,7 +21,7 @@ class AP_DEVO_Telem { public: //constructor - AP_DEVO_Telem(); + AP_DEVO_Telem() {} /* Do not allow copies */ AP_DEVO_Telem(const AP_DEVO_Telem &other) = delete; @@ -30,16 +30,6 @@ public: void init(); private: - typedef struct PACKED { - uint8_t header; ///< 0xAA for a valid packet - int32_t lon; - int32_t lat; - int32_t alt; - int16_t speed; - int16_t temp; - int16_t volt; - uint8_t checksum8; - } DevoMPacket; uint32_t gpsDdToDmsFormat(float ddm); @@ -49,8 +39,6 @@ private: // send_frames - sends updates down telemetry link void send_frames(); - DevoMPacket devoPacket; - AP_HAL::UARTDriver *_port; // UART used to send data to receiver uint32_t _last_frame_ms;