Files
PX4-Autopilot/src/drivers/frsky_telemetry/frsky_data.c
T
Beat Küng 9c73eae941 sensor_combined: replace accel & gyro integral with value, use float for dt
Reason: the value is easier to read & handle (for example plotting). In
most places the value is needed, not the integral.

Note that this breaks the replay format for sdlog2 replay
2016-07-07 11:35:50 +02:00

373 lines
10 KiB
C

/****************************************************************************
*
* 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 frsky_data.c
* @author Stefan Rado <px4@sradonia.net>
*
* FrSky telemetry implementation.
*
*/
#include "frsky_data.h"
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <stdbool.h>
#include <arch/math.h>
#include <geo/geo.h>
#include <stdbool.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_global_position.h>
#include <drivers/drv_hrt.h>
/* FrSky sensor hub data IDs */
#define FRSKY_ID_GPS_ALT_BP 0x01
#define FRSKY_ID_TEMP1 0x02
#define FRSKY_ID_RPM 0x03
#define FRSKY_ID_FUEL 0x04
#define FRSKY_ID_TEMP2 0x05
#define FRSKY_ID_VOLTS 0x06
#define FRSKY_ID_GPS_ALT_AP 0x09
#define FRSKY_ID_BARO_ALT_BP 0x10
#define FRSKY_ID_GPS_SPEED_BP 0x11
#define FRSKY_ID_GPS_LONG_BP 0x12
#define FRSKY_ID_GPS_LAT_BP 0x13
#define FRSKY_ID_GPS_COURS_BP 0x14
#define FRSKY_ID_GPS_DAY_MONTH 0x15
#define FRSKY_ID_GPS_YEAR 0x16
#define FRSKY_ID_GPS_HOUR_MIN 0x17
#define FRSKY_ID_GPS_SEC 0x18
#define FRSKY_ID_GPS_SPEED_AP 0x19
#define FRSKY_ID_GPS_LONG_AP 0x1A
#define FRSKY_ID_GPS_LAT_AP 0x1B
#define FRSKY_ID_GPS_COURS_AP 0x1C
#define FRSKY_ID_BARO_ALT_AP 0x21
#define FRSKY_ID_GPS_LONG_EW 0x22
#define FRSKY_ID_GPS_LAT_NS 0x23
#define FRSKY_ID_ACCEL_X 0x24
#define FRSKY_ID_ACCEL_Y 0x25
#define FRSKY_ID_ACCEL_Z 0x26
#define FRSKY_ID_CURRENT 0x28
#define FRSKY_ID_VARIO 0x30
#define FRSKY_ID_VFAS 0x39
#define FRSKY_ID_VOLTS_BP 0x3A
#define FRSKY_ID_VOLTS_AP 0x3B
#define frac(f) (f - (int)f)
static struct battery_status_s *battery_status;
static struct vehicle_global_position_s *global_pos;
static struct sensor_combined_s *sensor_combined;
static int battery_status_sub = -1;
static int vehicle_global_position_sub = -1;
static int sensor_sub = -1;
/**
* Initializes the uORB subscriptions.
*/
bool frsky_init()
{
battery_status = malloc(sizeof(struct battery_status_s));
global_pos = malloc(sizeof(struct vehicle_global_position_s));
sensor_combined = malloc(sizeof(struct sensor_combined_s));
if (battery_status == NULL || global_pos == NULL || sensor_combined == NULL) {
return false;
}
battery_status_sub = orb_subscribe(ORB_ID(battery_status));
vehicle_global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position));
sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
return true;
}
void frsky_deinit()
{
free(battery_status);
free(global_pos);
free(sensor_combined);
}
/**
* Sends a 0x5E start/stop byte.
*/
static void frsky_send_startstop(int uart)
{
static const uint8_t c = 0x5E;
write(uart, &c, sizeof(c));
}
/**
* Sends one byte, performing byte-stuffing if necessary.
*/
static void frsky_send_byte(int uart, uint8_t value)
{
const uint8_t x5E[] = { 0x5D, 0x3E };
const uint8_t x5D[] = { 0x5D, 0x3D };
switch (value) {
case 0x5E:
write(uart, x5E, sizeof(x5E));
break;
case 0x5D:
write(uart, x5D, sizeof(x5D));
break;
default:
write(uart, &value, sizeof(value));
break;
}
}
/**
* Sends one data id/value pair.
*/
static void frsky_send_data(int uart, uint8_t id, int16_t data)
{
/* Cast data to unsigned, because signed shift might behave incorrectly */
uint16_t udata = data;
frsky_send_startstop(uart);
frsky_send_byte(uart, id);
frsky_send_byte(uart, udata); /* LSB */
frsky_send_byte(uart, udata >> 8); /* MSB */
}
void frsky_update_topics()
{
bool updated;
/* get a local copy of the current sensor values */
orb_check(sensor_sub, &updated);
if (updated) {
orb_copy(ORB_ID(sensor_combined), sensor_sub, sensor_combined);
}
/* get a local copy of the battery data */
orb_check(battery_status_sub, &updated);
if (updated) {
orb_copy(ORB_ID(battery_status), battery_status_sub, battery_status);
}
/* get a local copy of the global position data */
orb_check(vehicle_global_position_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, global_pos);
}
}
/**
* Sends frame 1 (every 200ms):
* acceleration values, barometer altitude, temperature, battery voltage & current
*/
void frsky_send_frame1(int uart)
{
/* send formatted frame */
frsky_send_data(uart, FRSKY_ID_ACCEL_X, roundf(sensor_combined->accelerometer_m_s2[0] * 1000.0f));
frsky_send_data(uart, FRSKY_ID_ACCEL_Y, roundf(sensor_combined->accelerometer_m_s2[1] * 1000.0f));
frsky_send_data(uart, FRSKY_ID_ACCEL_Z, roundf(sensor_combined->accelerometer_m_s2[2] * 1000.0f));
frsky_send_data(uart, FRSKY_ID_BARO_ALT_BP,
sensor_combined->baro_alt_meter);
frsky_send_data(uart, FRSKY_ID_BARO_ALT_AP,
roundf(frac(sensor_combined->baro_alt_meter) * 100.0f));
frsky_send_data(uart, FRSKY_ID_TEMP1,
roundf(sensor_combined->baro_temp_celcius));
frsky_send_data(uart, FRSKY_ID_VFAS,
roundf(battery_status->voltage_v * 10.0f));
frsky_send_data(uart, FRSKY_ID_CURRENT,
(battery_status->current_a < 0) ? 0 : roundf(battery_status->current_a * 10.0f));
frsky_send_startstop(uart);
}
/**
* Formats the decimal latitude/longitude to the required degrees/minutes.
*/
static float frsky_format_gps(float dec)
{
float dm_deg = (int) dec;
return (dm_deg * 100.0f) + (dec - dm_deg) * 60;
}
/**
* Sends frame 2 (every 1000ms):
* GPS course, latitude, longitude, ground speed, GPS altitude, remaining battery level
*/
void frsky_send_frame2(int uart)
{
/* send formatted frame */
float course = 0, lat = 0, lon = 0, speed = 0, alt = 0;
char lat_ns = 0, lon_ew = 0;
int sec = 0;
if (global_pos->timestamp != 0 && hrt_absolute_time() < global_pos->timestamp + 20000) {
time_t time_gps = global_pos->time_utc_usec / 1000000ULL;
struct tm *tm_gps = gmtime(&time_gps);
course = (global_pos->yaw + M_PI_F) / M_PI_F * 180.0f;
lat = frsky_format_gps(fabsf(global_pos->lat));
lat_ns = (global_pos->lat < 0) ? 'S' : 'N';
lon = frsky_format_gps(fabsf(global_pos->lon));
lon_ew = (global_pos->lon < 0) ? 'W' : 'E';
speed = sqrtf(global_pos->vel_n * global_pos->vel_n + global_pos->vel_e * global_pos->vel_e)
* 25.0f / 46.0f;
alt = global_pos->alt;
sec = tm_gps->tm_sec;
}
frsky_send_data(uart, FRSKY_ID_GPS_COURS_BP, course);
frsky_send_data(uart, FRSKY_ID_GPS_COURS_AP, frac(course) * 1000.0f);
frsky_send_data(uart, FRSKY_ID_GPS_LAT_BP, lat);
frsky_send_data(uart, FRSKY_ID_GPS_LAT_AP, frac(lat) * 10000.0f);
frsky_send_data(uart, FRSKY_ID_GPS_LAT_NS, lat_ns);
frsky_send_data(uart, FRSKY_ID_GPS_LONG_BP, lon);
frsky_send_data(uart, FRSKY_ID_GPS_LONG_AP, frac(lon) * 10000.0f);
frsky_send_data(uart, FRSKY_ID_GPS_LONG_EW, lon_ew);
frsky_send_data(uart, FRSKY_ID_GPS_SPEED_BP, speed);
frsky_send_data(uart, FRSKY_ID_GPS_SPEED_AP, frac(speed) * 100.0f);
frsky_send_data(uart, FRSKY_ID_GPS_ALT_BP, alt);
frsky_send_data(uart, FRSKY_ID_GPS_ALT_AP, frac(alt) * 100.0f);
frsky_send_data(uart, FRSKY_ID_FUEL,
roundf(battery_status->remaining * 100.0f));
frsky_send_data(uart, FRSKY_ID_GPS_SEC, sec);
frsky_send_startstop(uart);
}
/**
* Sends frame 3 (every 5000ms):
* GPS date & time
*/
void frsky_send_frame3(int uart)
{
/* send formatted frame */
time_t time_gps = global_pos->time_utc_usec / 1000000ULL;
struct tm *tm_gps = gmtime(&time_gps);
uint16_t hour_min = (tm_gps->tm_min << 8) | (tm_gps->tm_hour & 0xff);
frsky_send_data(uart, FRSKY_ID_GPS_DAY_MONTH, tm_gps->tm_mday);
frsky_send_data(uart, FRSKY_ID_GPS_YEAR, tm_gps->tm_year);
frsky_send_data(uart, FRSKY_ID_GPS_HOUR_MIN, hour_min);
frsky_send_data(uart, FRSKY_ID_GPS_SEC, tm_gps->tm_sec);
frsky_send_startstop(uart);
}
/* parse 11 byte frames */
bool frsky_parse_host(uint8_t *sbuf, int nbytes, struct adc_linkquality *v)
{
bool data_ready = false;
static int dcount = 0;
static uint8_t type = 0;
static uint8_t data[11];
static enum {
HEADER = 0,
TYPE,
DATA,
TRAILER
} state = HEADER;
for (int i = 0; i < nbytes; i++) {
switch (state) {
case HEADER:
if (sbuf[i] == 0x7E) {
state = TYPE;
}
break;
case TYPE:
if (sbuf[i] != 0x7E) {
state = DATA;
type = sbuf[i];
dcount = 0;
}
break;
case DATA:
/* read 8 data bytes */
if (dcount < 7) {
data[dcount++] = sbuf[i];
} else {
/* received all data bytes */
state = TRAILER;
}
break;
case TRAILER:
state = HEADER;
if (sbuf[i] != 0x7E) {
// warnx("host packet error: %x", sbuf[i]);
} else {
data_ready = true;
if (type == 0xFE) {
/* this is an adc_linkquality packet */
v->ad1 = data[0];
v->ad2 = data[1];
v->linkq = data[2];
}
}
break;
}
}
return data_ready;
}