Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

FrSky telemetry updates #7708

Merged
merged 6 commits into from
Aug 8, 2017
Merged

FrSky telemetry updates #7708

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
1a096de
frsky_telemetry: fix heading for D protocol
bkueng Jul 31, 2017
e1a0725
frsky_telemetry: use new module documentation
bkueng Jul 31, 2017
3bb6da5
frsky_telemetry S.Port: refactor to use less memory & allocations
bkueng Jul 31, 2017
8234e8f
frsky_telemetry D protocol: refactor to use less memory & allocations
bkueng Jul 31, 2017
79067dc
frsky_telemetry: send flight mode & gps info
bkueng Jul 31, 2017
14123d7
frsky_telemetry: cleanup static vars, use less memory & fix process p…
bkueng Jul 31, 2017
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
81 changes: 81 additions & 0 deletions src/drivers/frsky_telemetry/common.h
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,81 @@
/****************************************************************************
*
* Copyright (c) 2017 PX4 Development Team. All rights reserved.
*
* 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 common.h
*
* common declarations
*
*/
#pragma once

#include <stdint.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


/**
* Map the PX4 flight mode (vehicle_status_s::nav_state) to the telemetry flight mode
*/
uint16_t get_telemetry_flight_mode(int px4_flight_mode);
145 changes: 78 additions & 67 deletions src/drivers/frsky_telemetry/frsky_data.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -41,6 +41,7 @@
*/

#include "frsky_data.h"
#include "common.h"

#include <stdlib.h>
#include <stdio.h>
Expand All @@ -53,76 +54,59 @@
#include <uORB/topics/battery_status.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_status.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;
struct frsky_subscription_data_s {
struct battery_status_s battery_status;
struct vehicle_global_position_s global_pos;
struct sensor_combined_s sensor_combined;
struct vehicle_gps_position_s vehicle_gps_position;
uint8_t current_flight_mode; // == vehicle_status.nav_state

int battery_status_sub;
int vehicle_global_position_sub;
int sensor_sub;
int vehicle_gps_position_sub;
int vehicle_status_sub;
};

static int battery_status_sub = -1;
static int vehicle_global_position_sub = -1;
static int sensor_sub = -1;
static struct frsky_subscription_data_s *subscription_data = NULL;

/**
* 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));
subscription_data = (struct frsky_subscription_data_s *)calloc(1, sizeof(struct frsky_subscription_data_s));

if (battery_status == NULL || global_pos == NULL || sensor_combined == NULL) {
if (!subscription_data) {
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));
subscription_data->battery_status_sub = orb_subscribe(ORB_ID(battery_status));
subscription_data->vehicle_global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position));
subscription_data->sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
subscription_data->vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
subscription_data->vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
return true;
}

void frsky_deinit()
{
free(battery_status);
free(global_pos);
free(sensor_combined);
if (subscription_data) {
orb_unsubscribe(subscription_data->battery_status_sub);
orb_unsubscribe(subscription_data->vehicle_global_position_sub);
orb_unsubscribe(subscription_data->sensor_sub);
orb_unsubscribe(subscription_data->vehicle_gps_position_sub);
orb_unsubscribe(subscription_data->vehicle_status_sub);
free(subscription_data);
subscription_data = NULL;
}
}

/**
Expand Down Expand Up @@ -174,26 +158,41 @@ static void frsky_send_data(int uart, uint8_t id, int16_t data)

void frsky_update_topics()
{
struct frsky_subscription_data_s *subs = subscription_data;
bool updated;
/* get a local copy of the current sensor values */
orb_check(sensor_sub, &updated);
orb_check(subs->sensor_sub, &updated);

if (updated) {
orb_copy(ORB_ID(sensor_combined), subs->sensor_sub, &subs->sensor_combined);
}

orb_check(subs->vehicle_gps_position_sub, &updated);

if (updated) {
orb_copy(ORB_ID(vehicle_gps_position), subs->vehicle_gps_position_sub, &subs->vehicle_gps_position);
}

