mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-15 04:15:44 +03:00
Remove telemetry_*.c's dependency on mw.h/board.h.
Finally, the only dependency on mw.h/board.h is now mw.c itself.
This commit is contained in:
parent
7af9ca4fdc
commit
cb63f6e2b5
10 changed files with 96 additions and 34 deletions
16
src/config.c
16
src/config.c
|
@ -91,7 +91,8 @@ void activateConfig(void)
|
||||||
generatePitchCurve(¤tProfile.controlRateConfig);
|
generatePitchCurve(¤tProfile.controlRateConfig);
|
||||||
generateThrottleCurve(¤tProfile.controlRateConfig, &masterConfig.escAndServoConfig);
|
generateThrottleCurve(¤tProfile.controlRateConfig, &masterConfig.escAndServoConfig);
|
||||||
|
|
||||||
useGyroConfig(&masterConfig.gyroConfig);
|
useGyroConfig(&masterConfig.gyroConfig);
|
||||||
|
useTelemetryConfig(&masterConfig.telemetryConfig);
|
||||||
setPIDController(currentProfile.pidController);
|
setPIDController(currentProfile.pidController);
|
||||||
gpsUseProfile(¤tProfile.gpsProfile);
|
gpsUseProfile(¤tProfile.gpsProfile);
|
||||||
gpsUsePIDs(¤tProfile.pidProfile);
|
gpsUsePIDs(¤tProfile.pidProfile);
|
||||||
|
@ -271,6 +272,13 @@ void resetFlight3DConfig(flight3DConfig_t *flight3DConfig)
|
||||||
flight3DConfig->deadband3d_throttle = 50;
|
flight3DConfig->deadband3d_throttle = 50;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void resetTelemetryConfig(telemetryConfig_t *telemetryConfig)
|
||||||
|
{
|
||||||
|
telemetryConfig->telemetry_provider = TELEMETRY_PROVIDER_FRSKY;
|
||||||
|
telemetryConfig->telemetry_port = TELEMETRY_PORT_UART;
|
||||||
|
telemetryConfig->telemetry_switch = 0;
|
||||||
|
|
||||||
|
}
|
||||||
// Default settings
|
// Default settings
|
||||||
static void resetConf(void)
|
static void resetConf(void)
|
||||||
{
|
{
|
||||||
|
@ -307,9 +315,9 @@ static void resetConf(void)
|
||||||
masterConfig.batteryConfig.vbatmaxcellvoltage = 43;
|
masterConfig.batteryConfig.vbatmaxcellvoltage = 43;
|
||||||
masterConfig.batteryConfig.vbatmincellvoltage = 33;
|
masterConfig.batteryConfig.vbatmincellvoltage = 33;
|
||||||
masterConfig.power_adc_channel = 0;
|
masterConfig.power_adc_channel = 0;
|
||||||
masterConfig.telemetry_provider = TELEMETRY_PROVIDER_FRSKY;
|
|
||||||
masterConfig.telemetry_port = TELEMETRY_PORT_UART;
|
resetTelemetryConfig(&masterConfig.telemetryConfig);
|
||||||
masterConfig.telemetry_switch = 0;
|
|
||||||
masterConfig.rxConfig.serialrx_type = 0;
|
masterConfig.rxConfig.serialrx_type = 0;
|
||||||
masterConfig.rxConfig.midrc = 1500;
|
masterConfig.rxConfig.midrc = 1500;
|
||||||
masterConfig.rxConfig.mincheck = 1100;
|
masterConfig.rxConfig.mincheck = 1100;
|
||||||
|
|
|
@ -55,9 +55,8 @@ typedef struct master_t {
|
||||||
int8_t gps_baudrate; // See GPSBaudRates enum.
|
int8_t gps_baudrate; // See GPSBaudRates enum.
|
||||||
|
|
||||||
serialConfig_t serialConfig;
|
serialConfig_t serialConfig;
|
||||||
uint8_t telemetry_provider; // See TelemetryProvider enum.
|
|
||||||
uint8_t telemetry_port; // See TelemetryPort enum.
|
telemetryConfig_t telemetryConfig;
|
||||||
uint8_t telemetry_switch; // Use aux channel to change serial output & baudrate( MSP / Telemetry ). It disables automatic switching to Telemetry when armed.
|
|
||||||
profile_t profile[3]; // 3 separate profiles
|
profile_t profile[3]; // 3 separate profiles
|
||||||
uint8_t current_profile_index; // currently loaded profile
|
uint8_t current_profile_index; // currently loaded profile
|
||||||
|
|
||||||
|
|
|
@ -23,11 +23,13 @@
|
||||||
#include "gimbal.h"
|
#include "gimbal.h"
|
||||||
#include "flight_mixer.h"
|
#include "flight_mixer.h"
|
||||||
|
|
||||||
|
// FIXME remove dependency on config.h
|
||||||
#include "boardalignment.h"
|
#include "boardalignment.h"
|
||||||
#include "battery.h"
|
#include "battery.h"
|
||||||
#include "escservo.h"
|
#include "escservo.h"
|
||||||
#include "rc_controls.h"
|
#include "rc_controls.h"
|
||||||
#include "rx_common.h"
|
#include "rx_common.h"
|
||||||
|
#include "telemetry_common.h"
|
||||||
#include "drivers/serial_common.h"
|
#include "drivers/serial_common.h"
|
||||||
#include "serial_common.h"
|
#include "serial_common.h"
|
||||||
#include "failsafe.h"
|
#include "failsafe.h"
|
||||||
|
|
1
src/mw.h
1
src/mw.h
|
@ -24,6 +24,7 @@ enum {
|
||||||
#include "serial_common.h"
|
#include "serial_common.h"
|
||||||
#include "rx_common.h"
|
#include "rx_common.h"
|
||||||
#include "gps_common.h"
|
#include "gps_common.h"
|
||||||
|
#include "telemetry_common.h"
|
||||||
#include "config.h"
|
#include "config.h"
|
||||||
#include "config_profile.h"
|
#include "config_profile.h"
|
||||||
#include "config_master.h"
|
#include "config_master.h"
|
||||||
|
|
|
@ -176,9 +176,9 @@ const clivalue_t valueTable[] = {
|
||||||
|
|
||||||
{ "serialrx_type", VAR_UINT8, &masterConfig.rxConfig.serialrx_type, 0, 3 },
|
{ "serialrx_type", VAR_UINT8, &masterConfig.rxConfig.serialrx_type, 0, 3 },
|
||||||
|
|
||||||
{ "telemetry_provider", VAR_UINT8, &masterConfig.telemetry_provider, 0, TELEMETRY_PROVIDER_MAX },
|
{ "telemetry_provider", VAR_UINT8, &masterConfig.telemetryConfig.telemetry_provider, 0, TELEMETRY_PROVIDER_MAX },
|
||||||
{ "telemetry_port", VAR_UINT8, &masterConfig.telemetry_port, 0, TELEMETRY_PORT_MAX },
|
{ "telemetry_port", VAR_UINT8, &masterConfig.telemetryConfig.telemetry_port, 0, TELEMETRY_PORT_MAX },
|
||||||
{ "telemetry_switch", VAR_UINT8, &masterConfig.telemetry_switch, 0, 1 },
|
{ "telemetry_switch", VAR_UINT8, &masterConfig.telemetryConfig.telemetry_switch, 0, 1 },
|
||||||
|
|
||||||
{ "vbatscale", VAR_UINT8, &masterConfig.batteryConfig.vbatscale, 10, 200 },
|
{ "vbatscale", VAR_UINT8, &masterConfig.batteryConfig.vbatscale, 10, 200 },
|
||||||
{ "vbatmaxcellvoltage", VAR_UINT8, &masterConfig.batteryConfig.vbatmaxcellvoltage, 10, 50 },
|
{ "vbatmaxcellvoltage", VAR_UINT8, &masterConfig.batteryConfig.vbatmaxcellvoltage, 10, 50 },
|
||||||
|
|
|
@ -21,6 +21,7 @@
|
||||||
#include "rx_common.h"
|
#include "rx_common.h"
|
||||||
#include "battery.h"
|
#include "battery.h"
|
||||||
#include "gimbal.h"
|
#include "gimbal.h"
|
||||||
|
#include "telemetry_common.h"
|
||||||
#include "sensors_common.h"
|
#include "sensors_common.h"
|
||||||
#include "sensors_acceleration.h"
|
#include "sensors_acceleration.h"
|
||||||
#include "sensors_barometer.h"
|
#include "sensors_barometer.h"
|
||||||
|
@ -308,7 +309,7 @@ void mspInit(void)
|
||||||
if (feature(FEATURE_INFLIGHT_ACC_CAL))
|
if (feature(FEATURE_INFLIGHT_ACC_CAL))
|
||||||
availableBoxes[idx++] = BOXCALIB;
|
availableBoxes[idx++] = BOXCALIB;
|
||||||
availableBoxes[idx++] = BOXOSD;
|
availableBoxes[idx++] = BOXOSD;
|
||||||
if (feature(FEATURE_TELEMETRY && masterConfig.telemetry_switch))
|
if (feature(FEATURE_TELEMETRY && masterConfig.telemetryConfig.telemetry_switch))
|
||||||
availableBoxes[idx++] = BOXTELEMETRY;
|
availableBoxes[idx++] = BOXTELEMETRY;
|
||||||
numberBoxItems = idx;
|
numberBoxItems = idx;
|
||||||
}
|
}
|
||||||
|
|
|
@ -1,9 +1,16 @@
|
||||||
#include "board.h"
|
#include <stdbool.h>
|
||||||
#include "mw.h"
|
#include <stdint.h>
|
||||||
|
|
||||||
|
#include "platform.h"
|
||||||
|
|
||||||
|
#include "drivers/timer_common.h"
|
||||||
#include "drivers/serial_common.h"
|
#include "drivers/serial_common.h"
|
||||||
|
#include "drivers/serial_softserial.h"
|
||||||
#include "serial_common.h"
|
#include "serial_common.h"
|
||||||
|
|
||||||
|
#include "runtime_config.h"
|
||||||
|
#include "config.h"
|
||||||
|
|
||||||
#include "telemetry_frsky.h"
|
#include "telemetry_frsky.h"
|
||||||
#include "telemetry_hott.h"
|
#include "telemetry_hott.h"
|
||||||
|
|
||||||
|
@ -11,14 +18,21 @@
|
||||||
|
|
||||||
static bool isTelemetryConfigurationValid = false; // flag used to avoid repeated configuration checks
|
static bool isTelemetryConfigurationValid = false; // flag used to avoid repeated configuration checks
|
||||||
|
|
||||||
|
telemetryConfig_t *telemetryConfig;
|
||||||
|
|
||||||
|
void useTelemetryConfig(telemetryConfig_t *telemetryConfigToUse)
|
||||||
|
{
|
||||||
|
telemetryConfig = telemetryConfigToUse;
|
||||||
|
}
|
||||||
|
|
||||||
bool isTelemetryProviderFrSky(void)
|
bool isTelemetryProviderFrSky(void)
|
||||||
{
|
{
|
||||||
return masterConfig.telemetry_provider == TELEMETRY_PROVIDER_FRSKY;
|
return telemetryConfig->telemetry_provider == TELEMETRY_PROVIDER_FRSKY;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool isTelemetryProviderHoTT(void)
|
bool isTelemetryProviderHoTT(void)
|
||||||
{
|
{
|
||||||
return masterConfig.telemetry_provider == TELEMETRY_PROVIDER_HOTT;
|
return telemetryConfig->telemetry_provider == TELEMETRY_PROVIDER_HOTT;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool canUseTelemetryWithCurrentConfiguration(void)
|
bool canUseTelemetryWithCurrentConfiguration(void)
|
||||||
|
@ -28,14 +42,14 @@ bool canUseTelemetryWithCurrentConfiguration(void)
|
||||||
}
|
}
|
||||||
|
|
||||||
if (!feature(FEATURE_SOFTSERIAL)) {
|
if (!feature(FEATURE_SOFTSERIAL)) {
|
||||||
if (masterConfig.telemetry_port == TELEMETRY_PORT_SOFTSERIAL_1 || masterConfig.telemetry_port == TELEMETRY_PORT_SOFTSERIAL_2) {
|
if (telemetryConfig->telemetry_port == TELEMETRY_PORT_SOFTSERIAL_1 || telemetryConfig->telemetry_port == TELEMETRY_PORT_SOFTSERIAL_2) {
|
||||||
// softserial feature must be enabled to use telemetry on softserial ports
|
// softserial feature must be enabled to use telemetry on softserial ports
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
if (isTelemetryProviderHoTT()) {
|
if (isTelemetryProviderHoTT()) {
|
||||||
if (masterConfig.telemetry_port == TELEMETRY_PORT_UART) {
|
if (telemetryConfig->telemetry_port == TELEMETRY_PORT_UART) {
|
||||||
// HoTT requires a serial port that supports RX/TX mode swapping
|
// HoTT requires a serial port that supports RX/TX mode swapping
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
@ -48,20 +62,20 @@ void initTelemetry(serialPorts_t *serialPorts)
|
||||||
{
|
{
|
||||||
// Force telemetry to uart when softserial disabled
|
// Force telemetry to uart when softserial disabled
|
||||||
if (!feature(FEATURE_SOFTSERIAL))
|
if (!feature(FEATURE_SOFTSERIAL))
|
||||||
masterConfig.telemetry_port = TELEMETRY_PORT_UART;
|
telemetryConfig->telemetry_port = TELEMETRY_PORT_UART;
|
||||||
|
|
||||||
#ifdef FY90Q
|
#ifdef FY90Q
|
||||||
// FY90Q does not support softserial
|
// FY90Q does not support softserial
|
||||||
masterConfig.telemetry_port = TELEMETRY_PORT_UART;
|
telemetryConfig->telemetry_port = TELEMETRY_PORT_UART;
|
||||||
serialPorts->telemport = serialPorts->mainport;
|
serialPorts->telemport = serialPorts->mainport;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
isTelemetryConfigurationValid = canUseTelemetryWithCurrentConfiguration();
|
isTelemetryConfigurationValid = canUseTelemetryWithCurrentConfiguration();
|
||||||
|
|
||||||
#ifndef FY90Q
|
#ifndef FY90Q
|
||||||
if (masterConfig.telemetry_port == TELEMETRY_PORT_SOFTSERIAL_1)
|
if (telemetryConfig->telemetry_port == TELEMETRY_PORT_SOFTSERIAL_1)
|
||||||
serialPorts->telemport = &(softSerialPorts[0].port);
|
serialPorts->telemport = &(softSerialPorts[0].port);
|
||||||
else if (masterConfig.telemetry_port == TELEMETRY_PORT_SOFTSERIAL_2)
|
else if (telemetryConfig->telemetry_port == TELEMETRY_PORT_SOFTSERIAL_2)
|
||||||
serialPorts->telemport = &(softSerialPorts[1].port);
|
serialPorts->telemport = &(softSerialPorts[1].port);
|
||||||
else
|
else
|
||||||
serialPorts->telemport = serialPorts->mainport;
|
serialPorts->telemport = serialPorts->mainport;
|
||||||
|
@ -76,8 +90,8 @@ bool determineNewTelemetryEnabledState(void)
|
||||||
{
|
{
|
||||||
bool enabled = true;
|
bool enabled = true;
|
||||||
|
|
||||||
if (masterConfig.telemetry_port == TELEMETRY_PORT_UART) {
|
if (telemetryConfig->telemetry_port == TELEMETRY_PORT_UART) {
|
||||||
if (!masterConfig.telemetry_switch)
|
if (!telemetryConfig->telemetry_switch)
|
||||||
enabled = f.ARMED;
|
enabled = f.ARMED;
|
||||||
else
|
else
|
||||||
enabled = rcOptions[BOXTELEMETRY];
|
enabled = rcOptions[BOXTELEMETRY];
|
||||||
|
|
|
@ -21,7 +21,15 @@ typedef enum {
|
||||||
TELEMETRY_PORT_MAX = TELEMETRY_PORT_SOFTSERIAL_2
|
TELEMETRY_PORT_MAX = TELEMETRY_PORT_SOFTSERIAL_2
|
||||||
} TelemetryPort;
|
} TelemetryPort;
|
||||||
|
|
||||||
|
typedef struct telemetryConfig_s {
|
||||||
|
uint8_t telemetry_provider; // See TelemetryProvider enum.
|
||||||
|
uint8_t telemetry_port; // See TelemetryPort enum.
|
||||||
|
uint8_t telemetry_switch; // Use aux channel to change serial output & baudrate( MSP / Telemetry ). It disables automatic switching to Telemetry when armed.
|
||||||
|
} telemetryConfig_t;
|
||||||
|
|
||||||
void checkTelemetryState(void);
|
void checkTelemetryState(void);
|
||||||
void handleTelemetry(void);
|
void handleTelemetry(void);
|
||||||
|
|
||||||
|
void useTelemetryConfig(telemetryConfig_t *telemetryConfig);
|
||||||
|
|
||||||
#endif /* TELEMETRY_COMMON_H_ */
|
#endif /* TELEMETRY_COMMON_H_ */
|
||||||
|
|
|
@ -1,19 +1,36 @@
|
||||||
/*
|
/*
|
||||||
* FrSky Telemetry implementation by silpstream @ rcgroups
|
* FrSky Telemetry implementation by silpstream @ rcgroups
|
||||||
*/
|
*/
|
||||||
#include "board.h"
|
#include <stdbool.h>
|
||||||
#include "mw.h"
|
#include <stdint.h>
|
||||||
|
|
||||||
#include "flight_common.h"
|
#include "platform.h"
|
||||||
|
|
||||||
|
#include "common/maths.h"
|
||||||
|
#include "common/axis.h"
|
||||||
|
|
||||||
|
#include "drivers/system_common.h"
|
||||||
|
#include "drivers/accgyro_common.h"
|
||||||
|
#include "drivers/timer_common.h"
|
||||||
#include "drivers/serial_common.h"
|
#include "drivers/serial_common.h"
|
||||||
#include "serial_common.h"
|
#include "serial_common.h"
|
||||||
|
|
||||||
|
#include "runtime_config.h"
|
||||||
|
#include "config.h"
|
||||||
|
|
||||||
|
#include "sensors_common.h"
|
||||||
|
#include "sensors_gyro.h"
|
||||||
|
#include "sensors_barometer.h"
|
||||||
|
#include "flight_common.h"
|
||||||
#include "gps_common.h"
|
#include "gps_common.h"
|
||||||
|
#include "battery.h"
|
||||||
|
|
||||||
#include "telemetry_common.h"
|
#include "telemetry_common.h"
|
||||||
#include "telemetry_frsky.h"
|
#include "telemetry_frsky.h"
|
||||||
|
|
||||||
|
extern telemetryConfig_t *telemetryConfig;
|
||||||
|
int16_t telemTemperature1; // FIXME dependency on mw.c
|
||||||
|
|
||||||
#define CYCLETIME 125
|
#define CYCLETIME 125
|
||||||
|
|
||||||
#define PROTOCOL_HEADER 0x5E
|
#define PROTOCOL_HEADER 0x5E
|
||||||
|
@ -57,9 +74,6 @@
|
||||||
|
|
||||||
#define ID_VERT_SPEED 0x30 //opentx vario
|
#define ID_VERT_SPEED 0x30 //opentx vario
|
||||||
|
|
||||||
// from sensors.c
|
|
||||||
extern uint8_t batteryCellCount;
|
|
||||||
|
|
||||||
static void sendDataHead(uint8_t id)
|
static void sendDataHead(uint8_t id)
|
||||||
{
|
{
|
||||||
serialWrite(serialPorts.telemport, PROTOCOL_HEADER);
|
serialWrite(serialPorts.telemport, PROTOCOL_HEADER);
|
||||||
|
@ -224,14 +238,14 @@ static void sendHeading(void)
|
||||||
|
|
||||||
void freeFrSkyTelemetryPort(void)
|
void freeFrSkyTelemetryPort(void)
|
||||||
{
|
{
|
||||||
if (masterConfig.telemetry_port == TELEMETRY_PORT_UART) {
|
if (telemetryConfig->telemetry_port == TELEMETRY_PORT_UART) {
|
||||||
resetMainSerialPort();
|
resetMainSerialPort();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void configureFrSkyTelemetryPort(void)
|
void configureFrSkyTelemetryPort(void)
|
||||||
{
|
{
|
||||||
if (masterConfig.telemetry_port == TELEMETRY_PORT_UART) {
|
if (telemetryConfig->telemetry_port == TELEMETRY_PORT_UART) {
|
||||||
openMainSerialPort(9600);
|
openMainSerialPort(9600);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
@ -40,17 +40,32 @@
|
||||||
* There is a technical discussion (in German) about HoTT here
|
* There is a technical discussion (in German) about HoTT here
|
||||||
* http://www.rc-network.de/forum/showthread.php/281496-Graupner-HoTT-Telemetrie-Sensoren-Eigenbau-DIY-Telemetrie-Protokoll-entschl%C3%BCsselt/page21
|
* http://www.rc-network.de/forum/showthread.php/281496-Graupner-HoTT-Telemetrie-Sensoren-Eigenbau-DIY-Telemetrie-Protokoll-entschl%C3%BCsselt/page21
|
||||||
*/
|
*/
|
||||||
|
#include <stdbool.h>
|
||||||
|
#include <stdint.h>
|
||||||
|
#include <string.h>
|
||||||
|
|
||||||
#include "board.h"
|
#include "platform.h"
|
||||||
#include "mw.h"
|
|
||||||
|
#include "common/axis.h"
|
||||||
|
|
||||||
|
#include "drivers/system_common.h"
|
||||||
|
|
||||||
#include "drivers/serial_common.h"
|
#include "drivers/serial_common.h"
|
||||||
#include "serial_common.h"
|
#include "serial_common.h"
|
||||||
|
|
||||||
#include "gps_common.h"
|
#include "runtime_config.h"
|
||||||
|
|
||||||
|
#include "sensors_common.h"
|
||||||
|
|
||||||
|
#include "flight_common.h"
|
||||||
|
#include "gps_common.h"
|
||||||
|
#include "battery.h"
|
||||||
|
|
||||||
|
#include "telemetry_common.h"
|
||||||
#include "telemetry_hott.h"
|
#include "telemetry_hott.h"
|
||||||
|
|
||||||
|
extern telemetryConfig_t *telemetryConfig;
|
||||||
|
|
||||||
|
|
||||||
const uint8_t kHoTTv4BinaryPacketSize = 45;
|
const uint8_t kHoTTv4BinaryPacketSize = 45;
|
||||||
const uint8_t kHoTTv4TextPacketSize = 173;
|
const uint8_t kHoTTv4TextPacketSize = 173;
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue