mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-12-03 11:24:10 +01:00
764 lines
22 KiB
C
764 lines
22 KiB
C
/**
|
|
******************************************************************************
|
|
* @addtogroup LibrePilotModules LibrePilot Modules
|
|
* @{
|
|
* @addtogroup UAVOFrSKYSensorHubBridge UAVO to FrSKY Bridge Module
|
|
* @{
|
|
*
|
|
* @file UAVOFrSKYSensorHubBridge.c
|
|
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017.
|
|
* dRonin, http://dRonin.org/, Copyright (C) 2016
|
|
* Tau Labs, http://taulabs.org, Copyright (C) 2014
|
|
* @brief Bridges selected UAVObjects to FrSKY Sensor Hub
|
|
* @see The GNU Public License (GPL) Version 3
|
|
*
|
|
*****************************************************************************/
|
|
/*
|
|
* 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/>
|
|
*
|
|
* Additional note on redistribution: The copyright and license notices above
|
|
* must be maintained in each individual source file that is a derivative work
|
|
* of this source file; otherwise redistribution is prohibited.
|
|
*/
|
|
|
|
// ****************
|
|
#include "pios.h"
|
|
#include "pios_board_io.h"
|
|
#include "openpilot.h"
|
|
#include "flightbatterysettings.h"
|
|
#include "flightbatterystate.h"
|
|
#include "homelocation.h"
|
|
#include "flightstatus.h"
|
|
#include "pios_thread.h"
|
|
#include "barosensor.h"
|
|
#include "accelsensor.h"
|
|
#include "gpspositionsensor.h"
|
|
#include "taskinfo.h"
|
|
|
|
// ****************
|
|
// Private functions
|
|
|
|
static void uavoFrSKYSensorHubBridgeTask(void *parameters);
|
|
|
|
static uint16_t frsky_pack_altitude(
|
|
float altitude,
|
|
uint8_t *serial_buf);
|
|
|
|
static uint16_t frsky_pack_temperature_01(
|
|
float temperature_01,
|
|
uint8_t *serial_buf);
|
|
|
|
static uint16_t frsky_pack_temperature_02(
|
|
float temperature_02,
|
|
uint8_t *serial_buf);
|
|
|
|
static uint16_t frsky_pack_accel(
|
|
float accels_x,
|
|
float accels_y,
|
|
float accels_z,
|
|
uint8_t *serial_buf);
|
|
|
|
static uint16_t frsky_pack_cellvoltage(
|
|
uint8_t cell,
|
|
float cell_voltage,
|
|
uint8_t *serial_buf);
|
|
|
|
static uint16_t frsky_pack_fas(
|
|
float voltage,
|
|
float current,
|
|
uint8_t *serial_buf);
|
|
|
|
static uint16_t frsky_pack_rpm(
|
|
uint16_t rpm,
|
|
uint8_t *serial_buf);
|
|
|
|
static uint16_t frsky_pack_gps(
|
|
float course,
|
|
int32_t latitude,
|
|
int32_t longitude,
|
|
float altitude,
|
|
float speed,
|
|
uint8_t *serial_buf);
|
|
|
|
static uint16_t frsky_pack_fuel(
|
|
float fuel_level,
|
|
uint8_t *serial_buf);
|
|
|
|
static uint16_t frsky_pack_stop(
|
|
uint8_t *serial_buf);
|
|
|
|
static int16_t frsky_acceleration_unit(float accel);
|
|
|
|
static void frsky_serialize_value(uint8_t valueid,
|
|
uint8_t *value,
|
|
uint8_t *serial_buf,
|
|
uint8_t *index);
|
|
|
|
static void frsky_write_userdata_byte(uint8_t byte,
|
|
uint8_t *serial_buf,
|
|
uint8_t *index);
|
|
|
|
static bool frame_trigger(uint8_t frame_num);
|
|
// ****************
|
|
// Private constants
|
|
|
|
|
|
#if defined(PIOS_FRSKY_SENSOR_HUB_STACK_SIZE)
|
|
#define STACK_SIZE_BYTES PIOS_FRSKY_SENSOR_HUB_STACK_SIZE
|
|
#else
|
|
#define STACK_SIZE_BYTES 672
|
|
#endif
|
|
|
|
#define TASK_PRIORITY (tskIDLE_PRIORITY)
|
|
#define TASK_RATE_HZ 10
|
|
|
|
#define FRSKY_MAX_PACKET_LEN 106
|
|
#define FRSKY_BAUD_RATE 9600
|
|
|
|
#define FRSKY_FRAME_DATA_HEADER 0x5E
|
|
#define FRSKY_FRAME_STOP 0x5E
|
|
|
|
enum FRSKY_VALUE_ID {
|
|
FRSKY_GPS_ALTITUDE_INTEGER = 0x01,
|
|
FRSKY_GPS_ALTITUDE_DECIMAL = FRSKY_GPS_ALTITUDE_INTEGER + 8,
|
|
FRSKY_TEMPERATURE_1 = 0x02,
|
|
FRSKY_RPM = 0x03,
|
|
FRSKY_FUEL_LEVEL = 0x04,
|
|
FRSKY_TEMPERATURE_2 = 0x05,
|
|
FRSKY_VOLTAGE = 0x06,
|
|
FRSKY_ALTITUDE_INTEGER = 0x10,
|
|
FRSKY_ALTITUDE_DECIMAL = 0x21,
|
|
FRSKY_GPS_SPEED_INTEGER = 0x11,
|
|
FRSKY_GPS_SPEED_DECIMAL = 0x11 + 8,
|
|
FRSKY_GPS_LONGITUDE_INTEGER = 0x12,
|
|
FRSKY_GPS_LONGITUDE_DECIMAL = FRSKY_GPS_LONGITUDE_INTEGER + 8,
|
|
FRSKY_GPS_E_W = 0x1A + 8,
|
|
FRSKY_GPS_LATITUDE_INTEGER = 0x13,
|
|
FRSKY_GPS_LATITUDE_DECIMAL = FRSKY_GPS_LATITUDE_INTEGER + 8,
|
|
FRSKY_GPS_N_S = 0x1B + 8,
|
|
FRSKY_GPS_COURSE_INTEGER = 0x14,
|
|
FRSKY_GPS_COURSE_DECIMAL = FRSKY_GPS_COURSE_INTEGER + 8,
|
|
FRSKY_DATE_MONTH = 0x15,
|
|
FRSKY_DATE_YEAR = 0x16,
|
|
FRSKY_HOUR_MINUTE = 0x17,
|
|
FRSKY_SECOND = 0x18,
|
|
FRSKY_ACCELERATION_X = 0x24,
|
|
FRSKY_ACCELERATION_Y = 0x25,
|
|
FRSKY_ACCELERATION_Z = 0x26,
|
|
FRSKY_VOLTAGE_AMPERE_SENSOR_INTEGER = 0x3A,
|
|
FRSKY_VOLTAGE_AMPERE_SENSOR_DECIMAL = 0x3B,
|
|
FRSKY_CURRENT = 0x28,
|
|
};
|
|
|
|
enum FRSKY_FRAME {
|
|
FRSKY_FRAME_VARIO,
|
|
FRSKY_FRAME_BATTERY,
|
|
FRSKY_FRAME_GPS,
|
|
};
|
|
|
|
static const uint8_t frsky_rates[] = {
|
|
[FRSKY_FRAME_VARIO] = 5, // 5Hz
|
|
[FRSKY_FRAME_BATTERY] = 5, // 5Hz
|
|
[FRSKY_FRAME_GPS] = 2, // 1Hz
|
|
};
|
|
|
|
#define MAXSTREAMS sizeof(frsky_rates)
|
|
#define KNOTS2M_PER_SECOND 0.514444F
|
|
#define GRAVITY 9.8F
|
|
|
|
// ****************
|
|
// Private variables
|
|
|
|
static struct {
|
|
uint32_t frsky_port;
|
|
uint8_t frame_ticks[MAXSTREAMS];
|
|
uint8_t serial_buf[FRSKY_MAX_PACKET_LEN];
|
|
} *shub_global;
|
|
|
|
/**
|
|
* Start the module
|
|
* \return -1 if start failed
|
|
* \return 0 on success
|
|
*/
|
|
static int32_t uavoFrSKYSensorHubBridgeStart(void)
|
|
{
|
|
if (shub_global) {
|
|
xTaskHandle taskHandle;
|
|
xTaskCreate(uavoFrSKYSensorHubBridgeTask, "uavoFrSKYSensorHubBridge", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle);
|
|
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_UAVOFRSKYSENSORHUBBRIDGE, taskHandle);
|
|
return 0;
|
|
}
|
|
return -1;
|
|
}
|
|
|
|
/**
|
|
* Initialize the module
|
|
* \return -1 if initialization failed
|
|
* \return 0 on success
|
|
*/
|
|
static int32_t uavoFrSKYSensorHubBridgeInitialize(void)
|
|
{
|
|
uintptr_t frsky_port = PIOS_COM_FRSKY_SENSORHUB;
|
|
|
|
if (frsky_port) {
|
|
shub_global = pios_malloc(sizeof(*shub_global));
|
|
|
|
if (shub_global == 0) {
|
|
return -1;
|
|
}
|
|
|
|
PIOS_COM_ChangeBaud(frsky_port, FRSKY_BAUD_RATE);
|
|
|
|
shub_global->frsky_port = frsky_port;
|
|
|
|
for (uint32_t x = 0; x < MAXSTREAMS; ++x) {
|
|
shub_global->frame_ticks[x] =
|
|
(TASK_RATE_HZ / frsky_rates[x]);
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
|
|
return -1;
|
|
}
|
|
MODULE_INITCALL(uavoFrSKYSensorHubBridgeInitialize, uavoFrSKYSensorHubBridgeStart);
|
|
|
|
/**
|
|
* Main task. It does not return.
|
|
*/
|
|
static void uavoFrSKYSensorHubBridgeTask(__attribute__((unused)) void *parameters)
|
|
{
|
|
uint32_t lastSysTime = PIOS_Thread_Systime();
|
|
|
|
// Main task loop
|
|
while (1) {
|
|
PIOS_Thread_Sleep_Until(&lastSysTime, 1000 / TASK_RATE_HZ);
|
|
|
|
if (frame_trigger(FRSKY_FRAME_VARIO)) {
|
|
uint16_t msg_length = 0;
|
|
|
|
// Get the accelerometer values
|
|
float accX = 0, accY = 0, accZ = 0;
|
|
if (AccelSensorHandle() != NULL) {
|
|
AccelSensorxGet(&accX);
|
|
AccelSensoryGet(&accY);
|
|
AccelSensorzGet(&accZ);
|
|
}
|
|
msg_length += frsky_pack_accel(
|
|
accX,
|
|
accY,
|
|
accZ,
|
|
shub_global->serial_buf + msg_length);
|
|
|
|
// Get the altritude from the barometer.
|
|
float altitude;
|
|
if (BaroSensorHandle() != NULL) {
|
|
BaroSensorAltitudeGet(&altitude);
|
|
}
|
|
msg_length += frsky_pack_altitude(
|
|
altitude,
|
|
shub_global->serial_buf + msg_length);
|
|
msg_length += frsky_pack_stop(shub_global->serial_buf + msg_length);
|
|
|
|
PIOS_COM_SendBuffer(shub_global->frsky_port,
|
|
shub_global->serial_buf, msg_length);
|
|
}
|
|
|
|
if (frame_trigger(FRSKY_FRAME_BATTERY)) {
|
|
uint16_t msg_length = 0;
|
|
|
|
FlightBatteryStateData batState;
|
|
if (FlightBatteryStateHandle() != NULL) {
|
|
FlightBatteryStateGet(&batState);
|
|
}
|
|
|
|
float voltage = voltage = batState.Voltage;
|
|
|
|
float current = current = batState.Current;
|
|
|
|
// As long as there is no voltage for each cell
|
|
// all cells will have the same voltage.
|
|
// Receiver will know number of cells.
|
|
FlightBatterySettingsData batSettings;
|
|
if (FlightBatterySettingsHandle() != NULL) {
|
|
FlightBatterySettingsGet(&batSettings);
|
|
}
|
|
if (batState.NbCellsAutodetected == FLIGHTBATTERYSTATE_NBCELLSAUTODETECTED_TRUE) {
|
|
batSettings.NbCells = batState.NbCells;
|
|
}
|
|
|
|
if (batSettings.NbCells > 0) {
|
|
float cell_v = voltage / batSettings.NbCells;
|
|
for (uint8_t i = 0; i < batSettings.NbCells; ++i) {
|
|
msg_length += frsky_pack_cellvoltage(
|
|
i,
|
|
cell_v,
|
|
shub_global->serial_buf + msg_length);
|
|
}
|
|
}
|
|
|
|
msg_length += frsky_pack_fas(
|
|
voltage,
|
|
current,
|
|
shub_global->serial_buf + msg_length);
|
|
|
|
if (batSettings.Capacity > 0) {
|
|
float fuel = 1.0f - batState.ConsumedEnergy / batSettings.Capacity;
|
|
msg_length += frsky_pack_fuel(
|
|
fuel,
|
|
shub_global->serial_buf + msg_length);
|
|
}
|
|
|
|
msg_length += frsky_pack_stop(shub_global->serial_buf + msg_length);
|
|
|
|
PIOS_COM_SendBuffer(shub_global->frsky_port,
|
|
shub_global->serial_buf, msg_length);
|
|
}
|
|
|
|
if (frame_trigger(FRSKY_FRAME_GPS)) {
|
|
uint16_t msg_length = 0;
|
|
|
|
/**
|
|
* Encodes ARM status and flight mode number as RPM value
|
|
* Since there is no RPM information in any UAVO available,
|
|
* we will intentionally misuse this item to encode other useful information.
|
|
* It will encode flight status as three-digit number as follow:
|
|
* most left digit encodes arm status (200=armed, 100=disarmed)
|
|
* two most right digits encode flight mode number (see FlightStatus UAVO FlightMode enum)
|
|
* To work properly on Taranis, you have to set Blades to "60" in telemetry setting
|
|
*/
|
|
FlightStatusData flight_status;
|
|
FlightStatusGet(&flight_status);
|
|
|
|
uint16_t status = 0;
|
|
float hdop, vdop;
|
|
|
|
status = (flight_status.Armed == FLIGHTSTATUS_ARMED_ARMED) ? 200 : 100;
|
|
status += flight_status.FlightMode;
|
|
|
|
msg_length += frsky_pack_rpm(status, shub_global->serial_buf + msg_length);
|
|
|
|
uint8_t hl_set = HOMELOCATION_SET_FALSE;
|
|
|
|
GPSPositionSensorData gpsPosData;
|
|
if (GPSPositionSensorHandle() != NULL) {
|
|
GPSPositionSensorGet(&gpsPosData);
|
|
}
|
|
|
|
if (HomeLocationHandle() != NULL) {
|
|
HomeLocationSetGet(&hl_set);
|
|
}
|
|
|
|
/**
|
|
* Encode GPS status and visible satellites as T1 value
|
|
* We will intentionally misuse this item to encode other useful information.
|
|
* Right-most two digits encode visible satellite count, left-most digit has following meaning:
|
|
* 1 - no GPS connected
|
|
* 2 - no fix
|
|
* 3 - 2D fix
|
|
* 4 - 3D fix
|
|
* 5 - 3D fix and HomeLocation is SET - should be safe for navigation
|
|
*/
|
|
switch (gpsPosData.Status) {
|
|
case GPSPOSITIONSENSOR_STATUS_NOGPS:
|
|
status = 100;
|
|
break;
|
|
case GPSPOSITIONSENSOR_STATUS_NOFIX:
|
|
status = 200;
|
|
break;
|
|
case GPSPOSITIONSENSOR_STATUS_FIX2D:
|
|
status = 300;
|
|
break;
|
|
case GPSPOSITIONSENSOR_STATUS_FIX3D:
|
|
case GPSPOSITIONSENSOR_STATUS_FIX3DDGNSS:
|
|
if (hl_set == HOMELOCATION_SET_TRUE) {
|
|
status = 500;
|
|
} else {
|
|
status = 400;
|
|
}
|
|
break;
|
|
}
|
|
|
|
if (gpsPosData.Satellites > 0) {
|
|
status += gpsPosData.Satellites;
|
|
}
|
|
|
|
msg_length += frsky_pack_temperature_01((float)status, shub_global->serial_buf + msg_length);
|
|
|
|
/**
|
|
* Encode GPS HDOP and VDOP as T2 value
|
|
* We will intentionally misuse this item to encode other useful information.
|
|
* VDOP in the upper 16 bits, max 256 (2.56 * 100)
|
|
* HDOP in the lower 16 bits, max 256 (2.56 * 100)
|
|
*/
|
|
hdop = gpsPosData.HDOP * 100.0f;
|
|
hdop = 0;
|
|
|
|
if (hdop > 255.0f) {
|
|
hdop = 255.0f;
|
|
}
|
|
|
|
vdop = gpsPosData.VDOP * 100.0f;
|
|
vdop = 0;
|
|
|
|
if (vdop > 255.0f) {
|
|
vdop = 255.0f;
|
|
}
|
|
|
|
msg_length += frsky_pack_temperature_02((vdop * 256 + hdop), shub_global->serial_buf + msg_length);
|
|
|
|
if (gpsPosData.Status == GPSPOSITIONSENSOR_STATUS_FIX2D ||
|
|
gpsPosData.Status == GPSPOSITIONSENSOR_STATUS_FIX3D ||
|
|
gpsPosData.Status == GPSPOSITIONSENSOR_STATUS_FIX3DDGNSS) {
|
|
msg_length += frsky_pack_gps(
|
|
gpsPosData.Heading,
|
|
gpsPosData.Latitude,
|
|
gpsPosData.Longitude,
|
|
gpsPosData.Altitude,
|
|
gpsPosData.Groundspeed,
|
|
shub_global->serial_buf + msg_length);
|
|
} else {
|
|
msg_length += frsky_pack_gps(0, 0, 0, 0, 0, shub_global->serial_buf + msg_length);
|
|
}
|
|
|
|
msg_length += frsky_pack_stop(shub_global->serial_buf + msg_length);
|
|
|
|
PIOS_COM_SendBuffer(shub_global->frsky_port,
|
|
shub_global->serial_buf, msg_length);
|
|
}
|
|
}
|
|
}
|
|
|
|
static bool frame_trigger(uint8_t frame_num)
|
|
{
|
|
uint8_t rate = frsky_rates[frame_num];
|
|
|
|
if (rate == 0) {
|
|
return false;
|
|
}
|
|
|
|
if (shub_global->frame_ticks[frame_num] == 0) {
|
|
// we're triggering now, setup the next trigger point
|
|
if (rate > TASK_RATE_HZ) {
|
|
rate = TASK_RATE_HZ;
|
|
}
|
|
shub_global->frame_ticks[frame_num] = (TASK_RATE_HZ / rate);
|
|
return true;
|
|
}
|
|
|
|
// count down at 50Hz
|
|
shub_global->frame_ticks[frame_num]--;
|
|
return false;
|
|
}
|
|
|
|
/**
|
|
* Writes altitude to buffer
|
|
* \return number of bytes written to the buffer
|
|
*/
|
|
static uint16_t frsky_pack_altitude(float altitude, uint8_t *serial_buf)
|
|
{
|
|
uint8_t index = 0;
|
|
|
|
float altitudeInteger = 0.0f;
|
|
|
|
uint16_t decimalValue = lroundf(modff(altitude, &altitudeInteger) * 100);
|
|
int16_t integerValue = lroundf(altitude);
|
|
|
|
frsky_serialize_value(FRSKY_ALTITUDE_INTEGER, (uint8_t *)&integerValue, serial_buf, &index);
|
|
frsky_serialize_value(FRSKY_ALTITUDE_DECIMAL, (uint8_t *)&decimalValue, serial_buf, &index);
|
|
|
|
return index;
|
|
}
|
|
|
|
/**
|
|
* Writes baro temperature to buffer
|
|
* \return number of bytes written to the buffer
|
|
*/
|
|
static uint16_t frsky_pack_temperature_01(
|
|
float temperature_01,
|
|
uint8_t *serial_buf)
|
|
{
|
|
uint8_t index = 0;
|
|
|
|
int16_t temperature = lroundf(temperature_01);
|
|
|
|
frsky_serialize_value(FRSKY_TEMPERATURE_1, (uint8_t *)&temperature, serial_buf, &index);
|
|
|
|
return index;
|
|
}
|
|
|
|
/**
|
|
* Writes accel temperature to buffer
|
|
* \return number of bytes written to the buffer
|
|
*/
|
|
static uint16_t frsky_pack_temperature_02(
|
|
float temperature_02,
|
|
uint8_t *serial_buf)
|
|
{
|
|
uint8_t index = 0;
|
|
|
|
int16_t temperature = lroundf(temperature_02);
|
|
|
|
frsky_serialize_value(FRSKY_TEMPERATURE_2, (uint8_t *)&temperature, serial_buf, &index);
|
|
|
|
return index;
|
|
}
|
|
|
|
/**
|
|
* Writes acceleration values to buffer
|
|
* \return number of bytes written to the buffer
|
|
*/
|
|
static uint16_t frsky_pack_accel(
|
|
float accels_x,
|
|
float accels_y,
|
|
float accels_z,
|
|
uint8_t *serial_buf)
|
|
{
|
|
uint8_t index = 0;
|
|
|
|
int16_t accel = frsky_acceleration_unit(accels_x);
|
|
|
|
frsky_serialize_value(FRSKY_ACCELERATION_X, (uint8_t *)&accel, serial_buf, &index);
|
|
|
|
accel = frsky_acceleration_unit(accels_y);
|
|
frsky_serialize_value(FRSKY_ACCELERATION_Y, (uint8_t *)&accel, serial_buf, &index);
|
|
|
|
accel = frsky_acceleration_unit(accels_z);
|
|
frsky_serialize_value(FRSKY_ACCELERATION_Z, (uint8_t *)&accel, serial_buf, &index);
|
|
|
|
return index;
|
|
}
|
|
|
|
static uint16_t frsky_pack_cellvoltage(
|
|
uint8_t cell,
|
|
float cell_voltage,
|
|
uint8_t *serial_buf)
|
|
{
|
|
// its not possible to address more than 15 cells
|
|
if (cell > 15) {
|
|
return 0;
|
|
}
|
|
|
|
uint8_t index = 0;
|
|
uint16_t v = lroundf((cell_voltage / 4.2f) * 2100);
|
|
if (v > 2100) {
|
|
v = 2100;
|
|
}
|
|
|
|
uint16_t voltage = ((v & 0x00ff) << 8) | (cell << 4) | ((v & 0x0f00) >> 8);
|
|
frsky_serialize_value(FRSKY_VOLTAGE, (uint8_t *)&voltage, serial_buf, &index);
|
|
|
|
return index;
|
|
}
|
|
/**
|
|
* Writes battery values to buffer
|
|
* \return number of bytes written to the buffer
|
|
*/
|
|
static uint16_t frsky_pack_fas(
|
|
float voltage,
|
|
float current,
|
|
uint8_t *serial_buf)
|
|
{
|
|
uint8_t index = 0;
|
|
|
|
voltage = (voltage * 110.0f) / 21.0f;
|
|
|
|
uint16_t integerValue = lroundf(voltage) / 100;
|
|
uint16_t decimalValue = lroundf(voltage - integerValue);
|
|
|
|
frsky_serialize_value(FRSKY_VOLTAGE_AMPERE_SENSOR_INTEGER, (uint8_t *)&integerValue, serial_buf, &index);
|
|
frsky_serialize_value(FRSKY_VOLTAGE_AMPERE_SENSOR_DECIMAL, (uint8_t *)&decimalValue, serial_buf, &index);
|
|
|
|
integerValue = lroundf(current * 10);
|
|
frsky_serialize_value(FRSKY_CURRENT, (uint8_t *)&integerValue, serial_buf, &index);
|
|
|
|
return index;
|
|
}
|
|
|
|
/**
|
|
* Writes rpm value to buffer
|
|
* \return number of bytes written to the buffer
|
|
*/
|
|
static uint16_t frsky_pack_rpm(uint16_t rpm, uint8_t *serial_buf)
|
|
{
|
|
uint8_t index = 0;
|
|
|
|
frsky_serialize_value(FRSKY_RPM, (uint8_t *)&rpm, serial_buf, &index);
|
|
|
|
return index;
|
|
}
|
|
|
|
/**
|
|
* Writes fuel value [0.0 ... 1.0] to buffer
|
|
* \return number of bytes written to the buffer
|
|
*/
|
|
static uint16_t frsky_pack_fuel(
|
|
float fuel_level,
|
|
uint8_t *serial_buf)
|
|
{
|
|
uint8_t index = 0;
|
|
|
|
uint16_t level = lroundf(abs(fuel_level * 100));
|
|
|
|
// Use fixed levels here because documentation says so.
|
|
if (level > 94) {
|
|
level = 100;
|
|
} else if (level > 69) {
|
|
level = 75;
|
|
} else if (level > 44) {
|
|
level = 50;
|
|
} else if (level > 19) {
|
|
level = 25;
|
|
} else {
|
|
level = 0;
|
|
}
|
|
|
|
frsky_serialize_value(FRSKY_FUEL_LEVEL, (uint8_t *)&level, serial_buf, &index);
|
|
|
|
return index;
|
|
}
|
|
|
|
/**
|
|
* Writes gps values to buffer
|
|
* \return number of bytes written to the buffer
|
|
*/
|
|
static uint16_t frsky_pack_gps(
|
|
float course,
|
|
int32_t latitude,
|
|
int32_t longitude,
|
|
float altitude,
|
|
float speed,
|
|
uint8_t *serial_buf)
|
|
{
|
|
uint8_t index = 0;
|
|
|
|
{
|
|
float courseInteger = 0.0f;
|
|
uint16_t decimalValue = lroundf(modff(course, &courseInteger) * 100);
|
|
uint16_t integerValue = lroundf(courseInteger);
|
|
|
|
frsky_serialize_value(FRSKY_GPS_COURSE_INTEGER, (uint8_t *)&integerValue, serial_buf, &index);
|
|
frsky_serialize_value(FRSKY_GPS_COURSE_DECIMAL, (uint8_t *)&decimalValue, serial_buf, &index);
|
|
}
|
|
|
|
// latitude
|
|
{
|
|
uint16_t integerValue = (abs(latitude) / 100000);
|
|
uint16_t decimalValue = (abs(latitude) / 10) % 10000;
|
|
|
|
frsky_serialize_value(FRSKY_GPS_LATITUDE_INTEGER, (uint8_t *)&integerValue, serial_buf, &index);
|
|
frsky_serialize_value(FRSKY_GPS_LATITUDE_DECIMAL, (uint8_t *)&decimalValue, serial_buf, &index);
|
|
|
|
uint16_t hemisphere = 'N';
|
|
if (latitude < 0) {
|
|
hemisphere = 'S';
|
|
}
|
|
frsky_serialize_value(FRSKY_GPS_N_S, (uint8_t *)&hemisphere, serial_buf, &index);
|
|
}
|
|
|
|
// longitude
|
|
{
|
|
uint16_t integerValue = (abs(longitude) / 100000);
|
|
uint16_t decimalValue = (abs(longitude) / 10) % 10000;
|
|
|
|
uint16_t hemisphere = 'E';
|
|
if (longitude < 0) {
|
|
hemisphere = 'W';
|
|
}
|
|
|
|
frsky_serialize_value(FRSKY_GPS_LONGITUDE_INTEGER, (uint8_t *)&integerValue, serial_buf, &index);
|
|
frsky_serialize_value(FRSKY_GPS_LONGITUDE_DECIMAL, (uint8_t *)&decimalValue, serial_buf, &index);
|
|
|
|
frsky_serialize_value(FRSKY_GPS_E_W, (uint8_t *)&hemisphere, serial_buf, &index);
|
|
}
|
|
|
|
// speed
|
|
{
|
|
float knots = speed / KNOTS2M_PER_SECOND;
|
|
|
|
float knotsInteger = 0.0f;
|
|
uint16_t decimalValue = lroundf(modff(knots, &knotsInteger) * 100);
|
|
int16_t integerValue = lroundf(knotsInteger);
|
|
|
|
frsky_serialize_value(FRSKY_GPS_SPEED_INTEGER, (uint8_t *)&integerValue, serial_buf, &index);
|
|
frsky_serialize_value(FRSKY_GPS_SPEED_DECIMAL, (uint8_t *)&decimalValue, serial_buf, &index);
|
|
}
|
|
|
|
// altitude
|
|
{
|
|
float altitudeInteger = 0.0F;
|
|
uint16_t decimalValue = lroundf(modff(altitude, &altitudeInteger) * 100);
|
|
int16_t integerValue = lroundf(altitudeInteger);
|
|
|
|
frsky_serialize_value(FRSKY_GPS_ALTITUDE_INTEGER, (uint8_t *)&integerValue, serial_buf, &index);
|
|
frsky_serialize_value(FRSKY_GPS_ALTITUDE_DECIMAL, (uint8_t *)&decimalValue, serial_buf, &index);
|
|
}
|
|
|
|
return index;
|
|
}
|
|
|
|
/**
|
|
* Writes stop byte to buffer
|
|
* \return number of bytes written to the buffer
|
|
*/
|
|
static uint16_t frsky_pack_stop(uint8_t *serial_buf)
|
|
{
|
|
serial_buf[0] = FRSKY_FRAME_STOP;
|
|
|
|
return 1;
|
|
}
|
|
|
|
/**
|
|
* Convert acceleration value to frsky acceleration unit
|
|
* \return Acceleration value
|
|
*/
|
|
static int16_t frsky_acceleration_unit(float accel)
|
|
{
|
|
accel = accel / GRAVITY; // convert to gravity
|
|
accel *= 1000;
|
|
return lroundf(accel);
|
|
}
|
|
|
|
/**
|
|
* Write value to buffer
|
|
*/
|
|
static void frsky_serialize_value(uint8_t valueid, uint8_t *value, uint8_t *serial_buf, uint8_t *index)
|
|
{
|
|
serial_buf[(*index)++] = FRSKY_FRAME_DATA_HEADER;
|
|
serial_buf[(*index)++] = valueid;
|
|
|
|
frsky_write_userdata_byte(value[0], serial_buf, index);
|
|
frsky_write_userdata_byte(value[1], serial_buf, index);
|
|
}
|
|
|
|
/**
|
|
* Write a byte to the buffer and do byte stuffing if needed.
|
|
*/
|
|
static void frsky_write_userdata_byte(uint8_t byte, uint8_t *serial_buf, uint8_t *index)
|
|
{
|
|
// ** byte stuffing
|
|
if ((byte == 0x5E) || (byte == 0x5D)) {
|
|
serial_buf[(*index)++] = 0x5D;
|
|
serial_buf[(*index)++] = ~(byte ^ 0x60);
|
|
} else {
|
|
serial_buf[(*index)++] = byte;
|
|
}
|
|
}
|
|
|
|
/**
|
|
* @}
|
|
* @}
|
|
*/
|