Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-15 12:25:20 +03:00
Code Issues Wiki Activity

Added SITL Gazebo (#12346)

Browse source
This commit is contained in:
Alejandro Hernández Cordero 2023-02-18 20:39:54 +01:00 • committed by GitHub
parent 1c92d83c6f
commit 9dc9c51d3e
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
12 changed files with 165 additions and 20 deletions
Download patch file Download diff file Expand all files Collapse all files

2
src/main/fc/rc_modes.c
View file

@ -255,7 +255,7 @@ void analyzeModeActivationConditions(void)
activeMacArray[activeMacCount++] = i;
}
}
#ifdef USE_PINIOBOX
#if defined(USE_PINIOBOX) && !defined(SIMULATOR_MULTITHREAD)
pinioBoxTaskControl();
#endif
}

5
src/main/fc/tasks.c
View file

@ -617,5 +617,8 @@ void tasksInit(void)
const bool useCRSF = rxRuntimeState.serialrxProvider == SERIALRX_CRSF;
setTaskEnabled(TASK_SPEED_NEGOTIATION, useCRSF);
#endif
}
#ifdef SIMULATOR_MULTITHREAD
rescheduleTask(TASK_RX, 1);
#endif
}

8
src/main/main.c
View file

@ -29,8 +29,14 @@
void run(void);
int main(void)
int main(int argc, char * argv[])
{
#ifdef SIMULATOR_BUILD
targetParseArgs(argc, argv);
#else
UNUSED(argc);
UNUSED(argv);
#endif
init();
run();

8
src/main/msp/msp.c
View file

@ -221,10 +221,12 @@ mspDescriptor_t mspDescriptorAlloc(void)
static uint32_t mspArmingDisableFlags = 0;
#ifndef SIMULATOR_BUILD
static void mspArmingDisableByDescriptor(mspDescriptor_t desc)
{
mspArmingDisableFlags |= (1 << desc);
}
#endif
static void mspArmingEnableByDescriptor(mspDescriptor_t desc)
{
@ -1516,7 +1518,7 @@ case MSP_NAME:
sbufWriteU16(dst, (uint16_t)constrain(gpsSol.llh.altCm / 100, 0, UINT16_MAX)); // alt changed from 1m to 0.01m per lsb since MSP API 1.39 by RTH. To maintain backwards compatibility compensate to 1m per lsb in MSP again.
sbufWriteU16(dst, gpsSol.groundSpeed);
sbufWriteU16(dst, gpsSol.groundCourse);
// Added in API version 1.44
// Added in API version 1.44
sbufWriteU16(dst, gpsSol.dop.hdop);
break;
@ -3549,11 +3551,13 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
disableRunawayTakeoff = sbufReadU8(src);
}
if (command) {
#ifndef SIMULATOR_BUILD // In simulator mode we can safely arm with MSP link.
mspArmingDisableByDescriptor(srcDesc);
setArmingDisabled(ARMING_DISABLED_MSP);
if (ARMING_FLAG(ARMED)) {
disarm(DISARM_REASON_ARMING_DISABLED);
}
#endif
#ifdef USE_RUNAWAY_TAKEOFF
runawayTakeoffTemporaryDisable(false);
#endif
@ -3702,7 +3706,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
#else
uint8_t emptyUid[6];
sbufReadData(src, emptyUid, 6);
#endif
#endif
}
break;
case MSP_SET_FAILSAFE_CONFIG:

3
src/main/rx/rx.c
View file

@ -381,7 +381,7 @@ void rxInit(void)
// Configurable amount of filtering to remove excessive jumpiness of the values on the osd
float k = (256.0f - rxConfig()->rssi_smoothing) / 256.0f;
pt1FilterInit(&rssiFilter, k);
pt1FilterInit(&rssiFilter, k);
#ifdef USE_RX_RSSI_DBM
pt1FilterInit(&rssiDbmFilter, k);
@ -534,6 +534,7 @@ FAST_CODE_NOINLINE void rxFrameCheck(timeUs_t currentTimeUs, timeDelta_t current
case RX_PROVIDER_SERIAL:
case RX_PROVIDER_MSP:
case RX_PROVIDER_SPI:
case RX_PROVIDER_UDP:
{
const uint8_t frameStatus = rxRuntimeState.rcFrameStatusFn(&rxRuntimeState);
DEBUG_SET(DEBUG_RX_SIGNAL_LOSS, 1, (frameStatus & RX_FRAME_FAILSAFE));

1
src/main/rx/rx.h
View file

@ -136,6 +136,7 @@ typedef enum {
RX_PROVIDER_SERIAL,
RX_PROVIDER_MSP,
RX_PROVIDER_SPI,
RX_PROVIDER_UDP,
} rxProvider_t;
typedef struct rxRuntimeState_s {

4
src/main/sensors/acceleration_init.c
View file

@ -114,7 +114,11 @@ static void setConfigCalibrationCompleted(void)
bool accHasBeenCalibrated(void)
{
#ifdef SIMULATOR_BUILD
return true;
#else
return accelerometerConfig()->accZero.values.calibrationCompleted;
#endif
}
void accResetRollAndPitchTrims(void)

16
src/main/sensors/barometer.c
View file

@ -173,6 +173,7 @@ static bool baroDetect(baroDev_t *baroDev, baroSensor_e baroHardwareToUse)
UNUSED(dev);
#endif
#ifndef USE_FAKE_BARO
switch (barometerConfig()->baro_busType) {
#ifdef USE_I2C
case BUS_TYPE_I2C:
@ -192,10 +193,10 @@ static bool baroDetect(baroDev_t *baroDev, baroSensor_e baroHardwareToUse)
}
break;
#endif
default:
return false;
}
#endif // USE_FAKE_BARO
switch (baroHardware) {
case BARO_DEFAULT:
@ -291,6 +292,15 @@ static bool baroDetect(baroDev_t *baroDev, baroSensor_e baroHardwareToUse)
#endif
FALLTHROUGH;
case BARO_FAKE:
#ifdef USE_FAKE_BARO
if (fakeBaroDetect(baroDev)) {
baroHardware = BARO_FAKE;
break;
}
#endif
FALLTHROUGH;
case BARO_NONE:
baroHardware = BARO_NONE;
break;
@ -307,7 +317,11 @@ static bool baroDetect(baroDev_t *baroDev, baroSensor_e baroHardwareToUse)
void baroInit(void)
{
#ifndef USE_FAKE_BARO
baroReady = baroDetect(&baro.dev, barometerConfig()->baro_hardware);
#else
baroReady = baroDetect(&baro.dev, BARO_FAKE);
#endif
}
bool baroIsCalibrated(void)

1
src/main/sensors/barometer.h
View file

@ -34,6 +34,7 @@ typedef enum {
BARO_BMP388 = 7,
BARO_DPS310 = 8,
BARO_2SMPB_02B = 9,
BARO_FAKE = 10,
} baroSensor_e;
typedef struct barometerConfig_s {

116
src/main/target/SITL/sitl.c
View file

@ -42,6 +42,7 @@
#include "drivers/timer_def.h"
#include "drivers/accgyro/accgyro_fake.h"
#include "drivers/barometer/barometer_fake.h"
#include "flight/imu.h"
#include "config/feature.h"
@ -59,16 +60,38 @@
uint32_t SystemCoreClock;
static fdm_packet fdmPkt;
static rc_packet rcPkt;
static servo_packet pwmPkt;
static servo_packet_raw pwmRawPkt;
static bool rc_received = false;
static bool fdm_received = false;
static struct timespec start_time;
static double simRate = 1.0;
static pthread_t tcpWorker, udpWorker;
static pthread_t tcpWorker, udpWorker, udpWorkerRC;
static bool workerRunning = true;
static udpLink_t stateLink, pwmLink;
static udpLink_t stateLink, pwmLink, pwmRawLink, rcLink;
static pthread_mutex_t updateLock;
static pthread_mutex_t mainLoopLock;
static char simulator_ip[32] = "127.0.0.1";
#define PORT_PWM_RAW 9001 // Out
#define PORT_PWM 9002 // Out
#define PORT_STATE 9003 // In
#define PORT_RC 9004 // In
int targetParseArgs(int argc, char * argv[])
{
//The first argument should be target IP.
if (argc > 1) {
strcpy(simulator_ip, argv[1]);
}
printf("[SITL] The SITL will output to IP %s:%d (Gazebo) and %s:%d (RealFlightBridge)\n",
simulator_ip, PORT_PWM, simulator_ip, PORT_PWM_RAW);
return 0;
}
int timeval_sub(struct timespec *result, struct timespec *x, struct timespec *y);
int lockMainPID(void)
@ -118,6 +141,8 @@ void updateState(const fdm_packet* pkt)
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]);
// temperature in 0.01 C = 25 deg
fakeBaroSet(pkt->pressure, 2500);
#if !defined(USE_IMU_CALC)
#if defined(SET_IMU_FROM_EULER)
// set from Euler
@ -184,7 +209,10 @@ static void* udpThread(void* data)
while (workerRunning) {
n = udpRecv(&stateLink, &fdmPkt, sizeof(fdm_packet), 100);
if (n == sizeof(fdm_packet)) {
// printf("[data]new fdm %d\n", n);
if (!fdm_received) {
printf("[SITL] new fdm %d t:%f from %s:%d\n", n, fdmPkt.timestamp, inet_ntoa(stateLink.recv.sin_addr), stateLink.recv.sin_port);
fdm_received = true;
}
updateState(&fdmPkt);
}
}
@ -193,6 +221,45 @@ static void* udpThread(void* data)
return NULL;
}
static float readRCSITL(const rxRuntimeState_t *rxRuntimeState, uint8_t channel)
{
UNUSED(rxRuntimeState);
return rcPkt.channels[channel];
}
static uint8_t rxRCFrameStatus(rxRuntimeState_t *rxRuntimeState)
{
UNUSED(rxRuntimeState);
return RX_FRAME_COMPLETE;
}
static void *udpRCThread(void *data)
{
UNUSED(data);
int n = 0;
while (workerRunning) {
n = udpRecv(&rcLink, &rcPkt, sizeof(rc_packet), 100);
if (n == sizeof(rc_packet)) {
if (!rc_received) {
printf("[SITL] new rc %d: t:%f AETR: %d %d %d %d AUX1-4: %d %d %d %d\n", n, rcPkt.timestamp,
rcPkt.channels[0], rcPkt.channels[1],rcPkt.channels[2],rcPkt.channels[3],
rcPkt.channels[4], rcPkt.channels[5],rcPkt.channels[6],rcPkt.channels[7]);
rxRuntimeState.channelCount = SIMULATOR_MAX_RC_CHANNELS;
rxRuntimeState.rcReadRawFn = readRCSITL;
rxRuntimeState.rcFrameStatusFn = rxRCFrameStatus;
rxRuntimeState.rxProvider = RX_PROVIDER_UDP;
rc_received = true;
}
}
}
printf("udpRCThread end!!\n");
return NULL;
}
static void* tcpThread(void* data)
{
UNUSED(data);
@ -236,8 +303,17 @@ void systemInit(void)
exit(1);
}
ret = udpInit(&pwmLink, "127.0.0.1", 9002, false);
printf("init PwmOut UDP link...%d\n", ret);
ret = udpInit(&pwmLink, simulator_ip, PORT_PWM, false);
printf("[SITL] init PwmOut UDP link to gazebo %s:%d...%d\n", simulator_ip, PORT_PWM, ret);
ret = udpInit(&pwmRawLink, simulator_ip, PORT_PWM_RAW, false);
printf("[SITL] init PwmOut UDP link to RF9 %s:%d...%d\n", simulator_ip, PORT_PWM_RAW, ret);
ret = udpInit(&stateLink, NULL, PORT_STATE, true);
printf("[SITL] start UDP server @%d...%d\n", PORT_STATE, ret);
ret = udpInit(&rcLink, NULL, PORT_RC, true);
printf("[SITL] start UDP server for RC input @%d...%d\n", PORT_RC, ret);
ret = udpInit(&stateLink, NULL, 9003, true);
printf("start UDP server...%d\n", ret);
@ -248,6 +324,11 @@ void systemInit(void)
exit(1);
}
ret = pthread_create(&udpWorkerRC, NULL, udpRCThread, NULL);
if (ret != 0) {
printf("Create udpRCThread error!\n");
exit(1);
}
}
void systemReset(void)
@ -430,7 +511,8 @@ static int16_t idlePulse;
void servoDevInit(const servoDevConfig_t *servoConfig)
{
UNUSED(servoConfig);
printf("[SITL] Init servos num %d rate %d center %d\n", MAX_SUPPORTED_SERVOS,
servoConfig->servoPwmRate, servoConfig->servoCenterPulse);
for (uint8_t servoIndex = 0; servoIndex < MAX_SUPPORTED_SERVOS; servoIndex++) {
servos[servoIndex].enabled = true;
}
@ -467,7 +549,17 @@ static bool pwmEnableMotors(void)
static void pwmWriteMotor(uint8_t index, float value)
{
motorsPwm[index] = value - idlePulse;
if (pthread_mutex_trylock(&updateLock) != 0) return;
if (index < MAX_SUPPORTED_MOTORS) {
motorsPwm[index] = value - idlePulse;
}
if (index < pwmRawPkt.motorCount) {
pwmRawPkt.pwm_output_raw[index] = value;
}
pthread_mutex_unlock(&updateLock); // can send PWM output now
}
static void pwmWriteMotorInt(uint8_t index, uint16_t value)
@ -504,11 +596,16 @@ static void pwmCompleteMotorUpdate(void)
if (pthread_mutex_trylock(&updateLock) != 0) return;
udpSend(&pwmLink, &pwmPkt, sizeof(servo_packet));
// printf("[pwm]%u:%u,%u,%u,%u\n", idlePulse, motorsPwm[0], motorsPwm[1], motorsPwm[2], motorsPwm[3]);
udpSend(&pwmRawLink, &pwmRawPkt, sizeof(servo_packet_raw));
}
void pwmWriteServo(uint8_t index, float value)
{
servosPwm[index] = value;
if (index + pwmRawPkt.motorCount < SIMULATOR_MAX_PWM_CHANNELS) {
// In pwmRawPkt, we put servo right after the motors.
pwmRawPkt.pwm_output_raw[index + pwmRawPkt.motorCount] = value;
}
}
static motorDevice_t motorPwmDevice = {
@ -532,9 +629,8 @@ motorDevice_t *motorPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t _id
UNUSED(motorConfig);
UNUSED(useUnsyncedPwm);
if (motorCount > 4) {
return NULL;
}
printf("Initialized motor count %d\n", motorCount);
pwmRawPkt.motorCount = motorCount;
idlePulse = _idlePulse;

17
src/main/target/SITL/target.h
View file

@ -234,6 +234,9 @@ typedef struct
#define UART7 ((USART_TypeDef *)0x0007)
#define UART8 ((USART_TypeDef *)0x0008)
#define SIMULATOR_MAX_RC_CHANNELS 16
#define SIMULATOR_MAX_PWM_CHANNELS 16
typedef struct
{
void* test;
@ -255,11 +258,23 @@ typedef struct {
double imu_orientation_quat[4]; //w, x, y, z
double velocity_xyz[3]; // m/s, earth frame
double position_xyz[3]; // meters, NED from origin
double pressure;
} fdm_packet;
typedef struct {
double timestamp; // in seconds
uint16_t channels[SIMULATOR_MAX_RC_CHANNELS]; // RC channels
} rc_packet;
typedef struct {
float motor_speed[4]; // normal: [0.0, 1.0], 3D: [-1.0, 1.0]
} servo_packet;
typedef struct {
uint16_t motorCount; // Count of motor in the PWM output.
float pwm_output_raw[SIMULATOR_MAX_PWM_CHANNELS]; // Raw PWM from 1100 to 1900
} servo_packet_raw;
void FLASH_Unlock(void);
void FLASH_Lock(void);
FLASH_Status FLASH_ErasePage(uintptr_t Page_Address);
@ -274,4 +289,4 @@ uint64_t millis64(void);
int lockMainPID(void);
int targetParseArgs(int argc, char * argv[]);

4
src/main/target/SITL/udplink.c
View file

@ -64,8 +64,8 @@ int udpRecv(udpLink_t* link, void* data, size_t size, uint32_t timeout_ms)
return -1;
}
socklen_t len;
socklen_t len = sizeof(link->si);;
int ret;
ret = recvfrom(link->fd, data, size, 0, (struct sockaddr *)&link->recv, &len);
ret = recvfrom(link->fd, data, size, 0, (struct sockaddr *)&link->si, &len);
return ret;
}