forked from Archive/PX4-Autopilot
add sPort telemetry handling into frsky_telemetry daemon
This commit is contained in:
Mark Whitehorn
Lorenz Meier
parent
8f8b4485f1
commit
c81c34b147
|
|
@ -0,0 +1,217 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
|
||||
* Author: Stefan Rado <px4@sradonia.net>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file sPort_data.c
|
||||
* @author Stefan Rado <px4@sradonia.net>
|
||||
* @author Mark Whitehorn <kd0aij@github.com>
|
||||
*
|
||||
* FrSky SmartPort telemetry implementation.
|
||||
*
|
||||
*/
|
||||
|
||||
#include "sPort_data.h"
|
||||
|
||||
#include <stdlib.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <arch/math.h>
|
||||
#include <geo/geo.h>
|
||||
|
||||
#include <uORB/topics/battery_status.h>
|
||||
#include <uORB/topics/sensor_combined.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
#define frac(f) (f - (int)f)
|
||||
|
||||
static int battery_sub = -1;
|
||||
static int sensor_sub = -1;
|
||||
static int global_position_sub = -1;
|
||||
static int vehicle_status_sub = -1;
|
||||
|
||||
/**
|
||||
* Initializes the uORB subscriptions.
|
||||
*/
|
||||
void sPort_init()
|
||||
{
|
||||
battery_sub = orb_subscribe(ORB_ID(battery_status));
|
||||
global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position));
|
||||
sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
|
||||
vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
|
||||
}
|
||||
|
||||
/**
|
||||
* Sends a 0x10 start byte.
|
||||
*/
|
||||
//static void sPort_send_start(int uart)
|
||||
//{
|
||||
// static const uint8_t c = 0x10;
|
||||
// write(uart, &c, 1);
|
||||
//}
|
||||
|
||||
static void update_crc(uint16_t *crc, unsigned char b)
|
||||
{
|
||||
*crc += b;
|
||||
*crc += *crc >> 8;
|
||||
*crc &= 0xFF;
|
||||
}
|
||||
|
||||
/**
|
||||
* Sends one byte, performing byte-stuffing if necessary.
|
||||
*/
|
||||
static void sPort_send_byte(int uart, uint8_t value)
|
||||
{
|
||||
const uint8_t x7E[] = { 0x7D, 0x5E };
|
||||
const uint8_t x7D[] = { 0x7D, 0x5D };
|
||||
|
||||
switch (value) {
|
||||
case 0x7E:
|
||||
write(uart, x7E, sizeof(x7E));
|
||||
break;
|
||||
|
||||
case 0x7D:
|
||||
write(uart, x7D, sizeof(x7D));
|
||||
break;
|
||||
|
||||
default:
|
||||
write(uart, &value, sizeof(value));
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Sends one data id/value pair.
|
||||
*/
|
||||
void sPort_send_data(int uart, uint16_t id, uint32_t data)
|
||||
{
|
||||
union {
|
||||
uint32_t word;
|
||||
uint8_t byte[4];
|
||||
} buf;
|
||||
|
||||
/* send start byte */
|
||||
static const uint8_t c = 0x10;
|
||||
write(uart, &c, 1);
|
||||
|
||||
/* init crc */
|
||||
uint16_t crc = c;
|
||||
|
||||
buf.word = id;
|
||||
|
||||
for (int i = 0; i < 2; i++) {
|
||||
update_crc(&crc, buf.byte[i]);
|
||||
sPort_send_byte(uart, buf.byte[i]); /* LSB first */
|
||||
}
|
||||
|
||||
buf.word = data;
|
||||
|
||||
for (int i = 0; i < 4; i++) {
|
||||
update_crc(&crc, buf.byte[i]);
|
||||
sPort_send_byte(uart, buf.byte[i]); /* LSB first */
|
||||
}
|
||||
|
||||
sPort_send_byte(uart, 0xFF - crc);
|
||||
}
|
||||
|
||||
|
||||
// scaling correct with OpenTX 2.1.7
|
||||
void sPort_send_BATV(int uart)
|
||||
{
|
||||
/* get a local copy of the vehicle status data */
|
||||
struct vehicle_status_s vehicle_status;
|
||||
memset(&vehicle_status, 0, sizeof(vehicle_status));
|
||||
orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status);
|
||||
|
||||
/* send battery voltage as VFAS */
|
||||
uint32_t voltage = (int)(100 * vehicle_status.battery_voltage);
|
||||
sPort_send_data(uart, SMARTPORT_ID_VFAS, voltage);
|
||||
}
|
||||
|
||||
// verified scaling
|
||||
void sPort_send_CUR(int uart)
|
||||
{
|
||||
/* get a local copy of the vehicle status data */
|
||||
struct vehicle_status_s vehicle_status;
|
||||
memset(&vehicle_status, 0, sizeof(vehicle_status));
|
||||
orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status);
|
||||
|
||||
/* send data */
|
||||
uint32_t current = (int)(10 * vehicle_status.battery_current);
|
||||
sPort_send_data(uart, SMARTPORT_ID_CURR, current);
|
||||
}
|
||||
|
||||
// verified scaling for "custom" altitude option
|
||||
// OpenTX uses the initial reading as field elevation and displays
|
||||
// the difference (altitude - field)
|
||||
void sPort_send_ALT(int uart)
|
||||
{
|
||||
/* get a local copy of the current sensor values */
|
||||
struct sensor_combined_s raw;
|
||||
memset(&raw, 0, sizeof(raw));
|
||||
orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw);
|
||||
|
||||
/* send data */
|
||||
uint32_t alt = (int)(100 * raw.baro_alt_meter[0]);
|
||||
sPort_send_data(uart, SMARTPORT_ID_ALT, alt);
|
||||
}
|
||||
|
||||
// verified scaling for "calculated" option
|
||||
void sPort_send_SPD(int uart)
|
||||
{
|
||||
/* get a local copy of the global position data */
|
||||
struct vehicle_global_position_s global_pos;
|
||||
memset(&global_pos, 0, sizeof(global_pos));
|
||||
orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos);
|
||||
|
||||
/* send data for A2 */
|
||||
float speed = sqrtf(global_pos.vel_n * global_pos.vel_n + global_pos.vel_e * global_pos.vel_e);
|
||||
uint32_t ispeed = (int)(10 * speed);
|
||||
sPort_send_data(uart, SMARTPORT_ID_GPS_SPD, ispeed);
|
||||
}
|
||||
|
||||
// verified scaling
|
||||
void sPort_send_FUEL(int uart)
|
||||
{
|
||||
/* get a local copy of the vehicle status data */
|
||||
struct vehicle_status_s vehicle_status;
|
||||
memset(&vehicle_status, 0, sizeof(vehicle_status));
|
||||
orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status);
|
||||
|
||||
/* send data */
|
||||
uint32_t fuel = (int)(100 * vehicle_status.battery_remaining);
|
||||
sPort_send_data(uart, SMARTPORT_ID_FUEL, fuel);
|
||||
}
|
||||
|
|
@ -0,0 +1,87 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
|
||||
* Author: Stefan Rado <px4@sradonia.net>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file sPort_data.h
|
||||
* @author Mark Whitehorn <kd0aij@github.com>
|
||||
*
|
||||
* FrSky SmartPort telemetry implementation.
|
||||
*
|
||||
*/
|
||||
#ifndef _SPORT_DATA_H
|
||||
#define _SPORT_DATA_H
|
||||
|
||||
#include <sys/types.h>
|
||||
|
||||
/* FrSky SmartPort polling IDs captured from X4R */
|
||||
#define SMARTPORT_POLL_1 0x1B
|
||||
#define SMARTPORT_POLL_2 0x34
|
||||
#define SMARTPORT_POLL_3 0x95
|
||||
#define SMARTPORT_POLL_4 0x16
|
||||
#define SMARTPORT_POLL_5 0xB7
|
||||
|
||||
/* FrSky SmartPort sensor IDs */
|
||||
#define SMARTPORT_ID_RSSI 0xf101
|
||||
#define SMARTPORT_ID_RXA1 0xf102 // supplied by RX
|
||||
#define SMARTPORT_ID_RXA2 0xf103 // supplied by RX
|
||||
#define SMARTPORT_ID_BATV 0xf104
|
||||
#define SMARTPORT_ID_SWR 0xf105
|
||||
#define SMARTPORT_ID_T1 0x0400
|
||||
#define SMARTPORT_ID_T2 0x0410
|
||||
#define SMARTPORT_ID_RPM 0x0500
|
||||
#define SMARTPORT_ID_FUEL 0x0600
|
||||
#define SMARTPORT_ID_ALT 0x0100
|
||||
#define SMARTPORT_ID_VARIO 0x0110
|
||||
#define SMARTPORT_ID_ACCX 0x0700
|
||||
#define SMARTPORT_ID_ACCY 0x0710
|
||||
#define SMARTPORT_ID_ACCZ 0x0720
|
||||
#define SMARTPORT_ID_CURR 0x0200
|
||||
#define SMARTPORT_ID_VFAS 0x0210
|
||||
#define SMARTPORT_ID_CELLS 0x0300
|
||||
#define SMARTPORT_ID_GPS_LON_LAT 0x0800
|
||||
#define SMARTPORT_ID_GPS_ALT 0x0820
|
||||
#define SMARTPORT_ID_GPS_SPD 0x0830
|
||||
#define SMARTPORT_ID_GPS_CRS 0x0840
|
||||
#define SMARTPORT_ID_GPS_TIME 0x0850
|
||||
|
||||
// Public functions
|
||||
void sPort_init(void);
|
||||
void sPort_send_data(int uart, uint16_t id, uint32_t data);
|
||||
void sPort_send_BATV(int uart);
|
||||
void sPort_send_CUR(int uart);
|
||||
void sPort_send_ALT(int uart);
|
||||
void sPort_send_SPD(int uart);
|
||||
void sPort_send_FUEL(int uart);
|
||||
|
||||
#endif /* _SPORT_TELEMETRY_H */
|
||||
Loading…
Reference in New Issue