orb_check(subs->vehicle_status_sub, &updated);

if (updated) {
orb_copy(ORB_ID(sensor_combined), sensor_sub, sensor_combined);
struct vehicle_status_s vehicle_status;
orb_copy(ORB_ID(vehicle_status), subs->vehicle_status_sub, &vehicle_status);
subs->current_flight_mode = vehicle_status.nav_state;
}

/* get a local copy of the battery data */
orb_check(battery_status_sub, &updated);
orb_check(subs->battery_status_sub, &updated);

if (updated) {
orb_copy(ORB_ID(battery_status), battery_status_sub, battery_status);
orb_copy(ORB_ID(battery_status), subs->battery_status_sub, &subs->battery_status);
}

/* get a local copy of the global position data */
orb_check(vehicle_global_position_sub, &updated);
orb_check(subs->vehicle_global_position_sub, &updated);

if (updated) {
orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, global_pos);
orb_copy(ORB_ID(vehicle_global_position), subs->vehicle_global_position_sub, &subs->global_pos);
}
}

Expand All @@ -203,23 +202,28 @@ void frsky_update_topics()
*/
void frsky_send_frame1(int uart)
{
struct frsky_subscription_data_s *subs = subscription_data;

/* 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_ACCEL_X, roundf(subs->sensor_combined.accelerometer_m_s2[0] * 1000.0f));
frsky_send_data(uart, FRSKY_ID_ACCEL_Y, roundf(subs->sensor_combined.accelerometer_m_s2[1] * 1000.0f));
frsky_send_data(uart, FRSKY_ID_ACCEL_Z, roundf(subs->sensor_combined.accelerometer_m_s2[2] * 1000.0f));

frsky_send_data(uart, FRSKY_ID_BARO_ALT_BP,
sensor_combined->baro_alt_meter);
subs->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));
roundf(frac(subs->sensor_combined.baro_alt_meter) * 100.0f));

frsky_send_data(uart, FRSKY_ID_VFAS,
roundf(battery_status->voltage_v * 10.0f));
roundf(subs->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));
(subs->battery_status.current_a < 0) ? 0 : roundf(subs->battery_status.current_a * 10.0f));

int16_t telem_flight_mode = get_telemetry_flight_mode(subs->current_flight_mode);
frsky_send_data(uart, FRSKY_ID_TEMP1, telem_flight_mode); // send flight mode as TEMP1. This matches with OpenTX & APM

frsky_send_data(uart, FRSKY_ID_TEMP2, subs->vehicle_gps_position.satellites_used * 10 +
subs->vehicle_gps_position.fix_type);

frsky_send_startstop(uart);
}
Expand All @@ -239,6 +243,8 @@ static float frsky_format_gps(float dec)
*/
void frsky_send_frame2(int uart)
{
struct vehicle_global_position_s *global_pos = &subscription_data->global_pos;
struct battery_status_s *battery_status = &subscription_data->battery_status;
/* send formatted frame */
float course = 0, lat = 0, lon = 0, speed = 0, alt = 0;
char lat_ns = 0, lon_ew = 0;
Expand All @@ -248,7 +254,12 @@ void frsky_send_frame2(int uart)
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;
course = global_pos->yaw / M_PI_F * 180.0f;

if (course < 0.f) { // course is in range [0, 360], 0=north, CW
course += 360.f;
}

lat = frsky_format_gps(fabsf(global_pos->lat));
lat_ns = (global_pos->lat < 0) ? 'S' : 'N';
lon = frsky_format_gps(fabsf(global_pos->lon));
Expand Down Expand Up @@ -291,7 +302,7 @@ void frsky_send_frame2(int uart)
void frsky_send_frame3(int uart)
{
/* send formatted frame */
time_t time_gps = global_pos->time_utc_usec / 1000000ULL;
time_t time_gps = subscription_data->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);
Expand Down
Loading