mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-03-21 13:28:58 +01:00
LP-500 HoTT Bridge Module ported from TauLabs
This commit is contained in:
Eric Price
parent
a2f833641b
commit
545f1c863b
379
flight/modules/UAVOHottBridge/inc/uavohottbridge.h
Normal file
379
flight/modules/UAVOHottBridge/inc/uavohottbridge.h
Normal file
@ -0,0 +1,379 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup Modules
|
||||
* @{
|
||||
* @addtogroup UAVOHoTTBridge UAVO to HoTT Bridge Telemetry Module
|
||||
* @{
|
||||
*
|
||||
* @file uavohottbridge.h
|
||||
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017.
|
||||
* Tau Labs, http://taulabs.org, Copyright (C) 2013
|
||||
* @brief sends telemery data on HoTT request
|
||||
*
|
||||
* @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, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
// timing variables
|
||||
#define IDLE_TIME 10 // idle line delay to prevent data crashes on telemetry line.
|
||||
#define DATA_TIME 3 // time between 2 transmitted bytes
|
||||
|
||||
// sizes and lengths
|
||||
#define climbratesize 50 // defines size of ring buffer for climbrate calculation
|
||||
#define statussize 21 // number of characters in status line
|
||||
#define HOTT_MAX_MESSAGE_LENGTH 200
|
||||
|
||||
// scale factors
|
||||
#define M_TO_CM 100 // scale m to cm or m/s to cm/s
|
||||
#define MS_TO_KMH 3.6f // scale m/s to km/h
|
||||
#define DEG_TO_UINT 0.5f // devide degrees by 2. then the value fits into a byte.
|
||||
|
||||
// offsets. used to make transmitted values unsigned.
|
||||
#define OFFSET_ALTITUDE 500 // 500m
|
||||
#define OFFSET_CLIMBRATE 30000 // 30000cm/s or 300m/s
|
||||
#define OFFSET_CLIMBRATE3S 120 // 120m/s
|
||||
#define OFFSET_TEMPERATURE 20 // 20 degrees
|
||||
|
||||
// Bits to invert display areas or values
|
||||
#define VARIO_INVERT_ALT (1 << 0) // altitude
|
||||
#define VARIO_INVERT_MAX (1 << 1) // max altitude
|
||||
#define VARIO_INVERT_MIN (1 << 2) // min altitude
|
||||
#define VARIO_INVERT_CR1S (1 << 3) // climbrate 1s
|
||||
#define VARIO_INVERT_CR3S (1 << 4) // climbrate 3s
|
||||
#define VARIO_INVERT_CR10S (1 << 5) // climbrate 10s
|
||||
|
||||
#define GPS_INVERT_HDIST (1 << 0) // home distance
|
||||
#define GPS_INVERT_SPEED (1 << 1) // speed (kmh)
|
||||
#define GPS_INVERT_ALT (1 << 2) // altitude
|
||||
#define GPS_INVERT_CR1S (1 << 3) // climbrate 1s
|
||||
#define GPS_INVERT_CR3S (1 << 4) // climbrate 3s
|
||||
#define GPS_INVERT2_POS (1 << 0) // GPS postion values
|
||||
|
||||
#define GAM_INVERT_CELL (1 << 0) // cell voltage
|
||||
#define GAM_INVERT_BATT1 (1 << 1) // battery 1 voltage
|
||||
#define GAM_INVERT_BATT2 (1 << 2) // battery 1 voltage
|
||||
#define GAM_INVERT_TEMP1 (1 << 3) // temperature 1
|
||||
#define GAM_INVERT_TEMP2 (1 << 4) // temperature 2
|
||||
#define GAM_INVERT_FUEL (1 << 5) // fuel
|
||||
#define GAM_INVERT2_CURRENT (1 << 0) // current
|
||||
#define GAM_INVERT2_VOLTAGE (1 << 1) // voltage
|
||||
#define GAM_INVERT2_ALT (1 << 2) // altitude
|
||||
#define GAM_INVERT2_CR1S (1 << 3) // climbrate 1s
|
||||
#define GAM_INVERT2_CR3S (1 << 4) // climbrate 3s
|
||||
|
||||
#define EAM_INVERT_CAPACITY (1 << 0) // capacity
|
||||
#define EAM_INVERT_BATT1 (1 << 1) // battery 1 voltage
|
||||
#define EAM_INVERT_BATT2 (1 << 2) // battery 1 voltage
|
||||
#define EAM_INVERT_TEMP1 (1 << 3) // temperature 1
|
||||
#define EAM_INVERT_TEMP2 (1 << 4) // temperature 2
|
||||
#define EAM_INVERT_ALT (1 << 5) // altitude
|
||||
#define EAM_INVERT_CURRENT (1 << 6) // current
|
||||
#define EAM_INVERT_VOLTAGE (1 << 7) // voltage
|
||||
#define EAM_INVERT2_ALT (1 << 2) // altitude
|
||||
#define EAM_INVERT2_CR1S (1 << 3) // climbrate 1s
|
||||
#define EAM_INVERT2_CR3S (1 << 4) // climbrate 3s
|
||||
|
||||
#define ESC_INVERT_VOLTAGE (1 << 0) // voltage
|
||||
#define ESC_INVERT_TEMP1 (1 << 1) // temperature 1
|
||||
#define ESC_INVERT_TEMP2 (1 << 2) // temperature 2
|
||||
#define ESC_INVERT_CURRENT (1 << 3) // current
|
||||
#define ESC_INVERT_RPM (1 << 4) // rpm
|
||||
#define ESC_INVERT_CAPACITY (1 << 5) // capacity
|
||||
#define ESC_INVERT_MAXCURRENT (1 << 6) // maximum current
|
||||
|
||||
// message codes
|
||||
#define HOTT_TEXT_ID 0x7f // Text request
|
||||
#define HOTT_BINARY_ID 0x80 // Binary request
|
||||
#define HOTT_VARIO_ID 0x89 // Vario Module ID
|
||||
#define HOTT_VARIO_TEXT_ID 0x90 // Vario Module TEXT ID
|
||||
#define HOTT_GPS_ID 0x8a // GPS Module ID
|
||||
#define HOTT_GPS_TEXT_ID 0xa0 // GPS Module TEXT ID
|
||||
#define HOTT_ESC_ID 0x8c // ESC Module ID
|
||||
#define HOTT_ESC_TEXT_ID 0xc0 // ESC Module TEXT ID
|
||||
#define HOTT_GAM_ID 0x8d // General Air Module ID
|
||||
#define HOTT_GAM_TEXT_ID 0xd0 // General Air Module TEXT ID
|
||||
#define HOTT_EAM_ID 0x8e // Electric Air Module ID
|
||||
#define HOTT_EAM_TEXT_ID 0xe0 // Electric Air Module TEXT ID
|
||||
#define HOTT_TEXT_START 0x7b // Start byte Text mode
|
||||
#define HOTT_START 0x7c // Start byte Binary mode
|
||||
#define HOTT_STOP 0x7d // End byte
|
||||
#define HOTT_BUTTON_DEC 0xEB // minus button
|
||||
#define HOTT_BUTTON_INC 0xED // plus button
|
||||
#define HOTT_BUTTON_SET 0xE9 // set button
|
||||
#define HOTT_BUTTON_NIL 0x0F // esc button
|
||||
#define HOTT_BUTTON_NEXT 0xEE // next button
|
||||
#define HOTT_BUTTON_PREV 0xE7 // previous button
|
||||
|
||||
// prefined signal tones or spoken announcments
|
||||
#define HOTT_TONE_A 1 // minimum speed
|
||||
#define HOTT_TONE_B 2 // sink rate 3 seconds
|
||||
#define HOTT_TONE_C 3 // sink rate 1 second
|
||||
#define HOTT_TONE_D 4 // maximum distance
|
||||
#define HOTT_TONE_E 5 // -
|
||||
#define HOTT_TONE_F 6 // minimum temperature sensor 1
|
||||
#define HOTT_TONE_G 7 // minimum temperature sensor 2
|
||||
#define HOTT_TONE_H 8 // maximum temperature sensor 1
|
||||
#define HOTT_TONE_I 9 // maximum temperature sensor 2
|
||||
#define HOTT_TONE_J 10 // overvoltage sensor 1
|
||||
#define HOTT_TONE_K 11 // overvoltage sensor 2
|
||||
#define HOTT_TONE_L 12 // maximum speed
|
||||
#define HOTT_TONE_M 13 // climb rate 3 seconds
|
||||
#define HOTT_TONE_N 14 // climb rate 1 second
|
||||
#define HOTT_TONE_O 15 // minimum height
|
||||
#define HOTT_TONE_P 16 // minimum input voltage
|
||||
#define HOTT_TONE_Q 17 // minimum cell voltage
|
||||
#define HOTT_TONE_R 18 // undervoltage sensor 1
|
||||
#define HOTT_TONE_S 19 // undervoltage sensor 2
|
||||
#define HOTT_TONE_T 20 // minimum rpm
|
||||
#define HOTT_TONE_U 21 // fuel reserve
|
||||
#define HOTT_TONE_V 22 // capacity
|
||||
#define HOTT_TONE_W 23 // maximum current
|
||||
#define HOTT_TONE_X 24 // maximum input voltage
|
||||
#define HOTT_TONE_Y 25 // maximum rpm
|
||||
#define HOTT_TONE_Z 26 // maximum height
|
||||
#define HOTT_TONE_20M 37 // 20 meters
|
||||
#define HOTT_TONE_40M 38 // 40 meters
|
||||
#define HOTT_TONE_60M 39 // 60 meters
|
||||
#define HOTT_TONE_80M 40 // 80 meters
|
||||
#define HOTT_TONE_100M 41 // 100 meters
|
||||
#define HOTT_TONE_42 42 // receiver voltage
|
||||
#define HOTT_TONE_43 43 // receiver temperature
|
||||
#define HOTT_TONE_200M 46 // 200 meters
|
||||
#define HOTT_TONE_400M 47 // 400 meters
|
||||
#define HOTT_TONE_600M 48 // 600 meters
|
||||
#define HOTT_TONE_800M 49 // 800 meters
|
||||
#define HOTT_TONE_1000M 50 // 10000 meters
|
||||
#define HOTT_TONE_51 51 // maximum servo temperature
|
||||
#define HOTT_TONE_52 52 // maximum servo position difference
|
||||
|
||||
|
||||
// Private types
|
||||
typedef struct {
|
||||
uint8_t l;
|
||||
uint8_t h;
|
||||
} uword_t;
|
||||
|
||||
// Private structures
|
||||
struct telemetrydata {
|
||||
HoTTBridgeSettingsData Settings;
|
||||
AttitudeStateData Attitude;
|
||||
BaroSensorData Baro;
|
||||
FlightBatteryStateData Battery;
|
||||
FlightStatusData FlightStatus;
|
||||
GPSPositionSensorData GPS;
|
||||
GPSTimeData GPStime;
|
||||
GyroSensorData Gyro;
|
||||
HomeLocationData Home;
|
||||
PositionStateData Position;
|
||||
SystemAlarmsData SysAlarms;
|
||||
VelocityStateData Velocity;
|
||||
int16_t climbratebuffer[climbratesize];
|
||||
uint8_t climbrate_pointer;
|
||||
float altitude;
|
||||
float min_altitude;
|
||||
float max_altitude;
|
||||
float altitude_last;
|
||||
float climbrate1s;
|
||||
float climbrate3s;
|
||||
float climbrate10s;
|
||||
float homedistance;
|
||||
float homecourse;
|
||||
uint8_t last_armed;
|
||||
char statusline[statussize];
|
||||
};
|
||||
|
||||
// VARIO Module message structure
|
||||
struct hott_vario_message {
|
||||
uint8_t start; // start byte
|
||||
uint8_t sensor_id; // VARIO sensor ID
|
||||
uint8_t warning; // 0…= warning beeps
|
||||
uint8_t sensor_text_id; // VARIO sensor text ID
|
||||
uint8_t alarm_inverse; // this inverts specific parts of display
|
||||
uword_t altitude; // altitude (meters), offset 500, 500 == 0m
|
||||
uword_t max_altitude; // max. altitude (meters)
|
||||
uword_t min_altitude; // min. altitude (meters)
|
||||
uword_t climbrate; // climb rate (0.01m/s), offset 30000, 30000 == 0.00m/s
|
||||
uword_t climbrate3s; // climb rate (0.01m/3s)
|
||||
uword_t climbrate10s; // climb rate (0.01m/10s)
|
||||
uint8_t ascii[21]; // 21 chars of text
|
||||
uint8_t ascii1; // ASCII Free character [1] (next to Alt)
|
||||
uint8_t ascii2; // ASCII Free character [2] (next to Dir)
|
||||
uint8_t ascii3; // ASCII Free character [3] (next to I)
|
||||
int8_t compass; // 1=2°, -1=-2°
|
||||
uint8_t version; // version number
|
||||
uint8_t stop; // stop byte
|
||||
uint8_t checksum; // Lower 8-bits of all bytes summed
|
||||
};
|
||||
|
||||
// GPS Module message structure
|
||||
struct hott_gps_message {
|
||||
uint8_t start; // start byte
|
||||
uint8_t sensor_id; // GPS sensor ID
|
||||
uint8_t warning; // 0…= warning beeps
|
||||
uint8_t sensor_text_id; // GPS Sensor text mode ID
|
||||
uint8_t alarm_inverse1; // this inverts specific parts of display
|
||||
uint8_t alarm_inverse2;
|
||||
uint8_t flight_direction; // flight direction (1 = 2°; 0° = north, 90° = east , 180° = south , 270° west)
|
||||
uword_t gps_speed; // GPS speed (km/h)
|
||||
uint8_t latitude_ns; // GPS latitude north/south (0 = N)
|
||||
uword_t latitude_min; // GPS latitude (min)
|
||||
uword_t latitude_sec; // GPS latitude (sec)
|
||||
uint8_t longitude_ew; // GPS longitude east/west (0 = E)
|
||||
uword_t longitude_min; // GPS longitude (min)
|
||||
uword_t longitude_sec; // GPS longitude (sec)
|
||||
uword_t distance; // distance from home location (meters)
|
||||
uword_t altitude; // altitude (meters), offset 500, 500 == 0m
|
||||
uword_t climbrate; // climb rate (0.01m/s), offset of 30000, 30000 = 0.00 m/s
|
||||
uint8_t climbrate3s; // climb rate in (m/3s). offset of 120, 120 == 0m/3sec
|
||||
uint8_t gps_num_sat; // GPS number of satelites
|
||||
uint8_t gps_fix_char; // GPS FixChar ('D'=DGPS, '2'=2D, '3'=3D)
|
||||
uint8_t home_direction; // home direction (1=2°, direction from starting point to model position)
|
||||
int8_t angle_roll; // angle x-direction (roll 1=2°, 255=-2°)
|
||||
int8_t angle_nick; // angle y-direction (nick)
|
||||
int8_t angle_compass; // angle z-direction (yaw)
|
||||
uint8_t gps_hour; // GPS time hours
|
||||
uint8_t gps_min; // GPS time minutes
|
||||
uint8_t gps_sec; // GPS time seconds
|
||||
uint8_t gps_msec; // GPS time .sss
|
||||
uword_t msl; // MSL or NN altitude
|
||||
uint8_t vibration; // vibration
|
||||
uint8_t ascii4; // ASCII Free Character [4] (next to home distance)
|
||||
uint8_t ascii5; // ASCII Free Character [5] (next to home direction)
|
||||
uint8_t ascii6; // ASCII Free Character [6] (next to number of sats)
|
||||
uint8_t version; // version number (0=gps, 1=gyro, 255=multicopter)
|
||||
uint8_t stop; // stop byte
|
||||
uint8_t checksum; // Lower 8-bits of all bytes summed
|
||||
};
|
||||
|
||||
// General Air Module message structure
|
||||
struct hott_gam_message {
|
||||
uint8_t start; // start byte
|
||||
uint8_t sensor_id; // GAM sensor ID
|
||||
uint8_t warning; // 0…= warning beeps
|
||||
uint8_t sensor_text_id; // EAM Sensor text mode ID
|
||||
uint8_t alarm_inverse1; // this inverts specific parts of display
|
||||
uint8_t alarm_inverse2;
|
||||
uint8_t cell1; // cell voltages in 0.02V steps, 210 == 4.2V
|
||||
uint8_t cell2;
|
||||
uint8_t cell3;
|
||||
uint8_t cell4;
|
||||
uint8_t cell5;
|
||||
uint8_t cell6;
|
||||
uword_t batt1_voltage; // battery sensor 1 in 0.1V steps, 50 == 5.5V
|
||||
uword_t batt2_voltage; // battery sensor 2 voltage
|
||||
uint8_t temperature1; // temperature 1 in °C, offset of 20, 20 == 0°C
|
||||
uint8_t temperature2; // temperature 2
|
||||
uint8_t fuel_procent; // fuel capacity in %, values from 0..100
|
||||
uword_t fuel_ml; // fuel capacity in ml, values from 0..65535
|
||||
uword_t rpm; // rpm, scale factor 10, 300 == 3000rpm
|
||||
uword_t altitude; // altitude in meters, offset of 500, 500 == 0m
|
||||
uword_t climbrate; // climb rate (0.01m/s), offset of 30000, 30000 = 0.00 m/s
|
||||
uint8_t climbrate3s; // climb rate (m/3sec). offset of 120, 120 == 0m/3sec
|
||||
uword_t current; // current in 0.1A steps
|
||||
uword_t voltage; // main power voltage in 0.1V steps
|
||||
uword_t capacity; // used battery capacity in 10mAh steps
|
||||
uword_t speed; // speed in km/h
|
||||
uint8_t min_cell_volt; // lowest cell voltage in 20mV steps. 124 == 2.48V
|
||||
uint8_t min_cell_volt_num; // number of the cell with the lowest voltage
|
||||
uword_t rpm2; // rpm2 in 10 rpm steps, 300 == 3000rpm
|
||||
uint8_t g_error_number; // general error number (Voice error == 12)
|
||||
uint8_t pressure; // pressure up to 15bar, 0.1bar steps
|
||||
uint8_t version; // version number
|
||||
uint8_t stop; // stop byte
|
||||
uint8_t checksum; // Lower 8-bits of all bytes summed
|
||||
};
|
||||
|
||||
// Electric Air Module message structure
|
||||
struct hott_eam_message {
|
||||
uint8_t start; // Start byte
|
||||
uint8_t sensor_id; // EAM sensor id
|
||||
uint8_t warning;
|
||||
uint8_t sensor_text_id; // EAM Sensor text mode ID
|
||||
uint8_t alarm_inverse1; // this inverts specific parts of display
|
||||
uint8_t alarm_inverse2;
|
||||
uint8_t cell1_L; // cell voltages of the lower battery
|
||||
uint8_t cell2_L;
|
||||
uint8_t cell3_L;
|
||||
uint8_t cell4_L;
|
||||
uint8_t cell5_L;
|
||||
uint8_t cell6_L;
|
||||
uint8_t cell7_L;
|
||||
uint8_t cell1_H; // cell voltages of higher battery
|
||||
uint8_t cell2_H;
|
||||
uint8_t cell3_H;
|
||||
uint8_t cell4_H;
|
||||
uint8_t cell5_H;
|
||||
uint8_t cell6_H;
|
||||
uint8_t cell7_H;
|
||||
uword_t batt1_voltage; // battery sensor 1 voltage, in steps of 0.02V
|
||||
uword_t batt2_voltage; // battery sensor 2 voltage, in steps of 0.02V
|
||||
uint8_t temperature1; // temperature sensor 1. 20 = 0 degrees
|
||||
uint8_t temperature2; // temperature sensor 2. 20 = 0 degrees
|
||||
uword_t altitude; // altitude (meters). 500 = 0 meters
|
||||
uword_t current; // current (A) in steps of 0.1A
|
||||
uword_t voltage; // main power voltage in steps of 0.1V
|
||||
uword_t capacity; // used battery capacity in steps of 10mAh
|
||||
uword_t climbrate; // climb rate in 0.01m/s. 0m/s = 30000
|
||||
uint8_t climbrate3s; // climb rate in m/3sec. 0m/3sec = 120
|
||||
uword_t rpm; // rpm in steps of 10 rpm
|
||||
uint8_t electric_min; // estaminated flight time in minutes.
|
||||
uint8_t electric_sec; // estaminated flight time in seconds.
|
||||
uword_t speed; // speed in km/h in steps of 1 km/h
|
||||
uint8_t stop; // Stop byte
|
||||
uint8_t checksum; // Lower 8-bits of all bytes summed.
|
||||
};
|
||||
|
||||
// ESC Module message structure
|
||||
struct hott_esc_message {
|
||||
uint8_t start; // Start byte
|
||||
uint8_t sensor_id; // EAM sensor id
|
||||
uint8_t warning;
|
||||
uint8_t sensor_text_id; // ESC Sensor text mode ID
|
||||
uint8_t alarm_inverse1;
|
||||
uint8_t alarm_inverse2;
|
||||
uword_t batt_voltage; // battery voltage in steps of 0.1V
|
||||
uword_t min_batt_voltage; // min battery voltage
|
||||
uword_t batt_capacity; // used battery capacity in steps of 10mAh
|
||||
uint8_t temperatureESC; // temperature of ESC . 20 = 0 degrees
|
||||
uint8_t max_temperatureESC; // max temperature of ESC
|
||||
uword_t current; // Current in steps of 0.1A
|
||||
uword_t max_current; // maximal current
|
||||
uword_t rpm; // rpm in steps of 10 rpm
|
||||
uword_t max_rpm; // max rpm
|
||||
uint8_t temperatureMOT; // motor temperature
|
||||
uint8_t max_temperatureMOT; // maximal motor temperature
|
||||
uint8_t dummy[19]; // 19 dummy bytes
|
||||
uint8_t stop; // Stop byte
|
||||
uint8_t checksum; // Lower 8-bits of all bytes summed.
|
||||
};
|
||||
|
||||
// TEXT message structure
|
||||
struct hott_text_message {
|
||||
uint8_t start; // Start byte
|
||||
uint8_t sensor_id; // TEXT id
|
||||
uint8_t warning;
|
||||
uint8_t text[21][8]; // text field 21 columns and 8 rows (bit 7=1 for inverse display)
|
||||
uint8_t stop; // Stop byte
|
||||
uint8_t checksum; // Lower 8-bits of all bytes summed.
|
||||
};
|
||||
|
||||
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
||||
@ -2,12 +2,13 @@
|
||||
******************************************************************************
|
||||
* @addtogroup OpenPilotModules OpenPilot Modules
|
||||
* @{
|
||||
* @addtogroup UAVOHoTTBridge UAVO to HoTT Bridge Module
|
||||
* @addtogroup UAVOHoTTBridge UAVO to HoTT Bridge Telemetry Module
|
||||
* @{
|
||||
*
|
||||
* @file uavohottridge.c
|
||||
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017.
|
||||
* @brief Bridges certain UAVObjects to HoTT on USB VCP
|
||||
* Tau Labs, http://taulabs.org, Copyright (C) 2013-2014
|
||||
* @brief sends telemery data on HoTT request
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
@ -36,213 +37,55 @@
|
||||
#include "hwsettings.h"
|
||||
#include "taskinfo.h"
|
||||
#include "callbackinfo.h"
|
||||
/*
|
||||
#include "insgps.h"
|
||||
#include "receiverstatus.h"
|
||||
#include "flightmodesettings.h"
|
||||
#include "flightbatterysettings.h"
|
||||
#include "flightbatterystate.h"
|
||||
#include "gpspositionsensor.h"
|
||||
#include "manualcontrolsettings.h"
|
||||
#include "oplinkstatus.h"
|
||||
#include "accessorydesired.h"
|
||||
#include "airspeedstate.h"
|
||||
#include "actuatorsettings.h"
|
||||
#include "systemstats.h"
|
||||
#include "systemalarms.h"
|
||||
#include "takeofflocation.h"
|
||||
#include "homelocation.h"
|
||||
#include "stabilizationdesired.h"
|
||||
#include "stabilizationsettings.h"
|
||||
#include "stabilizationbank.h"
|
||||
#include "stabilizationsettingsbank1.h"
|
||||
#include "stabilizationsettingsbank2.h"
|
||||
#include "stabilizationsettingsbank3.h"
|
||||
#include "magstate.h"
|
||||
#include "manualcontrolcommand.h"
|
||||
#include "accessorydesired.h"
|
||||
#include "actuatordesired.h"
|
||||
#include "auxpositionsensor.h"
|
||||
#include "pathdesired.h"
|
||||
#include "poilocation.h"
|
||||
#include "flightmodesettings.h"
|
||||
#include "flightstatus.h"
|
||||
#include "positionstate.h"
|
||||
#include "velocitystate.h"
|
||||
#include "hottbridgesettings.h"
|
||||
#include "attitudestate.h"
|
||||
#include "gyrostate.h"
|
||||
*/
|
||||
|
||||
#include "barosensor.h"
|
||||
#include "flightbatterystate.h"
|
||||
#include "flightstatus.h"
|
||||
#include "gyrosensor.h"
|
||||
#include "gpspositionsensor.h"
|
||||
#include "gpstime.h"
|
||||
#include "homelocation.h"
|
||||
#include "positionstate.h"
|
||||
#include "systemalarms.h"
|
||||
#include "velocitystate.h"
|
||||
#include "hottbridgestatus.h"
|
||||
#include "hottbridgesettings.h"
|
||||
#include "objectpersistence.h"
|
||||
|
||||
#include "pios_sensors.h"
|
||||
#include "uavohottbridge.h"
|
||||
|
||||
#if defined(PIOS_INCLUDE_HoTT_BRIDGE)
|
||||
|
||||
struct hott_bridge {
|
||||
uintptr_t com;
|
||||
|
||||
uint32_t lastPingTimestamp;
|
||||
uint8_t myPingSequence;
|
||||
uint8_t remotePingSequence;
|
||||
PiOSDeltatimeConfig roundtrip;
|
||||
double roundTripTime;
|
||||
int32_t pingTimer;
|
||||
int32_t stateTimer;
|
||||
int32_t rateTimer;
|
||||
float rateAccumulator[3];
|
||||
uint8_t rx_buffer[HoTTBRIDGEMESSAGE_BUFFERSIZE];
|
||||
size_t rx_length;
|
||||
volatile bool scheduled[HoTTBRIDGEMESSAGE_END_ARRAY_SIZE];
|
||||
};
|
||||
#if defined(PIOS_INCLUDE_HOTT_BRIDGE)
|
||||
|
||||
#if defined(PIOS_HoTT_STACK_SIZE)
|
||||
#define STACK_SIZE_BYTES PIOS_HoTT_STACK_SIZE
|
||||
#define STACK_SIZE_BYTES PIOS_HoTT_STACK_SIZE
|
||||
#else
|
||||
#define STACK_SIZE_BYTES 2048
|
||||
#define STACK_SIZE_BYTES 2048
|
||||
#endif
|
||||
#define TASK_PRIORITY CALLBACK_TASK_AUXILIARY
|
||||
#define CALLBACK_PRIORITY CALLBACK_PRIORITY_REGULAR
|
||||
#define CBTASK_PRIORITY CALLBACK_TASK_AUXILIARY
|
||||
#define TASK_PRIORITY CALLBACK_TASK_AUXILIARY
|
||||
|
||||
static bool module_enabled = false;
|
||||
static struct hott_bridge *hott;
|
||||
static int32_t uavoHoTTBridgeInitialize(void);
|
||||
static void uavoHoTTBridgeRxTask(void *parameters);
|
||||
static void uavoHoTTBridgeTxTask(void);
|
||||
static DelayedCallbackInfo *callbackHandle;
|
||||
static hottbridgemessage_handler ping_handler, ping_r_handler, pong_handler, pong_r_handler, fullstate_estimate_handler, imu_average_handler, gimbal_estimate_handler, flightcontrol_r_handler, pos_estimate_r_handler;
|
||||
static HoTTBridgeSettingsData settings;
|
||||
void AttitudeCb(__attribute__((unused)) UAVObjEvent *ev);
|
||||
void RateCb(__attribute__((unused)) UAVObjEvent *ev);
|
||||
|
||||
static hottbridgemessage_handler *const hottbridgemessagehandlers[HoTTBRIDGEMESSAGE_END_ARRAY_SIZE] = {
|
||||
ping_handler,
|
||||
NULL,
|
||||
NULL,
|
||||
NULL,
|
||||
pong_handler,
|
||||
fullstate_estimate_handler,
|
||||
imu_average_handler,
|
||||
gimbal_estimate_handler
|
||||
};
|
||||
|
||||
|
||||
/**
|
||||
* Process incoming bytes from an HoTT query thing.
|
||||
* @param[in] b received byte
|
||||
* @return true if we should continue processing bytes
|
||||
*/
|
||||
static void hott_receive_byte(struct hott_bridge *m, uint8_t b)
|
||||
{
|
||||
m->rx_buffer[m->rx_length] = b;
|
||||
m->rx_length++;
|
||||
hottbridgemessage_t *message = (hottbridgemessage_t *)m->rx_buffer;
|
||||
|
||||
// very simple parser - but not a state machine, just a few checks
|
||||
if (m->rx_length <= offsetof(hottbridgemessage_t, length)) {
|
||||
// check (partial) magic number - partial is important since we need to restart at any time if garbage is received
|
||||
uint32_t canary = 0xff;
|
||||
for (uint32_t t = 1; t < m->rx_length; t++) {
|
||||
canary = (canary << 8) || 0xff;
|
||||
}
|
||||
if ((message->magic & canary) != (HoTTBRIDGEMAGIC & canary)) {
|
||||
// parse error, not beginning of message
|
||||
goto rxfailure;
|
||||
}
|
||||
}
|
||||
if (m->rx_length == offsetof(hottbridgemessage_t, timestamp)) {
|
||||
if (message->length > (uint32_t)(HoTTBRIDGEMESSAGE_BUFFERSIZE - offsetof(hottbridgemessage_t, data))) {
|
||||
// parse error, no messages are that long
|
||||
goto rxfailure;
|
||||
}
|
||||
}
|
||||
if (m->rx_length == offsetof(hottbridgemessage_t, crc32)) {
|
||||
if (message->type >= HoTTBRIDGEMESSAGE_END_ARRAY_SIZE) {
|
||||
// parse error
|
||||
goto rxfailure;
|
||||
}
|
||||
if (message->length != HoTTBRIDGEMESSAGE_SIZES[message->type]) {
|
||||
// parse error
|
||||
goto rxfailure;
|
||||
}
|
||||
}
|
||||
if (m->rx_length < offsetof(hottbridgemessage_t, data)) {
|
||||
// not a parse failure, just not there yet
|
||||
return;
|
||||
}
|
||||
if (m->rx_length == offsetof(hottbridgemessage_t, data) + HoTTBRIDGEMESSAGE_SIZES[message->type]) {
|
||||
// complete message received and stored in pointer "message"
|
||||
// empty buffer for next message
|
||||
m->rx_length = 0;
|
||||
|
||||
if (PIOS_CRC32_updateCRC(0xffffffff, message->data, message->length) != message->crc32) {
|
||||
// crc mismatch
|
||||
goto rxfailure;
|
||||
}
|
||||
uint32_t rxpackets;
|
||||
HoTTBridgeStatusRxPacketsGet(&rxpackets);
|
||||
rxpackets++;
|
||||
HoTTBridgeStatusRxPacketsSet(&rxpackets);
|
||||
switch (message->type) {
|
||||
case HoTTBRIDGEMESSAGE_PING:
|
||||
ping_r_handler(m, message);
|
||||
break;
|
||||
case HoTTBRIDGEMESSAGE_POS_ESTIMATE:
|
||||
pos_estimate_r_handler(m, message);
|
||||
break;
|
||||
case HoTTBRIDGEMESSAGE_FLIGHTCONTROL:
|
||||
flightcontrol_r_handler(m, message);
|
||||
break;
|
||||
case HoTTBRIDGEMESSAGE_GIMBALCONTROL:
|
||||
// TODO implement gimbal control somehow
|
||||
break;
|
||||
case HoTTBRIDGEMESSAGE_PONG:
|
||||
pong_r_handler(m, message);
|
||||
break;
|
||||
default:
|
||||
// do nothing at all and discard the message
|
||||
break;
|
||||
}
|
||||
}
|
||||
return;
|
||||
|
||||
rxfailure:
|
||||
m->rx_length = 0;
|
||||
uint32_t rxfail;
|
||||
HoTTBridgeStatusRxFailGet(&rxfail);
|
||||
rxfail++;
|
||||
HoTTBridgeStatusRxFailSet(&rxfail);
|
||||
}
|
||||
|
||||
static uint32_t hwsettings_rosspeed_enum_to_baud(uint8_t baud)
|
||||
{
|
||||
switch (baud) {
|
||||
case HWSETTINGS_HoTTSPEED_2400:
|
||||
return 2400;
|
||||
|
||||
case HWSETTINGS_HoTTSPEED_4800:
|
||||
return 4800;
|
||||
|
||||
case HWSETTINGS_HoTTSPEED_9600:
|
||||
return 9600;
|
||||
|
||||
case HWSETTINGS_HoTTSPEED_19200:
|
||||
return 19200;
|
||||
|
||||
case HWSETTINGS_HoTTSPEED_38400:
|
||||
return 38400;
|
||||
|
||||
case HWSETTINGS_HoTTSPEED_57600:
|
||||
return 57600;
|
||||
|
||||
default:
|
||||
case HWSETTINGS_HoTTSPEED_115200:
|
||||
return 115200;
|
||||
}
|
||||
}
|
||||
// Private variables
|
||||
static bool module_enabled;
|
||||
static struct telemetrydata *telestate;
|
||||
static HoTTBridgeStatusData status;
|
||||
|
||||
// Private functions
|
||||
static void uavoHoTTBridgeTask(void *parameters);
|
||||
static uint16_t build_VARIO_message(struct hott_vario_message *msg);
|
||||
static uint16_t build_GPS_message(struct hott_gps_message *msg);
|
||||
static uint16_t build_GAM_message(struct hott_gam_message *msg);
|
||||
static uint16_t build_EAM_message(struct hott_eam_message *msg);
|
||||
static uint16_t build_ESC_message(struct hott_esc_message *msg);
|
||||
static uint16_t build_TEXT_message(struct hott_text_message *msg);
|
||||
static uint8_t calc_checksum(uint8_t *data, uint16_t size);
|
||||
static uint8_t generate_warning();
|
||||
static void update_telemetrydata();
|
||||
static void convert_long2gps(int32_t value, uint8_t *dir, uword_t *min, uword_t *sec);
|
||||
static uint8_t scale_float2uint8(float value, float scale, float offset);
|
||||
static int8_t scale_float2int8(float value, float scale, float offset);
|
||||
static uword_t scale_float2uword(float value, float scale, float offset);
|
||||
|
||||
/**
|
||||
* Module start routine automatically called after initialization routine
|
||||
@ -250,32 +93,20 @@ static uint32_t hwsettings_rosspeed_enum_to_baud(uint8_t baud)
|
||||
*/
|
||||
static int32_t uavoHoTTBridgeStart(void)
|
||||
{
|
||||
if (!module_enabled) {
|
||||
// give port to telemetry if it doesn't have one
|
||||
// stops board getting stuck in condition where it can't be connected to gcs
|
||||
if (!PIOS_COM_TELEM_RF) {
|
||||
PIOS_COM_TELEM_RF = PIOS_COM_HoTT;
|
||||
}
|
||||
status.TxPackets = 0;
|
||||
status.RxPackets = 0;
|
||||
status.TxFail = 0;
|
||||
status.RxFail = 0;
|
||||
|
||||
return -1;
|
||||
|
||||
// Start task
|
||||
if (module_enabled) {
|
||||
xTaskHandle taskHandle;
|
||||
|
||||
xTaskCreate(uavoHoTTBridgeTask, "uavoHoTTBridge", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle);
|
||||
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_UAVOHOTTBRIDGE, taskHandle);
|
||||
}
|
||||
|
||||
PIOS_DELTATIME_Init(&ros->roundtrip, 1e-3f, 1e-6f, 10.0f, 1e-1f);
|
||||
ros->pingTimer = 0;
|
||||
ros->stateTimer = 0;
|
||||
ros->rateTimer = 0;
|
||||
ros->rateAccumulator[0] = 0;
|
||||
ros->rateAccumulator[1] = 0;
|
||||
ros->rateAccumulator[2] = 0;
|
||||
ros->rx_length = 0;
|
||||
ros->myPingSequence = 0x66;
|
||||
|
||||
xTaskHandle taskHandle;
|
||||
|
||||
xTaskCreate(uavoHoTTBridgeRxTask, "uavoHoTTBridge", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle);
|
||||
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_UAVOHoTTBRIDGE, taskHandle);
|
||||
PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
@ -285,304 +116,846 @@ static int32_t uavoHoTTBridgeStart(void)
|
||||
*/
|
||||
static int32_t uavoHoTTBridgeInitialize(void)
|
||||
{
|
||||
if (PIOS_COM_HoTT) {
|
||||
ros = pios_malloc(sizeof(*ros));
|
||||
if (ros != NULL) {
|
||||
memset(ros, 0x00, sizeof(*ros));
|
||||
if (PIOS_COM_HOTT) {
|
||||
module_enabled = true;
|
||||
// HoTT telemetry baudrate is fixed to 19200
|
||||
|
||||
ros->com = PIOS_COM_HoTT;
|
||||
PIOS_COM_ChangeBaud(PIOS_COM_HOTT, 19200);
|
||||
PIOS_COM_SetHalfDuplex(PIOS_COM_HOTT, true);
|
||||
HoTTBridgeSettingsInitialize();
|
||||
HoTTBridgeStatusInitialize();
|
||||
|
||||
HoTTBridgeStatusInitialize();
|
||||
AUXPositionSensorInitialize();
|
||||
HwSettingsInitialize();
|
||||
HwSettingsHoTTSpeedOptions rosSpeed;
|
||||
HwSettingsHoTTSpeedGet(&rosSpeed);
|
||||
// allocate memory for telemetry data
|
||||
telestate = (struct telemetrydata *)pios_malloc(sizeof(*telestate));
|
||||
|
||||
PIOS_COM_ChangeBaud(PIOS_COM_HoTT, 19200); // HoTT uses 19200 only
|
||||
// TODO: set COM port to 1-wire here, once PIOS_COM_ioctl is implemented
|
||||
callbackHandle = PIOS_CALLBACKSCHEDULER_Create(&uavoHoTTBridgeTxTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_UAVOHoTTBRIDGE, STACK_SIZE_BYTES);
|
||||
//VelocityStateInitialize();
|
||||
//PositionStateInitialize();
|
||||
//AttitudeStateInitialize();
|
||||
//AttitudeStateConnectCallback(&AttitudeCb);
|
||||
//GyroStateInitialize();
|
||||
//GyroStateConnectCallback(&RateCb);
|
||||
//FlightStatusInitialize();
|
||||
//PathDesiredInitialize();
|
||||
//PoiLocationInitialize();
|
||||
//ActuatorDesiredInitialize();
|
||||
//FlightModeSettingsInitialize();
|
||||
//ManualControlCommandInitialize();
|
||||
//AccessoryDesiredInitialize();
|
||||
|
||||
module_enabled = true;
|
||||
return 0;
|
||||
if (telestate == NULL) {
|
||||
// there is not enough free memory. the module could not run.
|
||||
module_enabled = false;
|
||||
return -1;
|
||||
}
|
||||
} else {
|
||||
module_enabled = false;
|
||||
}
|
||||
|
||||
return -1;
|
||||
return 0;
|
||||
}
|
||||
MODULE_INITCALL(uavoHoTTBridgeInitialize, uavoHoTTBridgeStart);
|
||||
|
||||
/** various handlers **/
|
||||
static void ping_r_handler(struct ros_bridge *rb, rosbridgemessage_t *m)
|
||||
{
|
||||
rosbridgemessage_pingpong_t *data = (rosbridgemessage_pingpong_t *)&(m->data);
|
||||
|
||||
rb->remotePingSequence = data->sequence_number;
|
||||
rb->scheduled[HoTTBRIDGEMESSAGE_PONG] = true;
|
||||
PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle);
|
||||
}
|
||||
|
||||
static void ping_handler(struct ros_bridge *rb, rosbridgemessage_t *m)
|
||||
{
|
||||
rosbridgemessage_pingpong_t *data = (rosbridgemessage_pingpong_t *)&(m->data);
|
||||
|
||||
data->sequence_number = ++rb->myPingSequence;
|
||||
rb->roundtrip.last = PIOS_DELAY_GetRaw();
|
||||
}
|
||||
|
||||
static void pong_r_handler(struct ros_bridge *rb, rosbridgemessage_t *m)
|
||||
{
|
||||
rosbridgemessage_pingpong_t *data = (rosbridgemessage_pingpong_t *)&(m->data);
|
||||
|
||||
if (data->sequence_number != rb->myPingSequence) {
|
||||
return;
|
||||
}
|
||||
float roundtrip = PIOS_DELTATIME_GetAverageSeconds(&(rb->roundtrip));
|
||||
HoTTBridgeStatusPingRoundTripTimeSet(&roundtrip);
|
||||
}
|
||||
|
||||
static void flightcontrol_r_handler(__attribute__((unused)) struct ros_bridge *rb, rosbridgemessage_t *m)
|
||||
{
|
||||
rosbridgemessage_flightcontrol_t *data = (rosbridgemessage_flightcontrol_t *)&(m->data);
|
||||
FlightStatusFlightModeOptions mode;
|
||||
|
||||
FlightStatusFlightModeGet(&mode);
|
||||
if (mode != FLIGHTSTATUS_FLIGHTMODE_HoTTCONTROLLED) {
|
||||
return;
|
||||
}
|
||||
PathDesiredData pathDesired;
|
||||
PathDesiredGet(&pathDesired);
|
||||
switch (data->mode) {
|
||||
case HoTTBRIDGEMESSAGE_FLIGHTCONTROL_MODE_WAYPOINT:
|
||||
{
|
||||
FlightModeSettingsPositionHoldOffsetData offset;
|
||||
FlightModeSettingsPositionHoldOffsetGet(&offset);
|
||||
pathDesired.End.North = boundf(data->control[0], settings.GeoFenceBoxMin.North, settings.GeoFenceBoxMax.North);
|
||||
pathDesired.End.East = boundf(data->control[1], settings.GeoFenceBoxMin.East, settings.GeoFenceBoxMax.East);
|
||||
pathDesired.End.Down = boundf(data->control[2], settings.GeoFenceBoxMin.Down, settings.GeoFenceBoxMax.Down);
|
||||
pathDesired.Start.North = pathDesired.End.North + offset.Horizontal;
|
||||
pathDesired.Start.East = pathDesired.End.East;
|
||||
pathDesired.Start.Down = pathDesired.End.Down;
|
||||
pathDesired.StartingVelocity = 0.0f;
|
||||
pathDesired.EndingVelocity = 0.0f;
|
||||
pathDesired.Mode = PATHDESIRED_MODE_GOTOENDPOINT;
|
||||
}
|
||||
break;
|
||||
case HoTTBRIDGEMESSAGE_FLIGHTCONTROL_MODE_ATTITUDE:
|
||||
{
|
||||
pathDesired.ModeParameters[0] = data->control[0];
|
||||
pathDesired.ModeParameters[1] = data->control[1];
|
||||
pathDesired.ModeParameters[2] = data->control[2];
|
||||
pathDesired.ModeParameters[3] = data->control[3];
|
||||
pathDesired.Mode = PATHDESIRED_MODE_FIXEDATTITUDE;
|
||||
}
|
||||
break;
|
||||
}
|
||||
PathDesiredSet(&pathDesired);
|
||||
PoiLocationData poiLocation;
|
||||
PoiLocationGet(&poiLocation);
|
||||
poiLocation.North = data->poi[0];
|
||||
poiLocation.East = data->poi[1];
|
||||
poiLocation.Down = data->poi[2];
|
||||
PoiLocationSet(&poiLocation);
|
||||
}
|
||||
static void pos_estimate_r_handler(__attribute__((unused)) struct ros_bridge *rb, rosbridgemessage_t *m)
|
||||
{
|
||||
rosbridgemessage_pos_estimate_t *data = (rosbridgemessage_pos_estimate_t *)&(m->data);
|
||||
AUXPositionSensorData pos;
|
||||
|
||||
pos.North = data->position[0];
|
||||
pos.East = data->position[1];
|
||||
pos.Down = data->position[2];
|
||||
AUXPositionSensorSet(&pos);
|
||||
}
|
||||
|
||||
static void pong_handler(struct ros_bridge *rb, rosbridgemessage_t *m)
|
||||
{
|
||||
rosbridgemessage_pingpong_t *data = (rosbridgemessage_pingpong_t *)&(m->data);
|
||||
|
||||
|
||||
data->sequence_number = rb->remotePingSequence;
|
||||
}
|
||||
|
||||
static void fullstate_estimate_handler(__attribute__((unused)) struct ros_bridge *rb, rosbridgemessage_t *m)
|
||||
{
|
||||
PositionStateData pos;
|
||||
VelocityStateData vel;
|
||||
AttitudeStateData att;
|
||||
FlightStatusFlightModeOptions mode;
|
||||
FlightStatusArmedOptions armed;
|
||||
float thrust;
|
||||
AccessoryDesiredData accessory;
|
||||
ManualControlCommandData manualcontrol;
|
||||
|
||||
ManualControlCommandGet(&manualcontrol);
|
||||
FlightStatusArmedGet(&armed);
|
||||
FlightStatusFlightModeGet(&mode);
|
||||
ActuatorDesiredThrustGet(&thrust);
|
||||
PositionStateGet(&pos);
|
||||
VelocityStateGet(&vel);
|
||||
AttitudeStateGet(&att);
|
||||
rosbridgemessage_fullstate_estimate_t *data = (rosbridgemessage_fullstate_estimate_t *)&(m->data);
|
||||
data->quaternion[0] = att.q1;
|
||||
data->quaternion[1] = att.q2;
|
||||
data->quaternion[2] = att.q3;
|
||||
data->quaternion[3] = att.q4;
|
||||
data->position[0] = pos.North;
|
||||
data->position[1] = pos.East;
|
||||
data->position[2] = pos.Down;
|
||||
data->velocity[0] = vel.North;
|
||||
data->velocity[1] = vel.East;
|
||||
data->velocity[2] = vel.Down;
|
||||
data->rotation[0] = ros->rateAccumulator[0];
|
||||
data->rotation[1] = ros->rateAccumulator[1];
|
||||
data->rotation[2] = ros->rateAccumulator[2];
|
||||
data->thrust = thrust;
|
||||
data->HoTTControlled = (mode == FLIGHTSTATUS_FLIGHTMODE_HoTTCONTROLLED) ? 1 : 0;
|
||||
data->FlightMode = manualcontrol.FlightModeSwitchPosition;
|
||||
data->armed = (armed == FLIGHTSTATUS_ARMED_ARMED) ? 1 : 0;
|
||||
data->controls[0] = manualcontrol.Roll;
|
||||
data->controls[1] = manualcontrol.Pitch;
|
||||
data->controls[2] = manualcontrol.Yaw;
|
||||
data->controls[3] = manualcontrol.Thrust;
|
||||
data->controls[4] = manualcontrol.Collective;
|
||||
data->controls[5] = manualcontrol.Throttle;
|
||||
for (int t = 0; t < 4; t++) {
|
||||
if (AccessoryDesiredInstGet(t, &accessory) == 0) {
|
||||
data->accessory[t] = accessory.AccessoryVal;
|
||||
}
|
||||
}
|
||||
|
||||
int x, y;
|
||||
float *P[13];
|
||||
INSGetPAddress(P);
|
||||
for (x = 0; x < 10; x++) {
|
||||
for (y = 0; y < 10; y++) {
|
||||
data->matrix[x * 10 + y] = P[x][y];
|
||||
}
|
||||
}
|
||||
if (ros->rateTimer >= 1) {
|
||||
float factor = 1.0f / (float)ros->rateTimer;
|
||||
data->rotation[0] *= factor;
|
||||
data->rotation[1] *= factor;
|
||||
data->rotation[2] *= factor;
|
||||
ros->rateAccumulator[0] = 0;
|
||||
ros->rateAccumulator[1] = 0;
|
||||
ros->rateAccumulator[2] = 0;
|
||||
ros->rateTimer = 0;
|
||||
}
|
||||
}
|
||||
static void imu_average_handler(__attribute__((unused)) struct ros_bridge *rb, __attribute__((unused)) rosbridgemessage_t *m)
|
||||
{
|
||||
// TODO
|
||||
}
|
||||
static void gimbal_estimate_handler(__attribute__((unused)) struct ros_bridge *rb, __attribute__((unused)) rosbridgemessage_t *m)
|
||||
{
|
||||
// TODO
|
||||
}
|
||||
|
||||
/**
|
||||
* Main task routine
|
||||
* @param[in] parameters parameter given by PIOS_Callback_Create()
|
||||
* Main task. It does not return.
|
||||
*/
|
||||
static void uavoHoTTBridgeTxTask(void)
|
||||
static void uavoHoTTBridgeTask(__attribute__((unused)) void *parameters)
|
||||
{
|
||||
uint8_t buffer[HoTTBRIDGEMESSAGE_BUFFERSIZE]; // buffer on the stack? could also be in static RAM but not reuseale by other callbacks then
|
||||
rosbridgemessage_t *message = (rosbridgemessage_t *)buffer;
|
||||
uint8_t rx_buffer[2];
|
||||
uint8_t tx_buffer[HOTT_MAX_MESSAGE_LENGTH];
|
||||
uint16_t message_size;
|
||||
|
||||
for (rosbridgemessagetype_t type = HoTTBRIDGEMESSAGE_PING; type < HoTTBRIDGEMESSAGE_END_ARRAY_SIZE; type++) {
|
||||
if (ros->scheduled[type] && rosbridgemessagehandlers[type] != NULL) {
|
||||
message->magic = HoTTBRIDGEMAGIC;
|
||||
message->length = HoTTBRIDGEMESSAGE_SIZES[type];
|
||||
message->type = type;
|
||||
message->timestamp = PIOS_DELAY_GetuS();
|
||||
(*rosbridgemessagehandlers[type])(ros, message);
|
||||
message->crc32 = PIOS_CRC32_updateCRC(0xffffffff, message->data, message->length);
|
||||
int32_t ret = PIOS_COM_SendBufferNonBlocking(ros->com, buffer, offsetof(rosbridgemessage_t, data) + message->length);
|
||||
// int32_t ret = PIOS_COM_SendBuffer(ros->com, buffer, offsetof(rosbridgemessage_t, data) + message->length);
|
||||
ros->scheduled[type] = false;
|
||||
if (ret >= 0) {
|
||||
uint32_t txpackets;
|
||||
HoTTBridgeStatusTxPacketsGet(&txpackets);
|
||||
txpackets++;
|
||||
HoTTBridgeStatusTxPacketsSet(&txpackets);
|
||||
} else {
|
||||
uint32_t txfail;
|
||||
HoTTBridgeStatusTxFailGet(&txfail);
|
||||
txfail++;
|
||||
HoTTBridgeStatusTxFailSet(&txfail);
|
||||
}
|
||||
PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle);
|
||||
return;
|
||||
}
|
||||
}
|
||||
// nothing scheduled, do a ping in 10 secods time
|
||||
}
|
||||
// clear all state values
|
||||
memset(telestate, 0, sizeof(*telestate));
|
||||
|
||||
/**
|
||||
* Event Callback on Gyro updates (called with PIOS_SENSOR_RATE - roughly 500 Hz - from State Estimation)
|
||||
*/
|
||||
void RateCb(__attribute__((unused)) UAVObjEvent *ev)
|
||||
{
|
||||
GyroStateData gyr;
|
||||
// initialize timer variables
|
||||
portTickType lastSysTime = xTaskGetTickCount();
|
||||
// idle delay between telemetry request and answer
|
||||
uint32_t idledelay = IDLE_TIME;
|
||||
// data delay between transmitted bytes
|
||||
uint32_t datadelay = DATA_TIME;
|
||||
|
||||
GyroStateGet(&gyr);
|
||||
ros->rateAccumulator[0] += gyr.x;
|
||||
ros->rateAccumulator[1] += gyr.y;
|
||||
ros->rateAccumulator[2] += gyr.z;
|
||||
ros->rateTimer++;
|
||||
}
|
||||
|
||||
/**
|
||||
* Event Callback on Attitude updates (called with PIOS_SENSOR_RATE - roughly 500 Hz - from State Estimation)
|
||||
*/
|
||||
void AttitudeCb(__attribute__((unused)) UAVObjEvent *ev)
|
||||
{
|
||||
bool dispatch = false;
|
||||
|
||||
if (++ros->pingTimer >= settings.UpdateRate.Ping && settings.UpdateRate.Ping > 0) {
|
||||
ros->pingTimer = 0;
|
||||
dispatch = true;
|
||||
ros->scheduled[HoTTBRIDGEMESSAGE_PING] = true;
|
||||
}
|
||||
if (++ros->stateTimer >= settings.UpdateRate.State && settings.UpdateRate.State > 0) {
|
||||
ros->stateTimer = 0;
|
||||
dispatch = true;
|
||||
ros->scheduled[HoTTBRIDGEMESSAGE_FULLSTATE_ESTIMATE] = true;
|
||||
}
|
||||
if (dispatch && callbackHandle) {
|
||||
PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Main task routine
|
||||
* @param[in] parameters parameter given by PIOS_Thread_Create()
|
||||
*/
|
||||
static void uavoHoTTBridgeRxTask(__attribute__((unused)) void *parameters)
|
||||
{
|
||||
// work on hott telemetry. endless loop.
|
||||
while (1) {
|
||||
uint8_t b = 0;
|
||||
uint16_t count = PIOS_COM_ReceiveBuffer(ros->com, &b, 1, ~0);
|
||||
if (count) {
|
||||
ros_receive_byte(ros, b);
|
||||
// clear message size on every loop before processing
|
||||
message_size = 0;
|
||||
|
||||
// shift receiver buffer. make room for one byte.
|
||||
rx_buffer[1] = rx_buffer[0];
|
||||
|
||||
// wait for a byte of telemetry request in data delay interval
|
||||
while (PIOS_COM_ReceiveBuffer(PIOS_COM_HOTT, rx_buffer, 1, 0) == 0) {
|
||||
vTaskDelayUntil(&lastSysTime, datadelay / portTICK_RATE_MS);
|
||||
}
|
||||
// set start trigger point
|
||||
lastSysTime = xTaskGetTickCount();
|
||||
|
||||
// examine received data stream
|
||||
if (rx_buffer[1] == HOTT_BINARY_ID) {
|
||||
// first received byte looks like a binary request. check second received byte for a sensor id.
|
||||
switch (rx_buffer[0]) {
|
||||
case HOTT_VARIO_ID:
|
||||
message_size = build_VARIO_message((struct hott_vario_message *)tx_buffer);
|
||||
break;
|
||||
case HOTT_GPS_ID:
|
||||
message_size = build_GPS_message((struct hott_gps_message *)tx_buffer);
|
||||
break;
|
||||
case HOTT_GAM_ID:
|
||||
message_size = build_GAM_message((struct hott_gam_message *)tx_buffer);
|
||||
break;
|
||||
case HOTT_EAM_ID:
|
||||
message_size = build_EAM_message((struct hott_eam_message *)tx_buffer);
|
||||
break;
|
||||
case HOTT_ESC_ID:
|
||||
message_size = build_ESC_message((struct hott_esc_message *)tx_buffer);
|
||||
break;
|
||||
default:
|
||||
message_size = 0;
|
||||
}
|
||||
} else if (rx_buffer[1] == HOTT_TEXT_ID) {
|
||||
// first received byte looks like a text request. check second received byte for a valid button.
|
||||
switch (rx_buffer[0]) {
|
||||
case HOTT_BUTTON_DEC:
|
||||
case HOTT_BUTTON_INC:
|
||||
case HOTT_BUTTON_SET:
|
||||
case HOTT_BUTTON_NIL:
|
||||
case HOTT_BUTTON_NEXT:
|
||||
case HOTT_BUTTON_PREV:
|
||||
message_size = build_TEXT_message((struct hott_text_message *)tx_buffer);
|
||||
break;
|
||||
default:
|
||||
message_size = 0;
|
||||
}
|
||||
}
|
||||
|
||||
// check if a message is in the transmit buffer.
|
||||
if (message_size > 0) {
|
||||
status.RxPackets++;
|
||||
|
||||
// check idle line before transmit. pause, then check receiver buffer
|
||||
vTaskDelayUntil(&lastSysTime, idledelay / portTICK_RATE_MS);
|
||||
|
||||
if (PIOS_COM_ReceiveBuffer(PIOS_COM_HOTT, rx_buffer, 1, 0) == 0) {
|
||||
// nothing received means idle line. ready to transmit the requested message
|
||||
for (int i = 0; i < message_size; i++) {
|
||||
// send message content with pause between each byte
|
||||
PIOS_COM_SendCharNonBlocking(PIOS_COM_HOTT, tx_buffer[i]);
|
||||
// grab possible incoming loopback data and throw it away
|
||||
PIOS_COM_ReceiveBuffer(PIOS_COM_HOTT, rx_buffer, sizeof(rx_buffer), 0);
|
||||
vTaskDelayUntil(&lastSysTime, datadelay / portTICK_RATE_MS);
|
||||
}
|
||||
status.TxPackets++;
|
||||
|
||||
|
||||
// after transmitting the message, any loopback data needs to be cleaned up.
|
||||
vTaskDelayUntil(&lastSysTime, idledelay / portTICK_RATE_MS);
|
||||
PIOS_COM_ReceiveBuffer(PIOS_COM_HOTT, tx_buffer, message_size, 0);
|
||||
} else {
|
||||
status.RxFail++;
|
||||
}
|
||||
HoTTBridgeStatusSet(&status);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#endif // PIOS_INCLUDE_HoTT_BRIDGE
|
||||
/**
|
||||
* Build requested answer messages.
|
||||
* \return value sets message size
|
||||
*/
|
||||
uint16_t build_VARIO_message(struct hott_vario_message *msg)
|
||||
{
|
||||
update_telemetrydata();
|
||||
|
||||
if (telestate->Settings.Sensor.VARIO == HOTTBRIDGESETTINGS_SENSOR_DISABLED) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
// clear message buffer
|
||||
memset(msg, 0, sizeof(*msg));
|
||||
|
||||
// message header
|
||||
msg->start = HOTT_START;
|
||||
msg->stop = HOTT_STOP;
|
||||
msg->sensor_id = HOTT_VARIO_ID;
|
||||
msg->warning = generate_warning();
|
||||
msg->sensor_text_id = HOTT_VARIO_TEXT_ID;
|
||||
|
||||
// alarm inverse bits. invert display areas on limits
|
||||
msg->alarm_inverse |= (telestate->Settings.Limit.MinHeight > telestate->altitude) ? VARIO_INVERT_ALT : 0;
|
||||
msg->alarm_inverse |= (telestate->Settings.Limit.MaxHeight < telestate->altitude) ? VARIO_INVERT_ALT : 0;
|
||||
msg->alarm_inverse |= (telestate->Settings.Limit.MaxHeight < telestate->altitude) ? VARIO_INVERT_MAX : 0;
|
||||
msg->alarm_inverse |= (telestate->Settings.Limit.MinHeight > telestate->altitude) ? VARIO_INVERT_MIN : 0;
|
||||
msg->alarm_inverse |= (telestate->Settings.Limit.NegDifference1 > telestate->climbrate1s) ? VARIO_INVERT_CR1S : 0;
|
||||
msg->alarm_inverse |= (telestate->Settings.Limit.PosDifference1 < telestate->climbrate1s) ? VARIO_INVERT_CR1S : 0;
|
||||
msg->alarm_inverse |= (telestate->Settings.Limit.NegDifference2 > telestate->climbrate3s) ? VARIO_INVERT_CR3S : 0;
|
||||
msg->alarm_inverse |= (telestate->Settings.Limit.PosDifference2 < telestate->climbrate3s) ? VARIO_INVERT_CR3S : 0;
|
||||
msg->alarm_inverse |= (telestate->Settings.Limit.NegDifference2 > telestate->climbrate10s) ? VARIO_INVERT_CR10S : 0;
|
||||
msg->alarm_inverse |= (telestate->Settings.Limit.PosDifference2 < telestate->climbrate10s) ? VARIO_INVERT_CR10S : 0;
|
||||
|
||||
// altitude relative to ground
|
||||
msg->altitude = scale_float2uword(telestate->altitude, 1, OFFSET_ALTITUDE);
|
||||
msg->min_altitude = scale_float2uword(telestate->min_altitude, 1, OFFSET_ALTITUDE);
|
||||
msg->max_altitude = scale_float2uword(telestate->max_altitude, 1, OFFSET_ALTITUDE);
|
||||
|
||||
// climbrate
|
||||
msg->climbrate = scale_float2uword(telestate->climbrate1s, M_TO_CM, OFFSET_CLIMBRATE);
|
||||
msg->climbrate3s = scale_float2uword(telestate->climbrate3s, M_TO_CM, OFFSET_CLIMBRATE);
|
||||
msg->climbrate10s = scale_float2uword(telestate->climbrate10s, M_TO_CM, OFFSET_CLIMBRATE);
|
||||
|
||||
// compass
|
||||
msg->compass = scale_float2int8(telestate->Attitude.Yaw, DEG_TO_UINT, 0);
|
||||
|
||||
// statusline
|
||||
memcpy(msg->ascii, telestate->statusline, sizeof(msg->ascii));
|
||||
|
||||
// free display characters
|
||||
msg->ascii1 = 0;
|
||||
msg->ascii2 = 0;
|
||||
msg->ascii3 = 0;
|
||||
|
||||
msg->checksum = calc_checksum((uint8_t *)msg, sizeof(*msg));
|
||||
return sizeof(*msg);
|
||||
}
|
||||
|
||||
uint16_t build_GPS_message(struct hott_gps_message *msg)
|
||||
{
|
||||
update_telemetrydata();
|
||||
|
||||
if (telestate->Settings.Sensor.GPS == HOTTBRIDGESETTINGS_SENSOR_DISABLED) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
// clear message buffer
|
||||
memset(msg, 0, sizeof(*msg));
|
||||
|
||||
// message header
|
||||
msg->start = HOTT_START;
|
||||
msg->stop = HOTT_STOP;
|
||||
msg->sensor_id = HOTT_GPS_ID;
|
||||
msg->warning = generate_warning();
|
||||
msg->sensor_text_id = HOTT_GPS_TEXT_ID;
|
||||
|
||||
// alarm inverse bits. invert display areas on limits
|
||||
msg->alarm_inverse1 |= (telestate->Settings.Limit.MaxDistance < telestate->homedistance) ? GPS_INVERT_HDIST : 0;
|
||||
msg->alarm_inverse1 |= (telestate->Settings.Limit.MinSpeed > telestate->GPS.Groundspeed) ? GPS_INVERT_SPEED : 0;
|
||||
msg->alarm_inverse1 |= (telestate->Settings.Limit.MaxSpeed < telestate->GPS.Groundspeed) ? GPS_INVERT_SPEED : 0;
|
||||
msg->alarm_inverse1 |= (telestate->Settings.Limit.MinHeight > telestate->altitude) ? GPS_INVERT_ALT : 0;
|
||||
msg->alarm_inverse1 |= (telestate->Settings.Limit.MaxHeight < telestate->altitude) ? GPS_INVERT_ALT : 0;
|
||||
msg->alarm_inverse1 |= (telestate->Settings.Limit.NegDifference1 > telestate->climbrate1s) ? GPS_INVERT_CR1S : 0;
|
||||
msg->alarm_inverse1 |= (telestate->Settings.Limit.PosDifference1 < telestate->climbrate1s) ? GPS_INVERT_CR1S : 0;
|
||||
msg->alarm_inverse1 |= (telestate->Settings.Limit.NegDifference2 > telestate->climbrate3s) ? GPS_INVERT_CR3S : 0;
|
||||
msg->alarm_inverse1 |= (telestate->Settings.Limit.PosDifference2 < telestate->climbrate3s) ? GPS_INVERT_CR3S : 0;
|
||||
msg->alarm_inverse2 |= (telestate->SysAlarms.Alarm.GPS != SYSTEMALARMS_ALARM_OK) ? GPS_INVERT2_POS : 0;
|
||||
|
||||
// gps direction, groundspeed and postition
|
||||
msg->flight_direction = scale_float2uint8(telestate->GPS.Heading, DEG_TO_UINT, 0);
|
||||
msg->gps_speed = scale_float2uword(telestate->GPS.Groundspeed, MS_TO_KMH, 0);
|
||||
convert_long2gps(telestate->GPS.Latitude, &msg->latitude_ns, &msg->latitude_min, &msg->latitude_sec);
|
||||
convert_long2gps(telestate->GPS.Longitude, &msg->longitude_ew, &msg->longitude_min, &msg->longitude_sec);
|
||||
|
||||
// homelocation distance, course and state
|
||||
msg->distance = scale_float2uword(telestate->homedistance, 1, 0);
|
||||
msg->home_direction = scale_float2uint8(telestate->homecourse, DEG_TO_UINT, 0);
|
||||
msg->ascii5 = (telestate->Home.Set ? 'H' : '-');
|
||||
|
||||
// altitude relative to ground and climb rate
|
||||
msg->altitude = scale_float2uword(telestate->altitude, 1, OFFSET_ALTITUDE);
|
||||
msg->climbrate = scale_float2uword(telestate->climbrate1s, M_TO_CM, OFFSET_CLIMBRATE);
|
||||
msg->climbrate3s = scale_float2uint8(telestate->climbrate3s, 1, OFFSET_CLIMBRATE3S);
|
||||
|
||||
// number of satellites,gps fix and state
|
||||
msg->gps_num_sat = telestate->GPS.Satellites;
|
||||
switch (telestate->GPS.Status) {
|
||||
case GPSPOSITIONSENSOR_STATUS_FIX2D:
|
||||
msg->gps_fix_char = '2';
|
||||
break;
|
||||
case GPSPOSITIONSENSOR_STATUS_FIX3D:
|
||||
msg->gps_fix_char = '3';
|
||||
break;
|
||||
default:
|
||||
msg->gps_fix_char = 0;
|
||||
}
|
||||
switch (telestate->SysAlarms.Alarm.GPS) {
|
||||
case SYSTEMALARMS_ALARM_UNINITIALISED:
|
||||
msg->ascii6 = 0;
|
||||
// if there is no gps, show compass flight direction
|
||||
msg->flight_direction = scale_float2int8((telestate->Attitude.Yaw > 0) ? telestate->Attitude.Yaw : 360 + telestate->Attitude.Yaw, DEG_TO_UINT, 0);
|
||||
break;
|
||||
case SYSTEMALARMS_ALARM_OK:
|
||||
msg->ascii6 = '.';
|
||||
break;
|
||||
case SYSTEMALARMS_ALARM_WARNING:
|
||||
msg->ascii6 = '?';
|
||||
break;
|
||||
case SYSTEMALARMS_ALARM_ERROR:
|
||||
case SYSTEMALARMS_ALARM_CRITICAL:
|
||||
msg->ascii6 = '!';
|
||||
break;
|
||||
default:
|
||||
msg->ascii6 = 0;
|
||||
}
|
||||
|
||||
// model angles
|
||||
msg->angle_roll = scale_float2int8(telestate->Attitude.Roll, DEG_TO_UINT, 0);
|
||||
msg->angle_nick = scale_float2int8(telestate->Attitude.Pitch, DEG_TO_UINT, 0);
|
||||
msg->angle_compass = scale_float2int8(telestate->Attitude.Yaw, DEG_TO_UINT, 0);
|
||||
|
||||
// gps time
|
||||
msg->gps_hour = telestate->GPStime.Hour;
|
||||
msg->gps_min = telestate->GPStime.Minute;
|
||||
msg->gps_sec = telestate->GPStime.Second;
|
||||
msg->gps_msec = 0;
|
||||
|
||||
// gps MSL (NN) altitude MSL
|
||||
msg->msl = scale_float2uword(telestate->GPS.Altitude, 1, 0);
|
||||
|
||||
// free display chararacter
|
||||
msg->ascii4 = 0;
|
||||
|
||||
msg->checksum = calc_checksum((uint8_t *)msg, sizeof(*msg));
|
||||
return sizeof(*msg);
|
||||
}
|
||||
|
||||
uint16_t build_GAM_message(struct hott_gam_message *msg)
|
||||
{
|
||||
update_telemetrydata();
|
||||
|
||||
if (telestate->Settings.Sensor.GAM == HOTTBRIDGESETTINGS_SENSOR_DISABLED) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
// clear message buffer
|
||||
memset(msg, 0, sizeof(*msg));
|
||||
|
||||
// message header
|
||||
msg->start = HOTT_START;
|
||||
msg->stop = HOTT_STOP;
|
||||
msg->sensor_id = HOTT_GAM_ID;
|
||||
msg->warning = generate_warning();
|
||||
msg->sensor_text_id = HOTT_GAM_TEXT_ID;
|
||||
|
||||
// alarm inverse bits. invert display areas on limits
|
||||
msg->alarm_inverse2 |= (telestate->Settings.Limit.MaxCurrent < telestate->Battery.Current) ? GAM_INVERT2_CURRENT : 0;
|
||||
msg->alarm_inverse2 |= (telestate->Settings.Limit.MinPowerVoltage > telestate->Battery.Voltage) ? GAM_INVERT2_VOLTAGE : 0;
|
||||
msg->alarm_inverse2 |= (telestate->Settings.Limit.MaxPowerVoltage < telestate->Battery.Voltage) ? GAM_INVERT2_VOLTAGE : 0;
|
||||
msg->alarm_inverse2 |= (telestate->Settings.Limit.MinHeight > telestate->altitude) ? GAM_INVERT2_ALT : 0;
|
||||
msg->alarm_inverse2 |= (telestate->Settings.Limit.MaxHeight < telestate->altitude) ? GAM_INVERT2_ALT : 0;
|
||||
msg->alarm_inverse2 |= (telestate->Settings.Limit.NegDifference1 > telestate->climbrate1s) ? GAM_INVERT2_CR1S : 0;
|
||||
msg->alarm_inverse2 |= (telestate->Settings.Limit.PosDifference1 < telestate->climbrate1s) ? GAM_INVERT2_CR1S : 0;
|
||||
msg->alarm_inverse2 |= (telestate->Settings.Limit.NegDifference2 > telestate->climbrate3s) ? GAM_INVERT2_CR3S : 0;
|
||||
msg->alarm_inverse2 |= (telestate->Settings.Limit.PosDifference2 < telestate->climbrate3s) ? GAM_INVERT2_CR3S : 0;
|
||||
|
||||
// temperatures
|
||||
msg->temperature1 = scale_float2uint8(telestate->Gyro.temperature, 1, OFFSET_TEMPERATURE);
|
||||
msg->temperature2 = scale_float2uint8(telestate->Baro.Temperature, 1, OFFSET_TEMPERATURE);
|
||||
|
||||
// altitude
|
||||
msg->altitude = scale_float2uword(telestate->altitude, 1, OFFSET_ALTITUDE);
|
||||
|
||||
// climbrate
|
||||
msg->climbrate = scale_float2uword(telestate->climbrate1s, M_TO_CM, OFFSET_CLIMBRATE);
|
||||
msg->climbrate3s = scale_float2uint8(telestate->climbrate3s, 1, OFFSET_CLIMBRATE3S);
|
||||
|
||||
// main battery
|
||||
float voltage = (telestate->Battery.Voltage > 0) ? telestate->Battery.Voltage : 0;
|
||||
float current = (telestate->Battery.Current > 0) ? telestate->Battery.Current : 0;
|
||||
float energy = (telestate->Battery.ConsumedEnergy > 0) ? telestate->Battery.ConsumedEnergy : 0;
|
||||
msg->voltage = scale_float2uword(voltage, 10, 0);
|
||||
msg->current = scale_float2uword(current, 10, 0);
|
||||
msg->capacity = scale_float2uword(energy, 0.1f, 0);
|
||||
|
||||
// pressure kPa to 0.1Bar
|
||||
msg->pressure = scale_float2uint8(telestate->Baro.Pressure, 0.1f, 0);
|
||||
|
||||
msg->checksum = calc_checksum((uint8_t *)msg, sizeof(*msg));
|
||||
return sizeof(*msg);
|
||||
}
|
||||
|
||||
uint16_t build_EAM_message(struct hott_eam_message *msg)
|
||||
{
|
||||
update_telemetrydata();
|
||||
|
||||
if (telestate->Settings.Sensor.EAM == HOTTBRIDGESETTINGS_SENSOR_DISABLED) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
// clear message buffer
|
||||
memset(msg, 0, sizeof(*msg));
|
||||
|
||||
// message header
|
||||
msg->start = HOTT_START;
|
||||
msg->stop = HOTT_STOP;
|
||||
msg->sensor_id = HOTT_EAM_ID;
|
||||
msg->warning = generate_warning();
|
||||
msg->sensor_text_id = HOTT_EAM_TEXT_ID;
|
||||
|
||||
// alarm inverse bits. invert display areas on limits
|
||||
msg->alarm_inverse1 |= (telestate->Settings.Limit.MaxUsedCapacity < telestate->Battery.ConsumedEnergy) ? EAM_INVERT_CAPACITY : 0;
|
||||
msg->alarm_inverse1 |= (telestate->Settings.Limit.MaxCurrent < telestate->Battery.Current) ? EAM_INVERT_CURRENT : 0;
|
||||
msg->alarm_inverse1 |= (telestate->Settings.Limit.MinPowerVoltage > telestate->Battery.Voltage) ? EAM_INVERT_VOLTAGE : 0;
|
||||
msg->alarm_inverse1 |= (telestate->Settings.Limit.MaxPowerVoltage < telestate->Battery.Voltage) ? EAM_INVERT_VOLTAGE : 0;
|
||||
msg->alarm_inverse2 |= (telestate->Settings.Limit.MinHeight > telestate->altitude) ? EAM_INVERT2_ALT : 0;
|
||||
msg->alarm_inverse2 |= (telestate->Settings.Limit.MaxHeight < telestate->altitude) ? EAM_INVERT2_ALT : 0;
|
||||
msg->alarm_inverse2 |= (telestate->Settings.Limit.NegDifference1 > telestate->climbrate1s) ? EAM_INVERT2_CR1S : 0;
|
||||
msg->alarm_inverse2 |= (telestate->Settings.Limit.PosDifference1 < telestate->climbrate1s) ? EAM_INVERT2_CR1S : 0;
|
||||
msg->alarm_inverse2 |= (telestate->Settings.Limit.NegDifference2 > telestate->climbrate3s) ? EAM_INVERT2_CR3S : 0;
|
||||
msg->alarm_inverse2 |= (telestate->Settings.Limit.PosDifference2 < telestate->climbrate3s) ? EAM_INVERT2_CR3S : 0;
|
||||
|
||||
// main battery
|
||||
float voltage = (telestate->Battery.Voltage > 0) ? telestate->Battery.Voltage : 0;
|
||||
float current = (telestate->Battery.Current > 0) ? telestate->Battery.Current : 0;
|
||||
float energy = (telestate->Battery.ConsumedEnergy > 0) ? telestate->Battery.ConsumedEnergy : 0;
|
||||
msg->voltage = scale_float2uword(voltage, 10, 0);
|
||||
msg->current = scale_float2uword(current, 10, 0);
|
||||
msg->capacity = scale_float2uword(energy, 0.1f, 0);
|
||||
|
||||
// temperatures
|
||||
msg->temperature1 = scale_float2uint8(telestate->Gyro.temperature, 1, OFFSET_TEMPERATURE);
|
||||
msg->temperature2 = scale_float2uint8(telestate->Baro.Temperature, 1, OFFSET_TEMPERATURE);
|
||||
|
||||
// altitude
|
||||
msg->altitude = scale_float2uword(telestate->altitude, 1, OFFSET_ALTITUDE);
|
||||
|
||||
// climbrate
|
||||
msg->climbrate = scale_float2uword(telestate->climbrate1s, M_TO_CM, OFFSET_CLIMBRATE);
|
||||
msg->climbrate3s = scale_float2uint8(telestate->climbrate3s, 1, OFFSET_CLIMBRATE3S);
|
||||
|
||||
// flight time
|
||||
float flighttime = (telestate->Battery.EstimatedFlightTime <= 5999) ? telestate->Battery.EstimatedFlightTime : 5999;
|
||||
msg->electric_min = flighttime / 60;
|
||||
msg->electric_sec = flighttime - 60 * msg->electric_min;
|
||||
|
||||
msg->checksum = calc_checksum((uint8_t *)msg, sizeof(*msg));
|
||||
return sizeof(*msg);
|
||||
}
|
||||
|
||||
uint16_t build_ESC_message(struct hott_esc_message *msg)
|
||||
{
|
||||
update_telemetrydata();
|
||||
|
||||
if (telestate->Settings.Sensor.ESC == HOTTBRIDGESETTINGS_SENSOR_DISABLED) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
// clear message buffer
|
||||
memset(msg, 0, sizeof(*msg));
|
||||
|
||||
// message header
|
||||
msg->start = HOTT_START;
|
||||
msg->stop = HOTT_STOP;
|
||||
msg->sensor_id = HOTT_ESC_ID;
|
||||
msg->warning = 0;
|
||||
msg->sensor_text_id = HOTT_ESC_TEXT_ID;
|
||||
|
||||
// main batterie
|
||||
float voltage = (telestate->Battery.Voltage > 0) ? telestate->Battery.Voltage : 0;
|
||||
float current = (telestate->Battery.Current > 0) ? telestate->Battery.Current : 0;
|
||||
float max_current = (telestate->Battery.PeakCurrent > 0) ? telestate->Battery.PeakCurrent : 0;
|
||||
float energy = (telestate->Battery.ConsumedEnergy > 0) ? telestate->Battery.ConsumedEnergy : 0;
|
||||
msg->batt_voltage = scale_float2uword(voltage, 10, 0);
|
||||
msg->current = scale_float2uword(current, 10, 0);
|
||||
msg->max_current = scale_float2uword(max_current, 10, 0);
|
||||
msg->batt_capacity = scale_float2uword(energy, 0.1f, 0);
|
||||
|
||||
// temperatures
|
||||
msg->temperatureESC = scale_float2uint8(telestate->Gyro.temperature, 1, OFFSET_TEMPERATURE);
|
||||
msg->max_temperatureESC = scale_float2uint8(0, 1, OFFSET_TEMPERATURE);
|
||||
msg->temperatureMOT = scale_float2uint8(telestate->Baro.Temperature, 1, OFFSET_TEMPERATURE);
|
||||
msg->max_temperatureMOT = scale_float2uint8(0, 1, OFFSET_TEMPERATURE);
|
||||
|
||||
msg->checksum = calc_checksum((uint8_t *)msg, sizeof(*msg));
|
||||
return sizeof(*msg);
|
||||
}
|
||||
|
||||
uint16_t build_TEXT_message(struct hott_text_message *msg)
|
||||
{
|
||||
update_telemetrydata();
|
||||
|
||||
// clear message buffer
|
||||
memset(msg, 0, sizeof(*msg));
|
||||
|
||||
// message header
|
||||
msg->start = HOTT_START;
|
||||
msg->stop = HOTT_STOP;
|
||||
msg->sensor_id = HOTT_TEXT_ID;
|
||||
|
||||
msg->checksum = calc_checksum((uint8_t *)msg, sizeof(*msg));
|
||||
return sizeof(*msg);
|
||||
}
|
||||
|
||||
/**
|
||||
* update telemetry data
|
||||
* this is called on every telemetry request
|
||||
* calling interval is 200ms depending on TX
|
||||
* 200ms telemetry request is used as time base for timed calculations (5Hz interval)
|
||||
*/
|
||||
void update_telemetrydata()
|
||||
{
|
||||
// update all available data
|
||||
if (HoTTBridgeSettingsHandle() != NULL) {
|
||||
HoTTBridgeSettingsGet(&telestate->Settings);
|
||||
}
|
||||
if (AttitudeStateHandle() != NULL) {
|
||||
AttitudeStateGet(&telestate->Attitude);
|
||||
}
|
||||
if (BaroSensorHandle() != NULL) {
|
||||
BaroSensorGet(&telestate->Baro);
|
||||
}
|
||||
if (FlightBatteryStateHandle() != NULL) {
|
||||
FlightBatteryStateGet(&telestate->Battery);
|
||||
}
|
||||
if (FlightStatusHandle() != NULL) {
|
||||
FlightStatusGet(&telestate->FlightStatus);
|
||||
}
|
||||
if (GPSPositionSensorHandle() != NULL) {
|
||||
GPSPositionSensorGet(&telestate->GPS);
|
||||
}
|
||||
if (GPSTimeHandle() != NULL) {
|
||||
GPSTimeGet(&telestate->GPStime);
|
||||
}
|
||||
if (GyroSensorHandle() != NULL) {
|
||||
GyroSensorGet(&telestate->Gyro);
|
||||
}
|
||||
if (HomeLocationHandle() != NULL) {
|
||||
HomeLocationGet(&telestate->Home);
|
||||
}
|
||||
if (PositionStateHandle() != NULL) {
|
||||
PositionStateGet(&telestate->Position);
|
||||
}
|
||||
if (SystemAlarmsHandle() != NULL) {
|
||||
SystemAlarmsGet(&telestate->SysAlarms);
|
||||
}
|
||||
if (VelocityStateHandle() != NULL) {
|
||||
VelocityStateGet(&telestate->Velocity);
|
||||
}
|
||||
|
||||
// send actual climbrate value to ring buffer as mm per 0.2s values
|
||||
uint8_t n = telestate->climbrate_pointer;
|
||||
telestate->climbratebuffer[telestate->climbrate_pointer++] = -telestate->Velocity.Down * 200;
|
||||
telestate->climbrate_pointer %= climbratesize;
|
||||
|
||||
// calculate avarage climbrates in meters per 1, 3 and 10 second(s) based on 200ms interval
|
||||
telestate->climbrate1s = 0;
|
||||
telestate->climbrate3s = 0;
|
||||
telestate->climbrate10s = 0;
|
||||
for (uint8_t i = 0; i < climbratesize; i++) {
|
||||
telestate->climbrate1s += (i < 5) ? telestate->climbratebuffer[n] : 0;
|
||||
telestate->climbrate3s += (i < 15) ? telestate->climbratebuffer[n] : 0;
|
||||
telestate->climbrate10s += (i < 50) ? telestate->climbratebuffer[n] : 0;
|
||||
n += climbratesize - 1;
|
||||
n %= climbratesize;
|
||||
}
|
||||
telestate->climbrate1s = telestate->climbrate1s / 1000;
|
||||
telestate->climbrate3s = telestate->climbrate3s / 1000;
|
||||
telestate->climbrate10s = telestate->climbrate10s / 1000;
|
||||
|
||||
// set altitude offset and clear min/max values when arming
|
||||
if ((telestate->FlightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING) || ((telestate->last_armed != FLIGHTSTATUS_ARMED_ARMED) && (telestate->FlightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED))) {
|
||||
telestate->min_altitude = 0;
|
||||
telestate->max_altitude = 0;
|
||||
}
|
||||
telestate->last_armed = telestate->FlightStatus.Armed;
|
||||
|
||||
// calculate altitude relative to start position
|
||||
telestate->altitude = -telestate->Position.Down;
|
||||
|
||||
// check and set min/max values when armed.
|
||||
if (telestate->FlightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) {
|
||||
if (telestate->min_altitude > telestate->altitude) {
|
||||
telestate->min_altitude = telestate->altitude;
|
||||
}
|
||||
if (telestate->max_altitude < telestate->altitude) {
|
||||
telestate->max_altitude = telestate->altitude;
|
||||
}
|
||||
}
|
||||
|
||||
// gps home position and course
|
||||
telestate->homedistance = sqrtf(telestate->Position.North * telestate->Position.North + telestate->Position.East * telestate->Position.East);
|
||||
telestate->homecourse = acosf(-telestate->Position.North / telestate->homedistance) / 3.14159265f * 180;
|
||||
if (telestate->Position.East > 0) {
|
||||
telestate->homecourse = 360 - telestate->homecourse;
|
||||
}
|
||||
|
||||
// statusline
|
||||
const char *txt_unknown = "unknown";
|
||||
const char *txt_manual = "Manual";
|
||||
const char *txt_stabilized1 = "Stabilized1";
|
||||
const char *txt_stabilized2 = "Stabilized2";
|
||||
const char *txt_stabilized3 = "Stabilized3";
|
||||
const char *txt_stabilized4 = "Stabilized4";
|
||||
const char *txt_stabilized5 = "Stabilized5";
|
||||
const char *txt_stabilized6 = "Stabilized6";
|
||||
const char *txt_positionhold = "PositionHold";
|
||||
const char *txt_courselock = "CourseLock";
|
||||
const char *txt_velocityroam = "VelocityRoam";
|
||||
const char *txt_homeleash = "HomeLeash";
|
||||
const char *txt_absoluteposition = "AbsolutePosition";
|
||||
const char *txt_returntobase = "ReturnToBase";
|
||||
const char *txt_land = "Land";
|
||||
const char *txt_pathplanner = "PathPlanner";
|
||||
const char *txt_poi = "PointOfInterest";
|
||||
const char *txt_autocruise = "AutoCruise";
|
||||
const char *txt_autotakeoff = "AutoTakeOff";
|
||||
const char *txt_autotune = "Autotune";
|
||||
const char *txt_disarmed = "Disarmed";
|
||||
const char *txt_arming = "Arming";
|
||||
const char *txt_armed = "Armed";
|
||||
|
||||
const char *txt_flightmode;
|
||||
switch (telestate->FlightStatus.FlightMode) {
|
||||
case FLIGHTSTATUS_FLIGHTMODE_MANUAL:
|
||||
txt_flightmode = txt_manual;
|
||||
break;
|
||||
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
|
||||
txt_flightmode = txt_stabilized1;
|
||||
break;
|
||||
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
|
||||
txt_flightmode = txt_stabilized2;
|
||||
break;
|
||||
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
|
||||
txt_flightmode = txt_stabilized3;
|
||||
break;
|
||||
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED4:
|
||||
txt_flightmode = txt_stabilized4;
|
||||
break;
|
||||
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED5:
|
||||
txt_flightmode = txt_stabilized5;
|
||||
break;
|
||||
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED6:
|
||||
txt_flightmode = txt_stabilized6;
|
||||
break;
|
||||
case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
|
||||
txt_flightmode = txt_positionhold;
|
||||
break;
|
||||
case FLIGHTSTATUS_FLIGHTMODE_COURSELOCK:
|
||||
txt_flightmode = txt_courselock;
|
||||
break;
|
||||
case FLIGHTSTATUS_FLIGHTMODE_VELOCITYROAM:
|
||||
txt_flightmode = txt_velocityroam;
|
||||
break;
|
||||
case FLIGHTSTATUS_FLIGHTMODE_HOMELEASH:
|
||||
txt_flightmode = txt_homeleash;
|
||||
break;
|
||||
case FLIGHTSTATUS_FLIGHTMODE_ABSOLUTEPOSITION:
|
||||
txt_flightmode = txt_absoluteposition;
|
||||
break;
|
||||
case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE:
|
||||
txt_flightmode = txt_returntobase;
|
||||
break;
|
||||
case FLIGHTSTATUS_FLIGHTMODE_LAND:
|
||||
txt_flightmode = txt_land;
|
||||
break;
|
||||
case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER:
|
||||
txt_flightmode = txt_pathplanner;
|
||||
break;
|
||||
case FLIGHTSTATUS_FLIGHTMODE_POI:
|
||||
txt_flightmode = txt_poi;
|
||||
break;
|
||||
case FLIGHTSTATUS_FLIGHTMODE_AUTOCRUISE:
|
||||
txt_flightmode = txt_autocruise;
|
||||
break;
|
||||
case FLIGHTSTATUS_FLIGHTMODE_AUTOTAKEOFF:
|
||||
txt_flightmode = txt_autotakeoff;
|
||||
break;
|
||||
case FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE:
|
||||
txt_flightmode = txt_autotune;
|
||||
break;
|
||||
default:
|
||||
txt_flightmode = txt_unknown;
|
||||
}
|
||||
|
||||
const char *txt_armstate;
|
||||
switch (telestate->FlightStatus.Armed) {
|
||||
case FLIGHTSTATUS_ARMED_DISARMED:
|
||||
txt_armstate = txt_disarmed;
|
||||
break;
|
||||
case FLIGHTSTATUS_ARMED_ARMING:
|
||||
txt_armstate = txt_arming;
|
||||
break;
|
||||
case FLIGHTSTATUS_ARMED_ARMED:
|
||||
txt_armstate = txt_armed;
|
||||
break;
|
||||
default:
|
||||
txt_armstate = txt_unknown;
|
||||
}
|
||||
|
||||
snprintf(telestate->statusline, sizeof(telestate->statusline), "%12s,%8s", txt_flightmode, txt_armstate);
|
||||
}
|
||||
|
||||
/**
|
||||
* generate warning beeps or spoken announcements
|
||||
*/
|
||||
uint8_t generate_warning()
|
||||
{
|
||||
// set warning tone with hardcoded priority
|
||||
if ((telestate->Settings.Warning.MinSpeed == HOTTBRIDGESETTINGS_WARNING_ENABLED) &&
|
||||
(telestate->Settings.Limit.MinSpeed > telestate->GPS.Groundspeed * MS_TO_KMH)) {
|
||||
return HOTT_TONE_A; // maximum speed
|
||||
}
|
||||
if ((telestate->Settings.Warning.NegDifference2 == HOTTBRIDGESETTINGS_WARNING_ENABLED) &&
|
||||
(telestate->Settings.Limit.NegDifference2 > telestate->climbrate3s)) {
|
||||
return HOTT_TONE_B; // sink rate 3s
|
||||
}
|
||||
if ((telestate->Settings.Warning.NegDifference1 == HOTTBRIDGESETTINGS_WARNING_ENABLED) &&
|
||||
(telestate->Settings.Limit.NegDifference1 > telestate->climbrate1s)) {
|
||||
return HOTT_TONE_C; // sink rate 1s
|
||||
}
|
||||
if ((telestate->Settings.Warning.MaxDistance == HOTTBRIDGESETTINGS_WARNING_ENABLED) &&
|
||||
(telestate->Settings.Limit.MaxDistance < telestate->homedistance)) {
|
||||
return HOTT_TONE_D; // maximum distance
|
||||
}
|
||||
if ((telestate->Settings.Warning.MinSensor1Temp == HOTTBRIDGESETTINGS_WARNING_ENABLED) &&
|
||||
(telestate->Settings.Limit.MinSensor1Temp > telestate->Gyro.temperature)) {
|
||||
return HOTT_TONE_F; // minimum temperature sensor 1
|
||||
}
|
||||
if ((telestate->Settings.Warning.MinSensor2Temp == HOTTBRIDGESETTINGS_WARNING_ENABLED) &&
|
||||
(telestate->Settings.Limit.MinSensor2Temp > telestate->Baro.Temperature)) {
|
||||
return HOTT_TONE_G; // minimum temperature sensor 2
|
||||
}
|
||||
if ((telestate->Settings.Warning.MaxSensor1Temp == HOTTBRIDGESETTINGS_WARNING_ENABLED) &&
|
||||
(telestate->Settings.Limit.MaxSensor1Temp < telestate->Gyro.temperature)) {
|
||||
return HOTT_TONE_H; // maximum temperature sensor 1
|
||||
}
|
||||
if ((telestate->Settings.Warning.MaxSensor2Temp == HOTTBRIDGESETTINGS_WARNING_ENABLED) &&
|
||||
(telestate->Settings.Limit.MaxSensor2Temp < telestate->Baro.Temperature)) {
|
||||
return HOTT_TONE_I; // maximum temperature sensor 2
|
||||
}
|
||||
if ((telestate->Settings.Warning.MaxSpeed == HOTTBRIDGESETTINGS_WARNING_ENABLED) &&
|
||||
(telestate->Settings.Limit.MaxSpeed < telestate->GPS.Groundspeed * MS_TO_KMH)) {
|
||||
return HOTT_TONE_L; // maximum speed
|
||||
}
|
||||
if ((telestate->Settings.Warning.PosDifference2 == HOTTBRIDGESETTINGS_WARNING_ENABLED) &&
|
||||
(telestate->Settings.Limit.PosDifference2 > telestate->climbrate3s)) {
|
||||
return HOTT_TONE_M; // climb rate 3s
|
||||
}
|
||||
if ((telestate->Settings.Warning.PosDifference1 == HOTTBRIDGESETTINGS_WARNING_ENABLED) &&
|
||||
(telestate->Settings.Limit.PosDifference1 > telestate->climbrate1s)) {
|
||||
return HOTT_TONE_N; // climb rate 1s
|
||||
}
|
||||
if ((telestate->Settings.Warning.MinHeight == HOTTBRIDGESETTINGS_WARNING_ENABLED) &&
|
||||
(telestate->Settings.Limit.MinHeight > telestate->altitude)) {
|
||||
return HOTT_TONE_O; // minimum height
|
||||
}
|
||||
if ((telestate->Settings.Warning.MinPowerVoltage == HOTTBRIDGESETTINGS_WARNING_ENABLED) &&
|
||||
(telestate->Settings.Limit.MinPowerVoltage > telestate->Battery.Voltage)) {
|
||||
return HOTT_TONE_P; // minimum input voltage
|
||||
}
|
||||
if ((telestate->Settings.Warning.MaxUsedCapacity == HOTTBRIDGESETTINGS_WARNING_ENABLED) &&
|
||||
(telestate->Settings.Limit.MaxUsedCapacity < telestate->Battery.ConsumedEnergy)) {
|
||||
return HOTT_TONE_V; // capacity
|
||||
}
|
||||
if ((telestate->Settings.Warning.MaxCurrent == HOTTBRIDGESETTINGS_WARNING_ENABLED) &&
|
||||
(telestate->Settings.Limit.MaxCurrent < telestate->Battery.Current)) {
|
||||
return HOTT_TONE_W; // maximum current
|
||||
}
|
||||
if ((telestate->Settings.Warning.MaxPowerVoltage == HOTTBRIDGESETTINGS_WARNING_ENABLED) &&
|
||||
(telestate->Settings.Limit.MaxPowerVoltage < telestate->Battery.Voltage)) {
|
||||
return HOTT_TONE_X; // maximum input voltage
|
||||
}
|
||||
if ((telestate->Settings.Warning.MaxHeight == HOTTBRIDGESETTINGS_WARNING_ENABLED) &&
|
||||
(telestate->Settings.Limit.MaxHeight < telestate->altitude)) {
|
||||
return HOTT_TONE_Z; // maximum height
|
||||
}
|
||||
// altitude beeps when crossing altitude limits at 20,40,60,80,100,200,400,600,800 and 1000 meters
|
||||
if (telestate->Settings.Warning.AltitudeBeep == HOTTBRIDGESETTINGS_WARNING_ENABLED) {
|
||||
// update altitude when checked for beeps
|
||||
float last = telestate->altitude_last;
|
||||
float actual = telestate->altitude;
|
||||
telestate->altitude_last = telestate->altitude;
|
||||
if (((last < 20) && (actual >= 20)) || ((last > 20) && (actual <= 20))) {
|
||||
return HOTT_TONE_20M;
|
||||
}
|
||||
if (((last < 40) && (actual >= 40)) || ((last > 40) && (actual <= 40))) {
|
||||
return HOTT_TONE_40M;
|
||||
}
|
||||
if (((last < 60) && (actual >= 60)) || ((last > 60) && (actual <= 60))) {
|
||||
return HOTT_TONE_60M;
|
||||
}
|
||||
if (((last < 80) && (actual >= 80)) || ((last > 80) && (actual <= 80))) {
|
||||
return HOTT_TONE_80M;
|
||||
}
|
||||
if (((last < 100) && (actual >= 100)) || ((last > 100) && (actual <= 100))) {
|
||||
return HOTT_TONE_100M;
|
||||
}
|
||||
if (((last < 200) && (actual >= 200)) || ((last > 200) && (actual <= 200))) {
|
||||
return HOTT_TONE_200M;
|
||||
}
|
||||
if (((last < 400) && (actual >= 400)) || ((last > 400) && (actual <= 400))) {
|
||||
return HOTT_TONE_400M;
|
||||
}
|
||||
if (((last < 600) && (actual >= 600)) || ((last > 600) && (actual <= 600))) {
|
||||
return HOTT_TONE_600M;
|
||||
}
|
||||
if (((last < 800) && (actual >= 800)) || ((last > 800) && (actual <= 800))) {
|
||||
return HOTT_TONE_800M;
|
||||
}
|
||||
if (((last < 1000) && (actual >= 1000)) || ((last > 1000) && (actual <= 1000))) {
|
||||
return HOTT_TONE_1000M;
|
||||
}
|
||||
}
|
||||
|
||||
// there is no warning
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* calculate checksum of data buffer
|
||||
*/
|
||||
uint8_t calc_checksum(uint8_t *data, uint16_t size)
|
||||
{
|
||||
uint16_t sum = 0;
|
||||
|
||||
for (int i = 0; i < size; i++) {
|
||||
sum += data[i];
|
||||
}
|
||||
return sum;
|
||||
}
|
||||
|
||||
/**
|
||||
* scale float value with scale and offset to unsigned byte
|
||||
*/
|
||||
uint8_t scale_float2uint8(float value, float scale, float offset)
|
||||
{
|
||||
uint16_t temp = (uint16_t)roundf(value * scale + offset);
|
||||
uint8_t result;
|
||||
|
||||
result = (uint8_t)temp & 0xff;
|
||||
return result;
|
||||
}
|
||||
|
||||
/**
|
||||
* scale float value with scale and offset to signed byte (int8_t)
|
||||
*/
|
||||
int8_t scale_float2int8(float value, float scale, float offset)
|
||||
{
|
||||
int8_t result = (int8_t)roundf(value * scale + offset);
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
/**
|
||||
* scale float value with scale and offset to word
|
||||
*/
|
||||
uword_t scale_float2uword(float value, float scale, float offset)
|
||||
{
|
||||
uint16_t temp = (uint16_t)roundf(value * scale + offset);
|
||||
uword_t result;
|
||||
|
||||
result.l = (uint8_t)temp & 0xff;
|
||||
result.h = (uint8_t)(temp >> 8) & 0xff;
|
||||
return result;
|
||||
}
|
||||
|
||||
/**
|
||||
* convert dword gps value into HoTT gps format and write result to given pointers
|
||||
*/
|
||||
void convert_long2gps(int32_t value, uint8_t *dir, uword_t *min, uword_t *sec)
|
||||
{
|
||||
// convert gps decigrad value into degrees, minutes and seconds
|
||||
uword_t temp;
|
||||
uint32_t absvalue = abs(value);
|
||||
uint16_t degrees = (absvalue / 10000000);
|
||||
uint32_t seconds = (absvalue - degrees * 10000000) * 6;
|
||||
uint16_t minutes = seconds / 1000000;
|
||||
|
||||
seconds %= 1000000;
|
||||
seconds = seconds / 100;
|
||||
uint16_t degmin = degrees * 100 + minutes;
|
||||
// write results
|
||||
*dir = (value < 0) ? 1 : 0;
|
||||
temp.l = (uint8_t)degmin & 0xff;
|
||||
temp.h = (uint8_t)(degmin >> 8) & 0xff;
|
||||
*min = temp;
|
||||
temp.l = (uint8_t)seconds & 0xff;
|
||||
temp.h = (uint8_t)(seconds >> 8) & 0xff;
|
||||
*sec = temp;
|
||||
}
|
||||
|
||||
#endif // PIOS_INCLUDE_HOTT_BRIDGE
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
|
||||
@ -282,6 +282,30 @@ int32_t PIOS_COM_ChangeBaud(uint32_t com_id, uint32_t baud)
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Change the port type to halfduplex (shared rx/tx medium)
|
||||
* \param[in] port COM port
|
||||
* \param[in] halfduplex enabled
|
||||
* \return -1 if port not available
|
||||
* \return 0 on success
|
||||
*/
|
||||
int32_t PIOS_COM_SetHalfDuplex(uint32_t com_id, bool halfduplex)
|
||||
{
|
||||
struct pios_com_dev *com_dev = (struct pios_com_dev *)com_id;
|
||||
|
||||
if (!PIOS_COM_validate(com_dev)) {
|
||||
/* Undefined COM port for this board (see pios_board.c) */
|
||||
return -1;
|
||||
}
|
||||
|
||||
/* Invoke the driver function if it exists */
|
||||
if (com_dev->driver->set_halfduplex) {
|
||||
com_dev->driver->set_halfduplex(com_dev->lower_id, halfduplex);
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int32_t PIOS_COM_ChangeConfig(uint32_t com_id, enum PIOS_COM_Word_Length word_len, enum PIOS_COM_StopBits stop_bits, enum PIOS_COM_Parity parity, uint32_t baud_rate, enum PIOS_COM_Mode mode)
|
||||
{
|
||||
struct pios_com_dev *com_dev = (struct pios_com_dev *)com_id;
|
||||
|
||||
@ -72,6 +72,7 @@ enum PIOS_COM_Mode {
|
||||
struct pios_com_driver {
|
||||
void (*init)(uint32_t id);
|
||||
void (*set_baud)(uint32_t id, uint32_t baud);
|
||||
void (*set_halfduplex)(uint32_t id, bool halfduplex);
|
||||
void (*set_config)(uint32_t usart_id, enum PIOS_COM_Word_Length word_len, enum PIOS_COM_StopBits stop_bits, enum PIOS_COM_Parity parity, uint32_t baud_rate, enum PIOS_COM_Mode mode);
|
||||
void (*set_ctrl_line)(uint32_t id, uint32_t mask, uint32_t state);
|
||||
void (*tx_start)(uint32_t id, uint16_t tx_bytes_avail);
|
||||
@ -91,6 +92,7 @@ struct pios_com_driver {
|
||||
/* Public Functions */
|
||||
extern int32_t PIOS_COM_Init(uint32_t *com_id, const struct pios_com_driver *driver, uint32_t lower_id, uint8_t *rx_buffer, uint16_t rx_buffer_len, uint8_t *tx_buffer, uint16_t tx_buffer_len);
|
||||
extern int32_t PIOS_COM_ChangeBaud(uint32_t com_id, uint32_t baud);
|
||||
extern int32_t PIOS_COM_SetHalfDuplex(uint32_t com_id, bool halfduplex);
|
||||
extern int32_t PIOS_COM_ChangeConfig(uint32_t com_id, enum PIOS_COM_Word_Length word_len, enum PIOS_COM_StopBits stop_bits, enum PIOS_COM_Parity parity, uint32_t baud_rate, enum PIOS_COM_Mode mode);
|
||||
extern int32_t PIOS_COM_SetCtrlLine(uint32_t com_id, uint32_t mask, uint32_t state);
|
||||
extern int32_t PIOS_COM_RegisterCtrlLineCallback(uint32_t usart_id, pios_com_callback_ctrl_line ctrl_line_cb, uint32_t context);
|
||||
|
||||
@ -41,6 +41,7 @@
|
||||
|
||||
/* Provide a COM driver */
|
||||
static void PIOS_USART_ChangeBaud(uint32_t usart_id, uint32_t baud);
|
||||
static void PIOS_USART_SetHalfDuplex(uint32_t usart_id, bool halfduplex);
|
||||
static void PIOS_USART_ChangeConfig(uint32_t usart_id, enum PIOS_COM_Word_Length word_len, enum PIOS_COM_StopBits stop_bits, enum PIOS_COM_Parity parity, uint32_t baud_rate, enum PIOS_COM_Mode mode);
|
||||
static void PIOS_USART_SetCtrlLine(uint32_t usart_id, uint32_t mask, uint32_t state);
|
||||
static void PIOS_USART_RegisterRxCallback(uint32_t usart_id, pios_com_callback rx_in_cb, uint32_t context);
|
||||
@ -49,13 +50,14 @@ static void PIOS_USART_TxStart(uint32_t usart_id, uint16_t tx_bytes_avail);
|
||||
static void PIOS_USART_RxStart(uint32_t usart_id, uint16_t rx_bytes_avail);
|
||||
|
||||
const struct pios_com_driver pios_usart_com_driver = {
|
||||
.set_baud = PIOS_USART_ChangeBaud,
|
||||
.set_config = PIOS_USART_ChangeConfig,
|
||||
.set_ctrl_line = PIOS_USART_SetCtrlLine,
|
||||
.tx_start = PIOS_USART_TxStart,
|
||||
.rx_start = PIOS_USART_RxStart,
|
||||
.bind_tx_cb = PIOS_USART_RegisterTxCallback,
|
||||
.bind_rx_cb = PIOS_USART_RegisterRxCallback,
|
||||
.set_baud = PIOS_USART_ChangeBaud,
|
||||
.set_halfduplex = PIOS_USART_SetHalfDuplex,
|
||||
.set_config = PIOS_USART_ChangeConfig,
|
||||
.set_ctrl_line = PIOS_USART_SetCtrlLine,
|
||||
.tx_start = PIOS_USART_TxStart,
|
||||
.rx_start = PIOS_USART_RxStart,
|
||||
.bind_tx_cb = PIOS_USART_RegisterTxCallback,
|
||||
.bind_rx_cb = PIOS_USART_RegisterRxCallback,
|
||||
};
|
||||
|
||||
enum pios_usart_dev_magic {
|
||||
@ -286,6 +288,22 @@ static void PIOS_USART_ChangeBaud(uint32_t usart_id, uint32_t baud)
|
||||
USART_Init(usart_dev->cfg->regs, &usart_dev->init);
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the USART peripheral into half duplex mode
|
||||
* \param[in] usart_id USART name (GPS, TELEM, AUX)
|
||||
* \param[in] bool wether to set half duplex or not
|
||||
*/
|
||||
static void PIOS_USART_SetHalfDuplex(uint32_t usart_id, bool halfduplex)
|
||||
{
|
||||
struct pios_usart_dev *usart_dev = (struct pios_usart_dev *)usart_id;
|
||||
|
||||
bool valid = PIOS_USART_validate(usart_dev);
|
||||
|
||||
PIOS_Assert(valid);
|
||||
|
||||
USART_HalfDuplexCmd(usart_dev->cfg->regs, halfduplex ? ENABLE : DISABLE);
|
||||
}
|
||||
|
||||
/**
|
||||
* Changes configuration of the USART peripheral without re-initialising.
|
||||
* \param[in] usart_id USART name (GPS, TELEM, AUX)
|
||||
|
||||
@ -126,6 +126,7 @@ UAVOBJSRCFILENAMES += poilearnsettings
|
||||
UAVOBJSRCFILENAMES += mpugyroaccelsettings
|
||||
UAVOBJSRCFILENAMES += txpidsettings
|
||||
UAVOBJSRCFILENAMES += txpidstatus
|
||||
UAVOBJSRCFILENAMES += hottbridgesettings
|
||||
UAVOBJSRCFILENAMES += hottbridgestatus
|
||||
UAVOBJSRCFILENAMES += takeofflocation
|
||||
UAVOBJSRCFILENAMES += perfcounter
|
||||
|
||||
@ -119,6 +119,7 @@
|
||||
#define PIOS_INCLUDE_LED
|
||||
#define PIOS_INCLUDE_IAP
|
||||
#define PIOS_INCLUDE_SERVO
|
||||
#define PIOS_INCLUDE_HOTT_BRIDGE
|
||||
/* #define PIOS_INCLUDE_I2C_ESC */
|
||||
/* #define PIOS_INCLUDE_OVERO */
|
||||
/* #define PIOS_OVERO_SPI */
|
||||
|
||||
@ -1048,6 +1048,7 @@ void PIOS_Board_Init(void)
|
||||
case HWSETTINGS_RM_RCVRPORT_PPMMSP:
|
||||
case HWSETTINGS_RM_RCVRPORT_PPMMAVLINK:
|
||||
case HWSETTINGS_RM_RCVRPORT_PPMGPS:
|
||||
case HWSETTINGS_RM_RCVRPORT_PPMHOTT:
|
||||
#if defined(PIOS_INCLUDE_PPM)
|
||||
PIOS_Board_configure_ppm(&pios_ppm_cfg);
|
||||
|
||||
@ -1097,6 +1098,10 @@ void PIOS_Board_Init(void)
|
||||
case HWSETTINGS_RM_RCVRPORT_PPMGPS:
|
||||
PIOS_Board_configure_com(&pios_usart_rcvrport_cfg, PIOS_COM_GPS_RX_BUF_LEN, PIOS_COM_GPS_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_gps_id);
|
||||
break;
|
||||
case HWSETTINGS_RM_RCVRPORT_HOTT:
|
||||
case HWSETTINGS_RM_RCVRPORT_PPMHOTT:
|
||||
PIOS_Board_configure_com(&pios_usart_rcvrport_cfg, PIOS_COM_HOTT_RX_BUF_LEN, PIOS_COM_HOTT_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hott_id);
|
||||
break;
|
||||
case HWSETTINGS_RM_RCVRPORT_IBUS:
|
||||
#if defined(PIOS_INCLUDE_IBUS)
|
||||
PIOS_Board_configure_ibus(&pios_usart_ibus_rcvr_cfg);
|
||||
|
||||
@ -94,6 +94,7 @@ UAVOBJS = \
|
||||
$${UAVOBJ_XML_DIR}/gyrosensor.xml \
|
||||
$${UAVOBJ_XML_DIR}/gyrostate.xml \
|
||||
$${UAVOBJ_XML_DIR}/homelocation.xml \
|
||||
$${UAVOBJ_XML_DIR}/hottbridgesettings.xml \
|
||||
$${UAVOBJ_XML_DIR}/hottbridgestatus.xml \
|
||||
$${UAVOBJ_XML_DIR}/hwsettings.xml \
|
||||
$${UAVOBJ_XML_DIR}/i2cstats.xml \
|
||||
|
||||
82
shared/uavobjectdefinition/hottbridgesettings.xml
Normal file
82
shared/uavobjectdefinition/hottbridgesettings.xml
Normal file
@ -0,0 +1,82 @@
|
||||
<xml>
|
||||
<object name="HoTTBridgeSettings" singleinstance="true" settings="true" category="System">
|
||||
<description>Settings for the @ref HoTT Telemetry Module</description>
|
||||
<field name="Sensor" units="" type="enum" options="Disabled,Enabled" defaultvalue="Disabled">
|
||||
<elementnames>
|
||||
<elementname>VARIO</elementname>
|
||||
<elementname>GPS</elementname>
|
||||
<elementname>EAM</elementname>
|
||||
<elementname>GAM</elementname>
|
||||
<elementname>ESC</elementname>
|
||||
</elementnames>
|
||||
</field>
|
||||
<field name="Warning" units="" type="enum" options="Disabled,Enabled" defaultvalue="Disabled">
|
||||
<elementnames>
|
||||
<elementname>AltitudeBeep</elementname>
|
||||
<elementname>MinSpeed</elementname>
|
||||
<elementname>MaxSpeed</elementname>
|
||||
<elementname>NegDifference1</elementname>
|
||||
<elementname>PosDifference1</elementname>
|
||||
<elementname>NegDifference2</elementname>
|
||||
<elementname>PosDifference2</elementname>
|
||||
<elementname>MinHeight</elementname>
|
||||
<elementname>MaxHeight</elementname>
|
||||
<elementname>MaxDistance</elementname>
|
||||
<elementname>MinPowerVoltage</elementname>
|
||||
<elementname>MaxPowerVoltage</elementname>
|
||||
<elementname>MinSensor1Voltage</elementname>
|
||||
<elementname>MaxSensor1Voltage</elementname>
|
||||
<elementname>MinSensor2Voltage</elementname>
|
||||
<elementname>MaxSensor2Voltage</elementname>
|
||||
<elementname>MinCellVoltage</elementname>
|
||||
<elementname>MaxCurrent</elementname>
|
||||
<elementname>MaxUsedCapacity</elementname>
|
||||
<elementname>MinSensor1Temp</elementname>
|
||||
<elementname>MaxSensor1Temp</elementname>
|
||||
<elementname>MinSensor2Temp</elementname>
|
||||
<elementname>MaxSensor2Temp</elementname>
|
||||
<elementname>MaxServoTemp</elementname>
|
||||
<elementname>MinRPM</elementname>
|
||||
<elementname>MaxRPM</elementname>
|
||||
<elementname>MinFuel</elementname>
|
||||
<elementname>MaxServoDifference</elementname>
|
||||
</elementnames>
|
||||
</field>
|
||||
<field name="Limit" units="(km/h)/(m/s)/m/V/A/mAh/C/ml" type="float" defaultvalue="30,100,-10,10,-1,1,20,500,1500,5,30,5,30,5,30,3.3,40,2000,0,100,0,100,100,100,7000,1000,0">
|
||||
<elementnames>
|
||||
<elementname>MinSpeed</elementname>
|
||||
<elementname>MaxSpeed</elementname>
|
||||
<elementname>NegDifference1</elementname>
|
||||
<elementname>PosDifference1</elementname>
|
||||
<elementname>NegDifference2</elementname>
|
||||
<elementname>PosDifference2</elementname>
|
||||
<elementname>MinHeight</elementname>
|
||||
<elementname>MaxHeight</elementname>
|
||||
<elementname>MaxDistance</elementname>
|
||||
<elementname>MinPowerVoltage</elementname>
|
||||
<elementname>MaxPowerVoltage</elementname>
|
||||
<elementname>MinSensor1Voltage</elementname>
|
||||
<elementname>MaxSensor1Voltage</elementname>
|
||||
<elementname>MinSensor2Voltage</elementname>
|
||||
<elementname>MaxSensor2Voltage</elementname>
|
||||
<elementname>MinCellVoltage</elementname>
|
||||
<elementname>MaxCurrent</elementname>
|
||||
<elementname>MaxUsedCapacity</elementname>
|
||||
<elementname>MinSensor1Temp</elementname>
|
||||
<elementname>MaxSensor1Temp</elementname>
|
||||
<elementname>MinSensor2Temp</elementname>
|
||||
<elementname>MaxSensor2Temp</elementname>
|
||||
<elementname>MaxServoTemp</elementname>
|
||||
<elementname>MinRPM</elementname>
|
||||
<elementname>MaxRPM</elementname>
|
||||
<elementname>MinFuel</elementname>
|
||||
<elementname>MaxServoDifference</elementname>
|
||||
</elementnames>
|
||||
</field>
|
||||
|
||||
<access gcs="readwrite" flight="readwrite"/>
|
||||
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
|
||||
<telemetryflight acked="true" updatemode="onchange" period="0"/>
|
||||
<logging updatemode="manual" period="0"/>
|
||||
</object>
|
||||
</xml>
|
||||
@ -11,9 +11,9 @@
|
||||
<field name="RV_TelemetryPort" units="function" type="enum" elements="1" options="Disabled,Telemetry,ComAux,ComBridge,MSP,MAVLink" defaultvalue="Telemetry"/>
|
||||
<field name="RV_GPSPort" units="function" type="enum" elements="1" options="Disabled,Telemetry,GPS,ComAux,ComBridge,MSP,MAVLink" defaultvalue="GPS"/>
|
||||
|
||||
<field name="RM_RcvrPort" units="function" type="enum" elements="1" options="Disabled,PWM,PPM,PPM+PWM,PPM+Outputs,PPM+Telemetry,PPM+DebugConsole,PPM+ComBridge,PPM+MSP,PPM+MAVLink,PPM+GPS,Outputs,Telemetry,DebugConsole,ComBridge,MSP,MAVLink,GPS,IBus"
|
||||
<field name="RM_RcvrPort" units="function" type="enum" elements="1" options="Disabled,PWM,PPM,PPM+PWM,PPM+Outputs,PPM+Telemetry,PPM+DebugConsole,PPM+ComBridge,PPM+MSP,PPM+MAVLink,PPM+GPS,PPM+HoTT,Outputs,Telemetry,DebugConsole,ComBridge,MSP,MAVLink,GPS,HoTT,IBus"
|
||||
defaultvalue="PWM"
|
||||
limits="%0905NE:PPM+PWM:PPM+Telemetry:PPM+DebugConsole:PPM+ComBridge:PPM+MSP:PPM+MAVLink:PPM+GPS:Telemetry:DebugConsole:ComBridge:MSP:MAVLink:GPS:IBus;"/>
|
||||
limits="%0905NE:PPM+PWM:PPM+Telemetry:PPM+DebugConsole:PPM+ComBridge:PPM+MSP:PPM+MAVLink:PPM+GPS:PPM+HoTT:Telemetry:DebugConsole:ComBridge:MSP:MAVLink:GPS:HoTT:IBus;"/>
|
||||
<field name="RM_MainPort" units="function" type="enum" elements="1" options="Disabled,Telemetry,GPS,S.Bus,DSM,DebugConsole,ComBridge,OsdHk,MSP,MAVLink" defaultvalue="Disabled"/>
|
||||
<field name="RM_FlexiPort" units="function" type="enum" elements="1" options="Disabled,Telemetry,GPS,I2C,DSM,EX.Bus,HoTT SUMD,HoTT SUMH,SRXL,IBus,DebugConsole,ComBridge,OsdHk,MSP,MAVLink" defaultvalue="Disabled"/>
|
||||
|
||||
|
||||
@ -31,6 +31,7 @@
|
||||
<!-- optional -->
|
||||
<elementname>GPS</elementname>
|
||||
<elementname>OSDGen</elementname>
|
||||
<elementname>UAVOHoTTBridge</elementname>
|
||||
<elementname>UAVOMSPBridge</elementname>
|
||||
<elementname>AutoTune</elementname>
|
||||
<elementname>UAVOMAVLinkBridge</elementname>
|
||||
@ -66,6 +67,7 @@
|
||||
<!-- optional -->
|
||||
<elementname>GPS</elementname>
|
||||
<elementname>OSDGen</elementname>
|
||||
<elementname>UAVOHoTTBridge</elementname>
|
||||
<elementname>UAVOMSPBridge</elementname>
|
||||
<elementname>AutoTune</elementname>
|
||||
<elementname>UAVOMAVLinkBridge</elementname>
|
||||
@ -105,6 +107,7 @@
|
||||
<!-- optional -->
|
||||
<elementname>GPS</elementname>
|
||||
<elementname>OSDGen</elementname>
|
||||
<elementname>UAVOHoTTBridge</elementname>
|
||||
<elementname>UAVOMSPBridge</elementname>
|
||||
<elementname>AutoTune</elementname>
|
||||
<elementname>UAVOMAVLinkBridge</elementname>
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user