From a5f44f0024055b45886b3d7de56a9baf4e64e99a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Fri, 10 Feb 2023 18:43:32 +0100 Subject: [PATCH 1/5] Added SITL Gazebo MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- src/main/fc/rc_modes.c | 2 +- src/main/fc/tasks.c | 5 +- src/main/main.c | 8 +- src/main/msp/msp.c | 8 +- src/main/rx/rx.c | 3 +- src/main/rx/rx.h | 1 + src/main/sensors/acceleration_init.c | 4 + src/main/target/SITL/sitl.c | 112 ++++++++++++++++++++++++--- src/main/target/SITL/target.h | 16 +++- src/main/target/SITL/udplink.c | 4 +- 10 files changed, 144 insertions(+), 19 deletions(-) diff --git a/src/main/fc/rc_modes.c b/src/main/fc/rc_modes.c index 44da280bbe9..fe21ad8208e 100644 --- a/src/main/fc/rc_modes.c +++ b/src/main/fc/rc_modes.c @@ -255,7 +255,7 @@ void analyzeModeActivationConditions(void) activeMacArray[activeMacCount++] = i; } } -#ifdef USE_PINIOBOX +#if defined(USE_PINIOBOX) && !defined(SIMULATOR_MULTITHREAD) pinioBoxTaskControl(); #endif } diff --git a/src/main/fc/tasks.c b/src/main/fc/tasks.c index 7fcd6c75385..5c24e930098 100644 --- a/src/main/fc/tasks.c +++ b/src/main/fc/tasks.c @@ -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 +} diff --git a/src/main/main.c b/src/main/main.c index 5e4826dd502..7a4e9e8b889 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -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(); diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c index 553e7e03af2..be19493020f 100644 --- a/src/main/msp/msp.c +++ b/src/main/msp/msp.c @@ -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: diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index 2c117a40ab8..5d88c723d46 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -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)); diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index b9c714790fd..617dfddc000 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -136,6 +136,7 @@ typedef enum { RX_PROVIDER_SERIAL, RX_PROVIDER_MSP, RX_PROVIDER_SPI, + RX_PROVIDER_UDP, } rxProvider_t; typedef struct rxRuntimeState_s { diff --git a/src/main/sensors/acceleration_init.c b/src/main/sensors/acceleration_init.c index 12d23a10a29..83d6dd37576 100644 --- a/src/main/sensors/acceleration_init.c +++ b/src/main/sensors/acceleration_init.c @@ -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) diff --git a/src/main/target/SITL/sitl.c b/src/main/target/SITL/sitl.c index ad696972262..0b815129049 100644 --- a/src/main/target/SITL/sitl.c +++ b/src/main/target/SITL/sitl.c @@ -59,16 +59,39 @@ 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) @@ -184,7 +207,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 +219,44 @@ 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.rxRefreshRate = 20000; + rxRuntimeState.rxProvider = RX_PROVIDER_UDP; + rc_received = true; + } + } + } + + printf("udpRCThread end!!\n"); + return NULL; +} + static void* tcpThread(void* data) { UNUSED(data); @@ -236,8 +300,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 +321,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 +508,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 +546,15 @@ 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 < 4) { + 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 +591,17 @@ 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 +625,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; diff --git a/src/main/target/SITL/target.h b/src/main/target/SITL/target.h index 02e49f0e22c..a78156f2b48 100644 --- a/src/main/target/SITL/target.h +++ b/src/main/target/SITL/target.h @@ -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; @@ -256,10 +259,21 @@ typedef struct { double velocity_xyz[3]; // m/s, earth frame double position_xyz[3]; // meters, NED from origin } 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 +288,4 @@ uint64_t millis64(void); int lockMainPID(void); - +int targetParseArgs(int argc, char * argv[]); diff --git a/src/main/target/SITL/udplink.c b/src/main/target/SITL/udplink.c index d1ebbc75396..21658f63bb2 100644 --- a/src/main/target/SITL/udplink.c +++ b/src/main/target/SITL/udplink.c @@ -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; } From 6f089ae8fa708beb4d543c8d8bf38a5111c56292 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Mon, 13 Feb 2023 23:49:59 +0100 Subject: [PATCH 2/5] style MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- src/main/target/SITL/sitl.c | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/target/SITL/sitl.c b/src/main/target/SITL/sitl.c index 0b815129049..48deddbb12a 100644 --- a/src/main/target/SITL/sitl.c +++ b/src/main/target/SITL/sitl.c @@ -83,8 +83,7 @@ static char simulator_ip[32] = "127.0.0.1"; int targetParseArgs(int argc, char * argv[]) { //The first argument should be target IP. - if (argc > 1) - { + if (argc > 1) { strcpy(simulator_ip, argv[1]); } @@ -219,7 +218,8 @@ static void* udpThread(void* data) return NULL; } -static float readRCSITL(const rxRuntimeState_t *rxRuntimeState, uint8_t channel) { +static float readRCSITL(const rxRuntimeState_t *rxRuntimeState, uint8_t channel) +{ UNUSED(rxRuntimeState); return rcPkt.channels[channel]; } @@ -230,7 +230,8 @@ static uint8_t rxRCFrameStatus(rxRuntimeState_t *rxRuntimeState) return RX_FRAME_COMPLETE; } -static void* udpRCThread(void* data) { +static void* udpRCThread(void* data) +{ UNUSED(data); int n = 0; @@ -597,8 +598,7 @@ static void pwmCompleteMotorUpdate(void) void pwmWriteServo(uint8_t index, float value) { servosPwm[index] = value; - if (index + pwmRawPkt.motorCount < SIMULATOR_MAX_PWM_CHANNELS) - { + 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; } From c7731536bbe1ad916e1b1fd24e09a0e5ceda83d0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Wed, 15 Feb 2023 23:36:43 +0100 Subject: [PATCH 3/5] Added Fake Barometer MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- src/main/sensors/barometer.c | 16 +++++++++++++++- src/main/sensors/barometer.h | 1 + src/main/target/SITL/sitl.c | 3 +++ src/main/target/SITL/target.h | 1 + 4 files changed, 20 insertions(+), 1 deletion(-) diff --git a/src/main/sensors/barometer.c b/src/main/sensors/barometer.c index d92f2a32eb9..2b8cdc8a525 100644 --- a/src/main/sensors/barometer.c +++ b/src/main/sensors/barometer.c @@ -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 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) diff --git a/src/main/sensors/barometer.h b/src/main/sensors/barometer.h index d6a9836a7ae..ff823fdeb1b 100644 --- a/src/main/sensors/barometer.h +++ b/src/main/sensors/barometer.h @@ -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 { diff --git a/src/main/target/SITL/sitl.c b/src/main/target/SITL/sitl.c index 48deddbb12a..0f56416eff8 100644 --- a/src/main/target/SITL/sitl.c +++ b/src/main/target/SITL/sitl.c @@ -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" @@ -140,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 diff --git a/src/main/target/SITL/target.h b/src/main/target/SITL/target.h index a78156f2b48..d926061f3f7 100644 --- a/src/main/target/SITL/target.h +++ b/src/main/target/SITL/target.h @@ -258,6 +258,7 @@ 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 { From 25c3c8d614251fcc0002d49d2ba4e24d8029e66e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Thu, 16 Feb 2023 13:23:26 +0100 Subject: [PATCH 4/5] make linters happy MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- src/main/msp/msp.c | 2 +- src/main/sensors/barometer.c | 2 +- src/main/target/SITL/sitl.c | 33 +++++++++++++++++---------------- src/main/target/SITL/target.h | 6 +++--- 4 files changed, 22 insertions(+), 21 deletions(-) diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c index be19493020f..4726bb34bc9 100644 --- a/src/main/msp/msp.c +++ b/src/main/msp/msp.c @@ -3551,7 +3551,7 @@ 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. +#ifndef SIMULATOR_BUILD // In simulator mode we can safely arm with MSP link. mspArmingDisableByDescriptor(srcDesc); setArmingDisabled(ARMING_DISABLED_MSP); if (ARMING_FLAG(ARMED)) { diff --git a/src/main/sensors/barometer.c b/src/main/sensors/barometer.c index 2b8cdc8a525..e2d0260be4e 100644 --- a/src/main/sensors/barometer.c +++ b/src/main/sensors/barometer.c @@ -196,7 +196,7 @@ static bool baroDetect(baroDev_t *baroDev, baroSensor_e baroHardwareToUse) default: return false; } -#endif +#endif // USE_FAKE_BARO switch (baroHardware) { case BARO_DEFAULT: diff --git a/src/main/target/SITL/sitl.c b/src/main/target/SITL/sitl.c index 0f56416eff8..b4de5b24088 100644 --- a/src/main/target/SITL/sitl.c +++ b/src/main/target/SITL/sitl.c @@ -76,21 +76,21 @@ 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 +#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]); - } + //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; + 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); @@ -233,7 +233,7 @@ static uint8_t rxRCFrameStatus(rxRuntimeState_t *rxRuntimeState) return RX_FRAME_COMPLETE; } -static void* udpRCThread(void* data) +static void *udpRCThread(void *data) { UNUSED(data); int n = 0; @@ -250,7 +250,6 @@ static void* udpRCThread(void* data) rxRuntimeState.rcReadRawFn = readRCSITL; rxRuntimeState.rcFrameStatusFn = rxRCFrameStatus; - // rxRuntimeState.rxRefreshRate = 20000; rxRuntimeState.rxProvider = RX_PROVIDER_UDP; rc_received = true; } @@ -551,13 +550,15 @@ static bool pwmEnableMotors(void) static void pwmWriteMotor(uint8_t index, float value) { if (pthread_mutex_trylock(&updateLock) != 0) return; - if (index < 4) { + + 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 } @@ -602,8 +603,8 @@ 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; + // In pwmRawPkt, we put servo right after the motors. + pwmRawPkt.pwm_output_raw[index + pwmRawPkt.motorCount] = value; } } diff --git a/src/main/target/SITL/target.h b/src/main/target/SITL/target.h index d926061f3f7..bc75d4dd2dc 100644 --- a/src/main/target/SITL/target.h +++ b/src/main/target/SITL/target.h @@ -234,8 +234,8 @@ typedef struct #define UART7 ((USART_TypeDef *)0x0007) #define UART8 ((USART_TypeDef *)0x0008) -#define SIMULATOR_MAX_RC_CHANNELS 16 -#define SIMULATOR_MAX_PWM_CHANNELS 16 +#define SIMULATOR_MAX_RC_CHANNELS 16 +#define SIMULATOR_MAX_PWM_CHANNELS 16 typedef struct { @@ -271,7 +271,7 @@ typedef struct { } servo_packet; typedef struct { - uint16_t motorCount; //Count of motor in the PWM output. + 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; From 01ea09e42cb6f793ee3507d556d6ac46dfb2b64e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Fri, 17 Feb 2023 16:48:56 +0100 Subject: [PATCH 5/5] Added review feedback MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- src/main/main.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/main.c b/src/main/main.c index 7a4e9e8b889..a2a97a4e9b2 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -34,8 +34,8 @@ int main(int argc, char * argv[]) #ifdef SIMULATOR_BUILD targetParseArgs(argc, argv); #else - UNUSED(argc); - UNUSED(argv); + UNUSED(argc); + UNUSED(argv); #endif init();