diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index f53b7a9571..38391e7e4c 100755
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -2204,7 +2204,7 @@ static void updateNavigationFlightStatistics(void)
}
}
-int32_t getTotalTravelDistance(void)
+uint32_t getTotalTravelDistance(void)
{
return lrintf(posControl.totalTripDistance);
}
@@ -3226,7 +3226,9 @@ int32_t navigationGetHomeHeading(void)
// returns m/s
float calculateAverageSpeed() {
- return (float)getTotalTravelDistance() / (getFlightTime() * 100);
+ float flightTime = getFlightTime();
+ if (flightTime == 0.0f) return 0;
+ return (float)getTotalTravelDistance() / (flightTime * 100);
}
const navigationPIDControllers_t* getNavigationPIDControllers(void) {
diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h
index ac0219537c..824faca581 100755
--- a/src/main/navigation/navigation.h
+++ b/src/main/navigation/navigation.h
@@ -404,7 +404,7 @@ typedef struct {
float getEstimatedActualVelocity(int axis);
float getEstimatedActualPosition(int axis);
-int32_t getTotalTravelDistance(void);
+uint32_t getTotalTravelDistance(void);
void getEstimatedPositionAndVelocity(navPositionAndVelocity_t * pos);
/* Waypoint list access functions */
diff --git a/src/main/telemetry/sim.c b/src/main/telemetry/sim.c
index c4f9550d57..3d4e107fea 100644
--- a/src/main/telemetry/sim.c
+++ b/src/main/telemetry/sim.c
@@ -1,46 +1,119 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight 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.
+ *
+ * Cleanflight 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 Cleanflight. If not, see .
+ */
+
#include "platform.h"
#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_SIM)
#include
-#include "common/axis.h"
-#include "common/streambuf.h"
-#include "common/utils.h"
#include "common/printf.h"
#include "drivers/time.h"
-#include "fc/config.h"
#include "fc/fc_core.h"
-#include "fc/rc_controls.h"
#include "fc/runtime_config.h"
-#include "fc/fc_init.h"
#include "flight/imu.h"
-#include "flight/failsafe.h"
-#include "flight/pid.h"
#include "io/gps.h"
#include "io/serial.h"
#include "navigation/navigation.h"
-#include "navigation/navigation_private.h"
-#include "sensors/acceleration.h"
#include "sensors/battery.h"
-#include "sensors/boardalignment.h"
-#include "sensors/diagnostics.h"
-#include "sensors/gyro.h"
#include "sensors/sensors.h"
#include "common/string_light.h"
#include "common/typeconversion.h"
-#include "build/debug.h"
#include "telemetry/sim.h"
#include "telemetry/telemetry.h"
+
+#define SIM_AT_COMMAND_MAX_SIZE 255
+#define SIM_RESPONSE_BUFFER_SIZE 255
+#define SIM_CYCLE_MS 5000 // wait between sim command cycles
+#define SIM_AT_COMMAND_DELAY_MS 3000
+#define SIM_AT_COMMAND_DELAY_MIN_MS 500
+#define SIM_STARTUP_DELAY_MS 10000
+#define SIM_SMS_COMMAND_RTH "RTH"
+#define SIM_PIN "0000"
+#define SIM_LOW_ALT_WARNING_MODES (NAV_ALTHOLD_MODE || NAV_RTH_MODE || NAV_WP_MODE || FAILSAFE_MODE)
+
+#define SIM_RESPONSE_CODE_OK ('O' << 24 | 'K' << 16)
+#define SIM_RESPONSE_CODE_ERROR ('E' << 24 | 'R' << 16 | 'R' << 8 | 'O')
+#define SIM_RESPONSE_CODE_RING ('R' << 24 | 'I' << 16 | 'N' << 8 | 'G')
+#define SIM_RESPONSE_CODE_CLIP ('C' << 24 | 'L' << 16 | 'I' << 8 | 'P')
+#define SIM_RESPONSE_CODE_CREG ('C' << 24 | 'R' << 16 | 'E' << 8 | 'G')
+#define SIM_RESPONSE_CODE_CSQ ('C' << 24 | 'S' << 16 | 'Q' << 8 | ':')
+#define SIM_RESPONSE_CODE_CMT ('C' << 24 | 'M' << 16 | 'T' << 8 | ':')
+
+
+typedef enum {
+ SIM_TX_FLAG = (1 << 0),
+ SIM_TX_FLAG_FAILSAFE = (1 << 1),
+ SIM_TX_FLAG_GPS = (1 << 2),
+ SIM_TX_FLAG_ACC = (1 << 3),
+ SIM_TX_FLAG_LOW_ALT = (1 << 4),
+ SIM_TX_FLAG_RESPONSE = (1 << 5)
+} simTxFlags_e;
+
+typedef enum {
+ SIM_MODULE_NOT_DETECTED = 0,
+ SIM_MODULE_NOT_REGISTERED,
+ SIM_MODULE_REGISTERED,
+} simModuleState_e;
+
+typedef enum {
+ SIM_STATE_INIT = 0,
+ SIM_STATE_INIT2,
+ SIM_STATE_INIT_ENTER_PIN,
+ SIM_STATE_SET_MODES,
+ SIM_STATE_SEND_SMS,
+ SIM_STATE_SEND_SMS_ENTER_MESSAGE
+} simTelemetryState_e;
+
+typedef enum {
+ SIM_AT_OK = 0,
+ SIM_AT_ERROR,
+ SIM_AT_WAITING_FOR_RESPONSE
+} simATCommandState_e;
+
+typedef enum {
+ SIM_READSTATE_RESPONSE = 0,
+ SIM_READSTATE_SMS,
+ SIM_READSTATE_SKIP
+} simReadState_e;
+
+typedef enum {
+ SIM_TX_NO = 0,
+ SIM_TX_FS,
+ SIM_TX
+} simTransmissionState_e;
+
+typedef enum {
+ ACC_EVENT_NONE = 0,
+ ACC_EVENT_HIGH,
+ ACC_EVENT_LOW,
+ ACC_EVENT_NEG_X
+} accEvent_t;
+
+
static serialPort_t *simPort;
static serialPortConfig_t *portConfig;
static bool simEnabled = false;
@@ -55,20 +128,23 @@ static uint8_t readState = SIM_READSTATE_RESPONSE;
static uint8_t transmitFlags = 0;
static timeMs_t t_lastMessageSent = 0;
static uint8_t lastMessageTriggeredBy = 0;
-uint8_t simModuleState = SIM_MODULE_NOT_DETECTED;
+static uint8_t simModuleState = SIM_MODULE_NOT_DETECTED;
-int simRssi;
-uint8_t accEvent = ACC_EVENT_NONE;
-char* accEventDescriptions[] = { "", "HIT! ", "DROP ", "HIT " };
-char* modeDescriptions[] = { "MAN", "ACR", "ANG", "HOR", "ALH", "POS", "RTH", "WP", "LAU", "FS" };
-const char gpsFixIndicators[] = { '!', '*', ' ' };
+static int simRssi;
+static uint8_t accEvent = ACC_EVENT_NONE;
+static char* accEventDescriptions[] = { "", "HIT! ", "DROP ", "HIT " };
+static char* modeDescriptions[] = { "MAN", "ACR", "ANG", "HOR", "ALH", "POS", "RTH", "WP", "LAU", "FS" };
+static const char gpsFixIndicators[] = { '!', '*', ' ' };
-bool isGroundStationNumberDefined() {
+// XXX UNUSED
+#if 0
+static bool isGroundStationNumberDefined(void) {
return telemetryConfig()->simGroundStationNumber[0] != '\0';
}
+#endif
-bool checkGroundStationNumber(uint8_t* rv)
+static bool checkGroundStationNumber(uint8_t* rv)
{
int i;
const uint8_t* gsn = telemetryConfig()->simGroundStationNumber;
@@ -99,7 +175,7 @@ bool checkGroundStationNumber(uint8_t* rv)
}
-void readOriginatingNumber(uint8_t* rv)
+static void readOriginatingNumber(uint8_t* rv)
{
int i;
uint8_t* gsn = telemetryConfigMutable()->simGroundStationNumber;
@@ -110,7 +186,7 @@ void readOriginatingNumber(uint8_t* rv)
gsn[i] = '\0';
}
-void readTransmitFlags(const uint8_t* fs)
+static void readTransmitFlags(const uint8_t* fs)
{
int i;
@@ -136,7 +212,7 @@ void readTransmitFlags(const uint8_t* fs)
}
}
-void requestSendSMS(uint8_t trigger)
+static void requestSendSMS(uint8_t trigger)
{
lastMessageTriggeredBy = trigger;
if (simTelemetryState == SIM_STATE_SEND_SMS_ENTER_MESSAGE)
@@ -146,10 +222,10 @@ void requestSendSMS(uint8_t trigger)
sim_t_stateChange = 0; // send immediately
}
-void readSMS()
+static void readSMS(void)
{
if (sl_strcasecmp((char*)simResponse, SIM_SMS_COMMAND_RTH) == 0) {
- if (!posControl.flags.forcedRTHActivated) {
+ if (getStateOfForcedRTH() == RTH_IDLE) {
activateForcedRTH();
} else {
abortForcedRTH();
@@ -160,7 +236,7 @@ void readSMS()
requestSendSMS(SIM_TX_FLAG_RESPONSE);
}
-void readSimResponse()
+static void readSimResponse(void)
{
if (readState == SIM_READSTATE_SKIP) {
readState = SIM_READSTATE_RESPONSE;
@@ -226,7 +302,7 @@ void readSimResponse()
}
}
-int getAltMeters()
+static int16_t getAltitudeMeters(void)
{
#if defined(USE_NAV)
return getEstimatedActualPosition(Z) / 100;
@@ -235,7 +311,7 @@ int getAltMeters()
#endif
}
-void transmit()
+static void transmit(void)
{
timeMs_t timeSinceMsg = millis() - t_lastMessageSent;
uint8_t triggers = SIM_TX_FLAG;
@@ -257,9 +333,9 @@ void transmit()
if (FLIGHT_MODE(FAILSAFE_MODE))
triggers |= SIM_TX_FLAG_FAILSAFE;
- if (gpsSol.fixType != GPS_NO_FIX && posControl.flags.estPosStatus < EST_USABLE)
+ if (!navigationPositionEstimateIsHealthy())
triggers |= SIM_TX_FLAG_GPS;
- if (gpsSol.fixType != GPS_NO_FIX && FLIGHT_MODE(SIM_LOW_ALT_WARNING_MODES) && getAltMeters() < telemetryConfig()->simLowAltitude)
+ if (gpsSol.fixType != GPS_NO_FIX && FLIGHT_MODE(SIM_LOW_ALT_WARNING_MODES) && getAltitudeMeters() < telemetryConfig()->simLowAltitude)
triggers |= SIM_TX_FLAG_LOW_ALT;
triggers &= transmitFlags;
@@ -275,50 +351,44 @@ void transmit()
}
}
-void sendATCommand(const char* command)
+static void sendATCommand(const char* command)
{
atCommandStatus = SIM_AT_WAITING_FOR_RESPONSE;
- int len = strlen((char*)command);
- if (len > SIM_AT_COMMAND_MAX_SIZE)
- len = SIM_AT_COMMAND_MAX_SIZE;
+ uint8_t len = MIN((uint8_t)strlen(command), SIM_AT_COMMAND_MAX_SIZE);
serialWriteBuf(simPort, (const uint8_t*) command, len);
}
-void sendSMS(void)
+static void sendSMS(void)
{
int32_t lat = 0, lon = 0;
- int16_t gs = 0, gc = 0;
- int vbat = getBatteryVoltage();
+ int16_t groundSpeed = 0;
+ uint16_t vbat = getBatteryVoltage();
int16_t amps = isAmperageConfigured() ? getAmperage() / 10 : 0; // 1 = 100 milliamps
- int avgSpeed = (int)round(10 * calculateAverageSpeed());
+ uint16_t avgSpeed = lrintf(10 * calculateAverageSpeed());
uint32_t now = millis();
- if (getFlightTime() == 0.0F) {
- avgSpeed = 0;
- }
-
if (sensors(SENSOR_GPS)) {
lat = gpsSol.llh.lat;
lon = gpsSol.llh.lon;
- gs = gpsSol.groundSpeed / 100;
- gc = gpsSol.groundCourse / 10;
+ groundSpeed = gpsSol.groundSpeed / 100;
}
- int len;
- int32_t E7 = 10000000;
+ const int32_t E7 = 10000000;
+
// \x1a sends msg, \x1b cancels
- len = tfp_sprintf((char*)atCommand, "%s%d.%02dV %d.%dA ALT:%ld SPD:%ld/%d.%d DIS:%d/%d COG:%d SAT:%d%c SIG:%d %s maps.google.com/?q=@%ld.%07ld,%ld.%07ld\x1a",
+ uint8_t len = tfp_sprintf((char*)atCommand, "%s%d.%02dV %d.%dA ALT:%d SPD:%d/%d.%d DIS:%lu/%lu HDG:%d SAT:%d%c SIG:%d %s maps.google.com/?q=@%ld.%07ld,%ld.%07ld\x1a",
accEventDescriptions[accEvent],
vbat / 100, vbat % 100,
amps / 10, amps % 10,
- getAltMeters(),
- gs, avgSpeed / 10, avgSpeed % 10,
+ getAltitudeMeters(),
+ groundSpeed, avgSpeed / 10, avgSpeed % 10,
GPS_distanceToHome, getTotalTravelDistance() / 100,
- gc,
+ attitude.values.yaw,
gpsSol.numSat, gpsFixIndicators[gpsSol.fixType],
simRssi,
- posControl.flags.forcedRTHActivated ? "RTH" : modeDescriptions[getFlightModeForTelemetry()],
+ getStateOfForcedRTH() == RTH_IDLE ? modeDescriptions[getFlightModeForTelemetry()] : "RTH",
lat / E7, lat % E7, lon / E7, lon % E7);
+
serialWriteBuf(simPort, atCommand, len);
t_lastMessageSent = now;
accEvent = ACC_EVENT_NONE;
@@ -355,8 +425,8 @@ void handleSimTelemetry()
if (now < sim_t_stateChange)
return;
- sim_t_stateChange = now + SIM_AT_COMMAND_DELAY_MS; // by default, if OK or ERROR not received, wait this long
- simWaitAfterResponse = false; // by default, if OK or ERROR received, go to next state immediately.
+ sim_t_stateChange = now + SIM_AT_COMMAND_DELAY_MS; // by default, if OK or ERROR not received, wait this long
+ simWaitAfterResponse = false; // by default, if OK or ERROR received, go to next state immediately.
switch (simTelemetryState) {
case SIM_STATE_INIT:
sendATCommand("AT\r");
@@ -392,27 +462,22 @@ void handleSimTelemetry()
}
}
-void freeSimTelemetryPort(void)
+// XXX UNUSED
+#if 0
+static void freeSimTelemetryPort(void)
{
closeSerialPort(simPort);
simPort = NULL;
simEnabled = false;
}
+#endif
void initSimTelemetry(void)
{
portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_SIM);
}
-void checkSimTelemetryState(void)
-{
- if (simEnabled) {
- return;
- }
- configureSimTelemetryPort();
-}
-
-void configureSimTelemetryPort(void)
+static void configureSimTelemetryPort(void)
{
if (!portConfig) {
return;
@@ -432,4 +497,12 @@ void configureSimTelemetryPort(void)
simEnabled = true;
}
+void checkSimTelemetryState(void)
+{
+ if (simEnabled) {
+ return;
+ }
+ configureSimTelemetryPort();
+}
+
#endif
diff --git a/src/main/telemetry/sim.h b/src/main/telemetry/sim.h
index 182f496216..2df17f1d3d 100644
--- a/src/main/telemetry/sim.h
+++ b/src/main/telemetry/sim.h
@@ -1,78 +1,28 @@
-#define SIM_AT_COMMAND_MAX_SIZE 256
-#define SIM_RESPONSE_BUFFER_SIZE 256
-#define SIM_CYCLE_MS 5000 // wait between sim command cycles
-#define SIM_AT_COMMAND_DELAY_MS 3000
-#define SIM_AT_COMMAND_DELAY_MIN_MS 500
-#define SIM_STARTUP_DELAY_MS 10000
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight 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.
+ *
+ * Cleanflight 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 Cleanflight. If not, see .
+ */
+
+#pragma once
+
#define SIM_MIN_TRANSMIT_INTERVAL 10
#define SIM_DEFAULT_TRANSMIT_INTERVAL 60
-#define SIM_SMS_COMMAND_RTH "RTH"
-#define SIM_PIN "0000"
-#define SIM_LOW_ALT_WARNING_MODES (NAV_ALTHOLD_MODE || NAV_RTH_MODE || NAV_WP_MODE || FAILSAFE_MODE)
-
-#define SIM_RESPONSE_CODE_OK ('O' << 24 | 'K' << 16)
-#define SIM_RESPONSE_CODE_ERROR ('E' << 24 | 'R' << 16 | 'R' << 8 | 'O')
-#define SIM_RESPONSE_CODE_RING ('R' << 24 | 'I' << 16 | 'N' << 8 | 'G')
-#define SIM_RESPONSE_CODE_CLIP ('C' << 24 | 'L' << 16 | 'I' << 8 | 'P')
-#define SIM_RESPONSE_CODE_CREG ('C' << 24 | 'R' << 16 | 'E' << 8 | 'G')
-#define SIM_RESPONSE_CODE_CSQ ('C' << 24 | 'S' << 16 | 'Q' << 8 | ':')
-#define SIM_RESPONSE_CODE_CMT ('C' << 24 | 'M' << 16 | 'T' << 8 | ':')
-
-typedef enum {
- SIM_TX_FLAG = (1 << 0),
- SIM_TX_FLAG_FAILSAFE = (1 << 1),
- SIM_TX_FLAG_GPS = (1 << 2),
- SIM_TX_FLAG_ACC = (1 << 3),
- SIM_TX_FLAG_LOW_ALT = (1 << 4),
- SIM_TX_FLAG_RESPONSE = (1 << 5)
-} simTxFlags_e;
-
#define SIM_N_TX_FLAGS 5
#define SIM_DEFAULT_TX_FLAGS "f"
-typedef enum {
- SIM_MODULE_NOT_DETECTED = 0,
- SIM_MODULE_NOT_REGISTERED,
- SIM_MODULE_REGISTERED,
-} simModuleState_e;
-
-typedef enum {
- SIM_STATE_INIT = 0,
- SIM_STATE_INIT2,
- SIM_STATE_INIT_ENTER_PIN,
- SIM_STATE_SET_MODES,
- SIM_STATE_SEND_SMS,
- SIM_STATE_SEND_SMS_ENTER_MESSAGE
-} simTelemetryState_e;
-
-typedef enum {
- SIM_AT_OK = 0,
- SIM_AT_ERROR,
- SIM_AT_WAITING_FOR_RESPONSE
-} simATCommandState_e;
-
-typedef enum {
- SIM_READSTATE_RESPONSE = 0,
- SIM_READSTATE_SMS,
- SIM_READSTATE_SKIP
-} simReadState_e;
-
-typedef enum {
- SIM_TX_NO = 0,
- SIM_TX_FS,
- SIM_TX
-} simTransmissionState_e;
-
-typedef enum {
- ACC_EVENT_NONE = 0,
- ACC_EVENT_HIGH,
- ACC_EVENT_LOW,
- ACC_EVENT_NEG_X
-} accEvent_t;
-
void handleSimTelemetry(void);
-void freeSimTelemetryPort(void);
void initSimTelemetry(void);
void checkSimTelemetryState(void);
-void configureSimTelemetryPort(void);