diff --git a/libraries/AP_Devo_Telem/AP_Devo_Telem.cpp b/libraries/AP_Devo_Telem/AP_Devo_Telem.cpp new file mode 100644 index 0000000000..403e2181eb --- /dev/null +++ b/libraries/AP_Devo_Telem/AP_Devo_Telem.cpp @@ -0,0 +1,136 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . +*/ + +/* + DEVO Telemetry library +*/ + +#define DEVOM_SYNC_BYTE 0xAA + + +#define AP_SERIALMANAGER_DEVO_TELEM_BAUD 38400 +#define AP_SERIALMANAGER_DEVO_BUFSIZE_RX 0 +#define AP_SERIALMANAGER_DEVO_BUFSIZE_TX 32 + +#include "AP_Devo_Telem.h" +#include +#include + +extern const AP_HAL::HAL& hal; + + +//constructor +AP_DEVO_Telem::AP_DEVO_Telem(const AP_AHRS &ahrs) : + _ahrs(ahrs) +{ + devoPacket.header = DEVOM_SYNC_BYTE; +} + +// init - perform require initialisation including detecting which protocol to use +void AP_DEVO_Telem::init(const AP_SerialManager& serial_manager) +{ + // check for DEVO_DPort + if ((_port = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Devo_Telem, 0))) { + _port->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE); + // initialise uart + _port->begin(AP_SERIALMANAGER_DEVO_TELEM_BAUD, AP_SERIALMANAGER_DEVO_BUFSIZE_RX, AP_SERIALMANAGER_DEVO_BUFSIZE_TX); + + hal.scheduler->register_io_process(FUNCTOR_BIND_MEMBER(&AP_DEVO_Telem::tick, void)); + } +} + + +uint32_t AP_DEVO_Telem::gpsDdToDmsFormat(float ddm) +{ + int32_t deg = (int32_t)ddm; + float mm = (ddm - deg) * 60.0f; + + mm = ((float)deg * 100.0f + mm) /100.0; + + if ((mm < -180.0f) || (mm > 180.0f)) { + mm = 0.0f; + } + + return mm * 1.0e7; +} + + +/* + send_frames - sends updates down telemetry link + should be called by main program at 1hz +*/ + +#define DEVO_SPEED_FACTOR 0.0194384f + +void AP_DEVO_Telem::send_frames(uint8_t control_mode) +{ + // return immediately if not initialised + if (_port == nullptr) { + return; + } + + const AP_GPS &gps = AP::gps(); + + if (gps.status() >= (enum AP_GPS::GPS_Status)GPS_FIX_TYPE_3D_FIX) { +#if 0 + // GPS version, tested, working + Location loc = gps.location(); +#else + // AHRS version, untested + Location loc; + _ahrs.get_position(loc); +#endif + devoPacket.lat = gpsDdToDmsFormat(loc.lat); + devoPacket.lon = gpsDdToDmsFormat(loc.lng); + devoPacket.speed = (int16_t)(gps.ground_speed() * DEVO_SPEED_FACTOR * 100.0f); // * 100 for cm + + /* + Note that this isn't actually barometric altitude, it is the + inertial nav estimate of altitude above home. + */ + 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; + } + + + + devoPacket.volt = roundf(AP::battery().voltage() * 10.0f); + devoPacket.temp = control_mode; // Send mode as temperature + + devoPacket.checksum8 = 0; // Init Checksum with zero Byte + + uint8_t *b = (uint8_t *)&devoPacket; + for (uint8_t i = sizeof(devoPacket)-1; i !=0; i--) { + _port->write(b, 1); + devoPacket.checksum8 += *b++; // Add Checksum + } + _port->write(&devoPacket.checksum8, 1); +} + +void AP_DEVO_Telem::tick(void) +{ + uint32_t now = AP_HAL::millis(); + + if (now - _last_frame_ms > 1000) { + _last_frame_ms = now; + send_frames(_control_mode); + } +} diff --git a/libraries/AP_Devo_Telem/AP_Devo_Telem.h b/libraries/AP_Devo_Telem/AP_Devo_Telem.h new file mode 100644 index 0000000000..437d339fd8 --- /dev/null +++ b/libraries/AP_Devo_Telem/AP_Devo_Telem.h @@ -0,0 +1,69 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . +*/ + +#pragma once + +#include +#include +#include + +class AP_DEVO_Telem { +public: + //constructor + AP_DEVO_Telem(const AP_AHRS &ahrs); + + /* Do not allow copies */ + AP_DEVO_Telem(const AP_DEVO_Telem &other) = delete; + AP_DEVO_Telem &operator=(const AP_DEVO_Telem&) = delete; + + // init - perform require initialisation including detecting which protocol to use + void init(const AP_SerialManager& serial_manager); + + // update flight control mode. The control mode is vehicle type specific + void update_control_mode(uint8_t mode) + { + _control_mode = mode; + } + + +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); + + // tick - main call to send updates to transmitter (called by scheduler at 1kHz) + void tick(void); + + // send_frames - sends updates down telemetry link + void send_frames(uint8_t control_mode); + + DevoMPacket devoPacket; + + uint8_t _control_mode; + + const AP_AHRS &_ahrs; // reference to attitude estimate + AP_HAL::UARTDriver *_port; // UART used to send data to receiver + uint32_t _last_frame_ms; + +};