Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Devo_Telem: devo telemetry support (RX705/707)

Browse Source
This commit is contained in:
night-ghost 2018-03-04 14:41:06 +05:00 committed by Andrew Tridgell
parent c8488e3333
commit 85c71273cf
2 changed files with 205 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

136
libraries/AP_Devo_Telem/AP_Devo_Telem.cpp Normal file
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
/*
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 <AP_GPS/AP_GPS.h>
#include <AP_BattMonitor/AP_BattMonitor.h>
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);
}
}

69
libraries/AP_Devo_Telem/AP_Devo_Telem.h Normal file
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
#pragma once
#include <AP_HAL/AP_HAL.h>
#include <AP_AHRS/AP_AHRS.h>
#include <AP_SerialManager/AP_SerialManager.h>
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;
};