Merge pull request #2849 from martinbudden/bf_fake_accgyro_locks
Added locking to fake gyro and acc
This commit is contained in:
commit
3a672c82e8
|
@ -113,9 +113,6 @@ int tfp_sprintf(char *s, const char *fmt, ...);
|
|||
|
||||
int tfp_format(void *putp, void (*putf) (void *, char), const char *fmt, va_list va);
|
||||
|
||||
#define printf tfp_printf
|
||||
#define sprintf tfp_sprintf
|
||||
|
||||
struct serialPort_s;
|
||||
void setPrintfSerialPort(struct serialPort_s *serialPort);
|
||||
void printfSupportInit(void);
|
||||
|
|
|
@ -17,10 +17,14 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include "platform.h"
|
||||
#include "common/axis.h"
|
||||
#include "drivers/exti.h"
|
||||
#include "drivers/sensor.h"
|
||||
#include "drivers/accgyro_mpu.h"
|
||||
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD)
|
||||
#include <pthread.h>
|
||||
#endif
|
||||
|
||||
#ifndef MPU_I2C_INSTANCE
|
||||
#define MPU_I2C_INSTANCE I2C_DEVICE
|
||||
|
@ -57,7 +61,10 @@ typedef struct gyroDev_s {
|
|||
uint8_t lpf;
|
||||
gyroRateKHz_e gyroRateKHz;
|
||||
uint8_t mpuDividerDrops;
|
||||
volatile bool dataReady;
|
||||
bool dataReady;
|
||||
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD)
|
||||
pthread_mutex_t lock;
|
||||
#endif
|
||||
sensor_align_e gyroAlign;
|
||||
mpuDetectionResult_t mpuDetectionResult;
|
||||
const extiConfig_t *mpuIntExtiConfig;
|
||||
|
@ -71,7 +78,47 @@ typedef struct accDev_s {
|
|||
uint16_t acc_1G;
|
||||
int16_t ADCRaw[XYZ_AXIS_COUNT];
|
||||
char revisionCode; // a revision code for the sensor, if known
|
||||
bool dataReady;
|
||||
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD)
|
||||
pthread_mutex_t lock;
|
||||
#endif
|
||||
sensor_align_e accAlign;
|
||||
mpuDetectionResult_t mpuDetectionResult;
|
||||
mpuConfiguration_t mpuConfiguration;
|
||||
} accDev_t;
|
||||
|
||||
static inline void accDevLock(accDev_t *acc)
|
||||
{
|
||||
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD)
|
||||
pthread_mutex_lock(&acc->lock);
|
||||
#else
|
||||
(void)acc;
|
||||
#endif
|
||||
}
|
||||
|
||||
static inline void accDevUnLock(accDev_t *acc)
|
||||
{
|
||||
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD)
|
||||
pthread_mutex_unlock(&acc->lock);
|
||||
#else
|
||||
(void)acc;
|
||||
#endif
|
||||
}
|
||||
|
||||
static inline void gyroDevLock(gyroDev_t *gyro)
|
||||
{
|
||||
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD)
|
||||
pthread_mutex_lock(&gyro->lock);
|
||||
#else
|
||||
(void)gyro;
|
||||
#endif
|
||||
}
|
||||
|
||||
static inline void gyroDevUnLock(gyroDev_t *gyro)
|
||||
{
|
||||
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD)
|
||||
pthread_mutex_unlock(&gyro->lock);
|
||||
#else
|
||||
(void)gyro;
|
||||
#endif
|
||||
}
|
||||
|
|
|
@ -23,23 +23,6 @@
|
|||
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD)
|
||||
#include <stdio.h>
|
||||
#include <pthread.h>
|
||||
#define printf printf
|
||||
#define sprintf sprintf
|
||||
|
||||
#define ACC_LOCK pthread_mutex_unlock(&accUpdateLock)
|
||||
#define ACC_UNLOCK pthread_mutex_unlock(&accUpdateLock)
|
||||
|
||||
#define GYRO_LOCK pthread_mutex_unlock(&gyroUpdateLock)
|
||||
#define GYRO_UNLOCK pthread_mutex_unlock(&gyroUpdateLock)
|
||||
|
||||
#else
|
||||
|
||||
#define ACC_LOCK
|
||||
#define ACC_UNLOCK
|
||||
|
||||
#define GYRO_LOCK
|
||||
#define GYRO_UNLOCK
|
||||
|
||||
#endif
|
||||
|
||||
#ifdef USE_FAKE_GYRO
|
||||
|
@ -51,54 +34,45 @@
|
|||
#include "accgyro_fake.h"
|
||||
|
||||
static int16_t fakeGyroADC[XYZ_AXIS_COUNT];
|
||||
|
||||
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD)
|
||||
static pthread_mutex_t gyroUpdateLock;
|
||||
#endif
|
||||
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_GYRO_SYNC)
|
||||
static bool gyroUpdated = false;
|
||||
#endif
|
||||
gyroDev_t *fakeGyroDev;
|
||||
|
||||
static void fakeGyroInit(gyroDev_t *gyro)
|
||||
{
|
||||
UNUSED(gyro);
|
||||
fakeGyroDev = gyro;
|
||||
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD)
|
||||
if (pthread_mutex_init(&gyroUpdateLock, NULL) != 0) {
|
||||
printf("Create gyroUpdateLock error!\n");
|
||||
if (pthread_mutex_init(&gyro->lock, NULL) != 0) {
|
||||
printf("Create gyro lock error!\n");
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
void fakeGyroSet(int16_t x, int16_t y, int16_t z)
|
||||
void fakeGyroSet(gyroDev_t *gyro, int16_t x, int16_t y, int16_t z)
|
||||
{
|
||||
GYRO_LOCK;
|
||||
gyroDevLock(gyro);
|
||||
|
||||
fakeGyroADC[X] = x;
|
||||
fakeGyroADC[Y] = y;
|
||||
fakeGyroADC[Z] = z;
|
||||
|
||||
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_GYRO_SYNC)
|
||||
gyroUpdated = true;
|
||||
#endif
|
||||
GYRO_UNLOCK;
|
||||
gyro->dataReady = true;
|
||||
|
||||
gyroDevUnLock(gyro);
|
||||
}
|
||||
|
||||
static bool fakeGyroRead(gyroDev_t *gyro)
|
||||
{
|
||||
GYRO_LOCK;
|
||||
#if defined(SIMULATOR_GYRO_SYNC)
|
||||
if(gyroUpdated == false) {
|
||||
GYRO_UNLOCK;
|
||||
gyroDevLock(gyro);
|
||||
if (gyro->dataReady == false) {
|
||||
gyroDevUnLock(gyro);
|
||||
return false;
|
||||
}
|
||||
gyroUpdated = false;
|
||||
#endif
|
||||
gyro->dataReady = false;
|
||||
|
||||
gyro->gyroADCRaw[X] = fakeGyroADC[X];
|
||||
gyro->gyroADCRaw[Y] = fakeGyroADC[Y];
|
||||
gyro->gyroADCRaw[Z] = fakeGyroADC[Z];
|
||||
|
||||
GYRO_UNLOCK;
|
||||
gyroDevUnLock(gyro);
|
||||
return true;
|
||||
}
|
||||
|
||||
|
@ -134,54 +108,45 @@ bool fakeGyroDetect(gyroDev_t *gyro)
|
|||
#ifdef USE_FAKE_ACC
|
||||
|
||||
static int16_t fakeAccData[XYZ_AXIS_COUNT];
|
||||
|
||||
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD)
|
||||
static pthread_mutex_t accUpdateLock;
|
||||
#endif
|
||||
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_ACC_SYNC)
|
||||
static bool accUpdated = false;
|
||||
#endif
|
||||
accDev_t *fakeAccDev;
|
||||
|
||||
static void fakeAccInit(accDev_t *acc)
|
||||
{
|
||||
UNUSED(acc);
|
||||
fakeAccDev = acc;
|
||||
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD)
|
||||
if (pthread_mutex_init(&accUpdateLock, NULL) != 0) {
|
||||
printf("Create accUpdateLock error!\n");
|
||||
if (pthread_mutex_init(&acc->lock, NULL) != 0) {
|
||||
printf("Create acc lock error!\n");
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
void fakeAccSet(int16_t x, int16_t y, int16_t z)
|
||||
void fakeAccSet(accDev_t *acc, int16_t x, int16_t y, int16_t z)
|
||||
{
|
||||
ACC_LOCK;
|
||||
accDevLock(acc);
|
||||
|
||||
fakeAccData[X] = x;
|
||||
fakeAccData[Y] = y;
|
||||
fakeAccData[Z] = z;
|
||||
|
||||
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_ACC_SYNC)
|
||||
accUpdated = true;
|
||||
#endif
|
||||
ACC_LOCK;
|
||||
acc->dataReady = true;
|
||||
|
||||
accDevUnLock(acc);
|
||||
}
|
||||
|
||||
static bool fakeAccRead(accDev_t *acc)
|
||||
{
|
||||
ACC_LOCK;
|
||||
#if defined(SIMULATOR_ACC_SYNC)
|
||||
if(accUpdated == false) {
|
||||
ACC_UNLOCK;
|
||||
accDevLock(acc);
|
||||
if (acc->dataReady == false) {
|
||||
accDevUnLock(acc);
|
||||
return false;
|
||||
}
|
||||
accUpdated = false;
|
||||
#endif
|
||||
acc->dataReady = false;
|
||||
|
||||
acc->ADCRaw[X] = fakeAccData[X];
|
||||
acc->ADCRaw[Y] = fakeAccData[Y];
|
||||
acc->ADCRaw[Z] = fakeAccData[Z];
|
||||
|
||||
ACC_UNLOCK;
|
||||
accDevUnLock(acc);
|
||||
return true;
|
||||
}
|
||||
|
||||
|
|
|
@ -18,9 +18,11 @@
|
|||
#pragma once
|
||||
|
||||
struct accDev_s;
|
||||
extern struct accDev_s *fakeAccDev;
|
||||
bool fakeAccDetect(struct accDev_s *acc);
|
||||
void fakeAccSet(int16_t x, int16_t y, int16_t z);
|
||||
void fakeAccSet(struct accDev_s *acc, int16_t x, int16_t y, int16_t z);
|
||||
|
||||
struct gyroDev_s;
|
||||
extern struct gyroDev_s *fakeGyroDev;
|
||||
bool fakeGyroDetect(struct gyroDev_s *gyro);
|
||||
void fakeGyroSet(int16_t x, int16_t y, int16_t z);
|
||||
void fakeGyroSet(struct gyroDev_s *gyro, int16_t x, int16_t y, int16_t z);
|
||||
|
|
|
@ -15,14 +15,14 @@
|
|||
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include "platform.h"
|
||||
|
||||
#include "io.h"
|
||||
#include "io_impl.h"
|
||||
#include "rcc.h"
|
||||
|
||||
#include "common/utils.h"
|
||||
|
||||
#include "target.h"
|
||||
|
||||
// io ports defs are stored in array by index now
|
||||
struct ioPortDef_s {
|
||||
rccPeriphTag_t rcc;
|
||||
|
|
|
@ -21,6 +21,7 @@
|
|||
#include <stdint.h>
|
||||
|
||||
#include <platform.h>
|
||||
|
||||
#include "resource.h"
|
||||
|
||||
#include "io_types.h"
|
||||
|
|
|
@ -18,9 +18,10 @@
|
|||
#pragma once
|
||||
|
||||
// TODO - GPIO_TypeDef include
|
||||
#include "io.h"
|
||||
#include "platform.h"
|
||||
|
||||
#include "io.h"
|
||||
|
||||
typedef struct ioDef_s {
|
||||
ioTag_t tag;
|
||||
} ioDef_t;
|
||||
|
|
|
@ -1883,12 +1883,12 @@ static void cliSerialPassthrough(char *cmdline)
|
|||
tok = strtok_r(NULL, " ", &saveptr);
|
||||
}
|
||||
|
||||
printf("Port %d ", id);
|
||||
tfp_printf("Port %d ", id);
|
||||
serialPort_t *passThroughPort;
|
||||
serialPortUsage_t *passThroughPortUsage = findSerialPortUsageByIdentifier(id);
|
||||
if (!passThroughPortUsage || passThroughPortUsage->serialPort == NULL) {
|
||||
if (!baud) {
|
||||
printf("closed, specify baud.\r\n");
|
||||
tfp_printf("closed, specify baud.\r\n");
|
||||
return;
|
||||
}
|
||||
if (!mode)
|
||||
|
@ -1898,17 +1898,17 @@ static void cliSerialPassthrough(char *cmdline)
|
|||
baud, mode,
|
||||
SERIAL_NOT_INVERTED);
|
||||
if (!passThroughPort) {
|
||||
printf("could not be opened.\r\n");
|
||||
tfp_printf("could not be opened.\r\n");
|
||||
return;
|
||||
}
|
||||
printf("opened, baud = %d.\r\n", baud);
|
||||
tfp_printf("opened, baud = %d.\r\n", baud);
|
||||
} else {
|
||||
passThroughPort = passThroughPortUsage->serialPort;
|
||||
// If the user supplied a mode, override the port's mode, otherwise
|
||||
// leave the mode unchanged. serialPassthrough() handles one-way ports.
|
||||
printf("already open.\r\n");
|
||||
tfp_printf("already open.\r\n");
|
||||
if (mode && passThroughPort->mode != mode) {
|
||||
printf("mode changed from %d to %d.\r\n",
|
||||
tfp_printf("mode changed from %d to %d.\r\n",
|
||||
passThroughPort->mode, mode);
|
||||
serialSetMode(passThroughPort, mode);
|
||||
}
|
||||
|
@ -1919,7 +1919,7 @@ static void cliSerialPassthrough(char *cmdline)
|
|||
}
|
||||
}
|
||||
|
||||
printf("forwarding, power cycle to exit.\r\n");
|
||||
tfp_printf("forwarding, power cycle to exit.\r\n");
|
||||
|
||||
serialPassthrough(cliPort, passThroughPort, NULL, NULL);
|
||||
}
|
||||
|
@ -3202,11 +3202,11 @@ static void cliGpsPassthrough(char *cmdline)
|
|||
static int parseEscNumber(char *pch, bool allowAllEscs) {
|
||||
int escNumber = atoi(pch);
|
||||
if ((escNumber >= 0) && (escNumber < getMotorCount())) {
|
||||
printf("Programming on ESC %d.\r\n", escNumber);
|
||||
tfp_printf("Programming on ESC %d.\r\n", escNumber);
|
||||
} else if (allowAllEscs && escNumber == ALL_ESCS) {
|
||||
printf("Programming on all ESCs.\r\n");
|
||||
tfp_printf("Programming on all ESCs.\r\n");
|
||||
} else {
|
||||
printf("Invalid ESC number, range: 0 to %d.\r\n", getMotorCount() - 1);
|
||||
tfp_printf("Invalid ESC number, range: 0 to %d.\r\n", getMotorCount() - 1);
|
||||
|
||||
return -1;
|
||||
}
|
||||
|
@ -3254,9 +3254,9 @@ static void cliDshotProg(char *cmdline)
|
|||
delay(10); // wait for sound output to finish
|
||||
}
|
||||
|
||||
printf("Command %d written.\r\n", command);
|
||||
tfp_printf("Command %d written.\r\n", command);
|
||||
} else {
|
||||
printf("Invalid command, range 1 to %d.\r\n", DSHOT_MIN_THROTTLE - 1);
|
||||
tfp_printf("Invalid command, range 1 to %d.\r\n", DSHOT_MIN_THROTTLE - 1);
|
||||
}
|
||||
|
||||
break;
|
||||
|
@ -3691,7 +3691,7 @@ static void cliTasks(char *cmdline)
|
|||
getTaskInfo(taskId, &taskInfo);
|
||||
if (taskInfo.isEnabled) {
|
||||
int taskFrequency;
|
||||
int subTaskFrequency;
|
||||
int subTaskFrequency = 0;
|
||||
if (taskId == TASK_GYROPID) {
|
||||
subTaskFrequency = taskInfo.latestDeltaTime == 0 ? 0 : (int)(1000000.0f / ((float)taskInfo.latestDeltaTime));
|
||||
taskFrequency = subTaskFrequency / pidConfig()->pid_process_denom;
|
||||
|
|
|
@ -477,7 +477,7 @@ void processRx(timeUs_t currentTimeUs)
|
|||
|
||||
static void subTaskPidController(void)
|
||||
{
|
||||
uint32_t startTime;
|
||||
uint32_t startTime = 0;
|
||||
if (debugMode == DEBUG_PIDLOOP) {startTime = micros();}
|
||||
// PID - note this is function pointer set by setPIDController()
|
||||
pidController(currentPidProfile, &accelerometerConfig()->accelerometerTrims);
|
||||
|
@ -486,7 +486,7 @@ static void subTaskPidController(void)
|
|||
|
||||
static void subTaskMainSubprocesses(timeUs_t currentTimeUs)
|
||||
{
|
||||
uint32_t startTime;
|
||||
uint32_t startTime = 0;
|
||||
if (debugMode == DEBUG_PIDLOOP) {startTime = micros();}
|
||||
|
||||
// Read out gyro temperature if used for telemmetry
|
||||
|
@ -560,7 +560,7 @@ static void subTaskMainSubprocesses(timeUs_t currentTimeUs)
|
|||
|
||||
static void subTaskMotorUpdate(void)
|
||||
{
|
||||
uint32_t startTime;
|
||||
uint32_t startTime = 0;
|
||||
if (debugMode == DEBUG_CYCLETIME) {
|
||||
startTime = micros();
|
||||
static uint32_t previousMotorUpdateTime;
|
||||
|
@ -621,7 +621,7 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
|
|||
// 1 - pidController()
|
||||
// 2 - subTaskMainSubprocesses()
|
||||
// 3 - subTaskMotorUpdate()
|
||||
uint32_t startTime;
|
||||
uint32_t startTime = 0;
|
||||
if (debugMode == DEBUG_PIDLOOP) {startTime = micros();}
|
||||
gyroUpdate();
|
||||
DEBUG_SET(DEBUG_PIDLOOP, 0, micros() - startTime);
|
||||
|
|
|
@ -52,8 +52,6 @@
|
|||
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD)
|
||||
#include <stdio.h>
|
||||
#include <pthread.h>
|
||||
#define printf printf
|
||||
#define sprintf sprintf
|
||||
|
||||
static pthread_mutex_t imuUpdateLock;
|
||||
|
||||
|
|
|
@ -377,7 +377,7 @@ void generateLedConfig(ledConfig_t *ledConfig, char *ledConfigBuffer, size_t buf
|
|||
*fptr = 0;
|
||||
|
||||
// TODO - check buffer length
|
||||
sprintf(ledConfigBuffer, "%u,%u:%s:%s:%u", ledGetX(ledConfig), ledGetY(ledConfig), directions, baseFunctionOverlays, ledGetColor(ledConfig));
|
||||
tfp_sprintf(ledConfigBuffer, "%u,%u:%s:%s:%u", ledGetX(ledConfig), ledGetY(ledConfig), directions, baseFunctionOverlays, ledGetColor(ledConfig));
|
||||
}
|
||||
|
||||
typedef enum {
|
||||
|
|
|
@ -74,6 +74,7 @@
|
|||
#include "fc/rc_controls.h"
|
||||
#include "fc/runtime_config.h"
|
||||
|
||||
#include "flight/altitude.h"
|
||||
#include "flight/imu.h"
|
||||
#include "flight/altitude.h"
|
||||
|
||||
|
@ -189,14 +190,14 @@ static void osdDrawSingleElement(uint8_t item)
|
|||
osdRssi = 99;
|
||||
|
||||
buff[0] = SYM_RSSI;
|
||||
sprintf(buff + 1, "%d", osdRssi);
|
||||
tfp_sprintf(buff + 1, "%d", osdRssi);
|
||||
break;
|
||||
}
|
||||
|
||||
case OSD_MAIN_BATT_VOLTAGE:
|
||||
{
|
||||
buff[0] = SYM_BATT_5;
|
||||
sprintf(buff + 1, "%d.%1dV", getBatteryVoltage() / 10, getBatteryVoltage() % 10);
|
||||
tfp_sprintf(buff + 1, "%d.%1dV", getBatteryVoltage() / 10, getBatteryVoltage() % 10);
|
||||
break;
|
||||
}
|
||||
|
||||
|
@ -204,14 +205,14 @@ static void osdDrawSingleElement(uint8_t item)
|
|||
{
|
||||
int32_t amperage = getAmperage();
|
||||
buff[0] = SYM_AMP;
|
||||
sprintf(buff + 1, "%d.%02d", abs(amperage) / 100, abs(amperage) % 100);
|
||||
tfp_sprintf(buff + 1, "%d.%02d", abs(amperage) / 100, abs(amperage) % 100);
|
||||
break;
|
||||
}
|
||||
|
||||
case OSD_MAH_DRAWN:
|
||||
{
|
||||
buff[0] = SYM_MAH;
|
||||
sprintf(buff + 1, "%d", getMAhDrawn());
|
||||
tfp_sprintf(buff + 1, "%d", getMAhDrawn());
|
||||
break;
|
||||
}
|
||||
|
||||
|
@ -219,14 +220,14 @@ static void osdDrawSingleElement(uint8_t item)
|
|||
case OSD_GPS_SATS:
|
||||
{
|
||||
buff[0] = 0x1f;
|
||||
sprintf(buff + 1, "%d", GPS_numSat);
|
||||
tfp_sprintf(buff + 1, "%d", GPS_numSat);
|
||||
break;
|
||||
}
|
||||
|
||||
case OSD_GPS_SPEED:
|
||||
{
|
||||
// FIXME ideally we want to use SYM_KMH symbol but it's not in the font any more, so we use K.
|
||||
sprintf(buff, "%dK", CM_S_TO_KM_H(GPS_speed) * 10);
|
||||
tfp_sprintf(buff, "%dK", CM_S_TO_KM_H(GPS_speed) * 10);
|
||||
break;
|
||||
}
|
||||
|
||||
|
@ -258,7 +259,7 @@ static void osdDrawSingleElement(uint8_t item)
|
|||
case OSD_ALTITUDE:
|
||||
{
|
||||
int32_t alt = osdGetAltitude(getEstimatedAltitude());
|
||||
sprintf(buff, "%c%d.%01d%c", alt < 0 ? '-' : ' ', abs(alt / 100), abs((alt % 100) / 10), osdGetAltitudeSymbol());
|
||||
tfp_sprintf(buff, "%c%d.%01d%c", alt < 0 ? '-' : ' ', abs(alt / 100), abs((alt % 100) / 10), osdGetAltitudeSymbol());
|
||||
break;
|
||||
}
|
||||
|
||||
|
@ -266,14 +267,14 @@ static void osdDrawSingleElement(uint8_t item)
|
|||
{
|
||||
uint32_t seconds = micros() / 1000000;
|
||||
buff[0] = SYM_ON_M;
|
||||
sprintf(buff + 1, "%02d:%02d", seconds / 60, seconds % 60);
|
||||
tfp_sprintf(buff + 1, "%02d:%02d", seconds / 60, seconds % 60);
|
||||
break;
|
||||
}
|
||||
|
||||
case OSD_FLYTIME:
|
||||
{
|
||||
buff[0] = SYM_FLY_M;
|
||||
sprintf(buff + 1, "%02d:%02d", flyTime / 60, flyTime % 60);
|
||||
tfp_sprintf(buff + 1, "%02d:%02d", flyTime / 60, flyTime % 60);
|
||||
break;
|
||||
}
|
||||
|
||||
|
@ -314,7 +315,7 @@ static void osdDrawSingleElement(uint8_t item)
|
|||
{
|
||||
buff[0] = SYM_THR;
|
||||
buff[1] = SYM_THR1;
|
||||
sprintf(buff + 2, "%d", (constrain(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN));
|
||||
tfp_sprintf(buff + 2, "%d", (constrain(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN));
|
||||
break;
|
||||
}
|
||||
|
||||
|
@ -325,9 +326,9 @@ static void osdDrawSingleElement(uint8_t item)
|
|||
#if defined(VTX)
|
||||
const char vtxBandLetter = vtx58BandLetter[vtxConfig()->vtx_band + 1];
|
||||
const char *vtxChannelName = vtx58ChannelNames[vtxConfig()->vtx_channel + 1];
|
||||
sprintf(buff, "%c:%s", vtxBandLetter, vtxChannelName);
|
||||
tfp_sprintf(buff, "%c:%s", vtxBandLetter, vtxChannelName);
|
||||
#elif defined(USE_RTC6705)
|
||||
sprintf(buff, "CH:%d", current_vtx_channel % CHANNELS_PER_BAND + 1);
|
||||
tfp_sprintf(buff, "CH:%d", current_vtx_channel % CHANNELS_PER_BAND + 1);
|
||||
#endif
|
||||
break;
|
||||
}
|
||||
|
@ -410,27 +411,27 @@ static void osdDrawSingleElement(uint8_t item)
|
|||
case OSD_ROLL_PIDS:
|
||||
{
|
||||
const pidProfile_t *pidProfile = currentPidProfile;
|
||||
sprintf(buff, "ROL %3d %3d %3d", pidProfile->P8[PIDROLL], pidProfile->I8[PIDROLL], pidProfile->D8[PIDROLL]);
|
||||
tfp_sprintf(buff, "ROL %3d %3d %3d", pidProfile->P8[PIDROLL], pidProfile->I8[PIDROLL], pidProfile->D8[PIDROLL]);
|
||||
break;
|
||||
}
|
||||
|
||||
case OSD_PITCH_PIDS:
|
||||
{
|
||||
const pidProfile_t *pidProfile = currentPidProfile;
|
||||
sprintf(buff, "PIT %3d %3d %3d", pidProfile->P8[PIDPITCH], pidProfile->I8[PIDPITCH], pidProfile->D8[PIDPITCH]);
|
||||
tfp_sprintf(buff, "PIT %3d %3d %3d", pidProfile->P8[PIDPITCH], pidProfile->I8[PIDPITCH], pidProfile->D8[PIDPITCH]);
|
||||
break;
|
||||
}
|
||||
|
||||
case OSD_YAW_PIDS:
|
||||
{
|
||||
const pidProfile_t *pidProfile = currentPidProfile;
|
||||
sprintf(buff, "YAW %3d %3d %3d", pidProfile->P8[PIDYAW], pidProfile->I8[PIDYAW], pidProfile->D8[PIDYAW]);
|
||||
tfp_sprintf(buff, "YAW %3d %3d %3d", pidProfile->P8[PIDYAW], pidProfile->I8[PIDYAW], pidProfile->D8[PIDYAW]);
|
||||
break;
|
||||
}
|
||||
|
||||
case OSD_POWER:
|
||||
{
|
||||
sprintf(buff, "%dW", getAmperage() * getBatteryVoltage() / 1000);
|
||||
tfp_sprintf(buff, "%dW", getAmperage() * getBatteryVoltage() / 1000);
|
||||
break;
|
||||
}
|
||||
|
||||
|
@ -438,7 +439,7 @@ static void osdDrawSingleElement(uint8_t item)
|
|||
{
|
||||
const uint8_t pidProfileIndex = getCurrentPidProfileIndex();
|
||||
const uint8_t rateProfileIndex = getCurrentControlRateProfileIndex();
|
||||
sprintf(buff, "%d-%d", pidProfileIndex + 1, rateProfileIndex + 1);
|
||||
tfp_sprintf(buff, "%d-%d", pidProfileIndex + 1, rateProfileIndex + 1);
|
||||
break;
|
||||
}
|
||||
|
||||
|
@ -446,11 +447,11 @@ static void osdDrawSingleElement(uint8_t item)
|
|||
{
|
||||
switch(getBatteryState()) {
|
||||
case BATTERY_WARNING:
|
||||
sprintf(buff, "LOW BATTERY");
|
||||
tfp_sprintf(buff, "LOW BATTERY");
|
||||
break;
|
||||
|
||||
case BATTERY_CRITICAL:
|
||||
sprintf(buff, "LAND NOW");
|
||||
tfp_sprintf(buff, "LAND NOW");
|
||||
elemOffsetX += 1;
|
||||
break;
|
||||
|
||||
|
@ -464,7 +465,7 @@ static void osdDrawSingleElement(uint8_t item)
|
|||
{
|
||||
uint16_t cellV = getBatteryVoltage() * 10 / getBatteryCellCount();
|
||||
buff[0] = SYM_BATT_5;
|
||||
sprintf(buff + 1, "%d.%dV", cellV / 100, cellV % 100);
|
||||
tfp_sprintf(buff + 1, "%d.%dV", cellV / 100, cellV % 100);
|
||||
break;
|
||||
}
|
||||
|
||||
|
@ -603,7 +604,7 @@ void osdInit(displayPort_t *osdDisplayPortToUse)
|
|||
osdDrawLogo(3, 1);
|
||||
|
||||
char string_buffer[30];
|
||||
sprintf(string_buffer, "V%s", FC_VERSION_STRING);
|
||||
tfp_sprintf(string_buffer, "V%s", FC_VERSION_STRING);
|
||||
displayWrite(osdDisplayPort, 20, 6, string_buffer);
|
||||
#ifdef CMS
|
||||
displayWrite(osdDisplayPort, 7, 8, CMS_STARTUP_HELP_TEXT1);
|
||||
|
@ -763,7 +764,7 @@ static void osdShowStats(void)
|
|||
}
|
||||
|
||||
displayWrite(osdDisplayPort, 2, top, "MIN BATTERY :");
|
||||
sprintf(buff, "%d.%1dV", stats.min_voltage / 10, stats.min_voltage % 10);
|
||||
tfp_sprintf(buff, "%d.%1dV", stats.min_voltage / 10, stats.min_voltage % 10);
|
||||
displayWrite(osdDisplayPort, 22, top++, buff);
|
||||
|
||||
displayWrite(osdDisplayPort, 2, top, "MIN RSSI :");
|
||||
|
@ -785,7 +786,7 @@ static void osdShowStats(void)
|
|||
|
||||
displayWrite(osdDisplayPort, 2, top, "MAX ALTITUDE :");
|
||||
int32_t alt = osdGetAltitude(stats.max_altitude);
|
||||
sprintf(buff, "%c%d.%01d%c", alt < 0 ? '-' : ' ', abs(alt / 100), abs((alt % 100) / 10), osdGetAltitudeSymbol());
|
||||
tfp_sprintf(buff, "%c%d.%01d%c", alt < 0 ? '-' : ' ', abs(alt / 100), abs((alt % 100) / 10), osdGetAltitudeSymbol());
|
||||
displayWrite(osdDisplayPort, 22, top++, buff);
|
||||
|
||||
#ifdef BLACKBOX
|
||||
|
|
|
@ -98,7 +98,7 @@ void osdSlaveInit(displayPort_t *osdDisplayPortToUse)
|
|||
osdDrawLogo(3, 1);
|
||||
|
||||
char string_buffer[30];
|
||||
sprintf(string_buffer, "V%s", FC_VERSION_STRING);
|
||||
tfp_sprintf(string_buffer, "V%s", FC_VERSION_STRING);
|
||||
displayWrite(osdDisplayPort, 20, 6, string_buffer);
|
||||
displayWrite(osdDisplayPort, 13, 6, "OSD");
|
||||
|
||||
|
@ -139,7 +139,7 @@ void osdSlaveUpdate(timeUs_t currentTimeUs)
|
|||
#ifdef OSD_SLAVE_DEBUG
|
||||
char buff[32];
|
||||
for (int i = 0; i < 4; i ++) {
|
||||
sprintf(buff, "%5d", debug[i]);
|
||||
tfp_sprintf(buff, "%5d", debug[i]);
|
||||
displayWrite(osdDisplayPort, i * 8, 0, buff);
|
||||
}
|
||||
#endif
|
||||
|
|
|
@ -19,8 +19,6 @@
|
|||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <math.h>
|
||||
#define printf printf
|
||||
#define sprintf sprintf
|
||||
|
||||
#include <errno.h>
|
||||
#include <time.h>
|
||||
|
@ -96,13 +94,13 @@ void updateState(const fdm_packet* pkt) {
|
|||
x = -pkt->imu_linear_acceleration_xyz[0] * ACC_SCALE;
|
||||
y = -pkt->imu_linear_acceleration_xyz[1] * ACC_SCALE;
|
||||
z = -pkt->imu_linear_acceleration_xyz[2] * ACC_SCALE;
|
||||
fakeAccSet(x, y, z);
|
||||
fakeAccSet(fakeAccDev, x, y, z);
|
||||
// printf("[acc]%lf,%lf,%lf\n", pkt->imu_linear_acceleration_xyz[0], pkt->imu_linear_acceleration_xyz[1], pkt->imu_linear_acceleration_xyz[2]);
|
||||
|
||||
x = pkt->imu_angular_velocity_rpy[0] * GYRO_SCALE * RAD2DEG;
|
||||
y = -pkt->imu_angular_velocity_rpy[1] * GYRO_SCALE * RAD2DEG;
|
||||
z = -pkt->imu_angular_velocity_rpy[2] * GYRO_SCALE * RAD2DEG;
|
||||
fakeGyroSet(x, y, z);
|
||||
fakeGyroSet(fakeGyroDev, x, y, z);
|
||||
// printf("[gyr]%lf,%lf,%lf\n", pkt->imu_angular_velocity_rpy[0], pkt->imu_angular_velocity_rpy[1], pkt->imu_angular_velocity_rpy[2]);
|
||||
|
||||
#if defined(SKIP_IMU_CALC)
|
||||
|
|
|
@ -162,7 +162,7 @@ typedef struct
|
|||
uint32_t BRR;
|
||||
} GPIO_TypeDef;
|
||||
|
||||
#define GPIOA_BASE (0x0000)
|
||||
#define GPIOA_BASE (0x0001)
|
||||
|
||||
typedef struct
|
||||
{
|
||||
|
|
Loading…
Reference in New Issue