mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-23 00:05:28 +03:00
USE_SERVO assumed always true (since it is) and removed
This commit is contained in:
parent
1d3c6c9195
commit
e5f2abd770
18 changed files with 2 additions and 111 deletions
|
@ -1176,10 +1176,8 @@ static void loadMainState(timeUs_t currentTimeUs)
|
||||||
|
|
||||||
blackboxCurrent->rssi = getRSSI();
|
blackboxCurrent->rssi = getRSSI();
|
||||||
|
|
||||||
#ifdef USE_SERVOS
|
|
||||||
//Tail servo for tricopters
|
//Tail servo for tricopters
|
||||||
blackboxCurrent->servo[5] = servo[5];
|
blackboxCurrent->servo[5] = servo[5];
|
||||||
#endif
|
|
||||||
|
|
||||||
#ifdef NAV_BLACKBOX
|
#ifdef NAV_BLACKBOX
|
||||||
blackboxCurrent->navState = navCurrentState;
|
blackboxCurrent->navState = navCurrentState;
|
||||||
|
|
|
@ -28,10 +28,7 @@
|
||||||
|
|
||||||
#include "fc/config.h"
|
#include "fc/config.h"
|
||||||
#include "fc/rc_controls.h"
|
#include "fc/rc_controls.h"
|
||||||
|
|
||||||
#ifdef USE_SERVOS
|
|
||||||
#include "flight/servos.h"
|
#include "flight/servos.h"
|
||||||
#endif
|
|
||||||
|
|
||||||
#include "io/osd.h"
|
#include "io/osd.h"
|
||||||
#include "io/ledstrip.h"
|
#include "io/ledstrip.h"
|
||||||
|
|
|
@ -179,7 +179,6 @@ pwmIOConfiguration_t *pwmInit(drv_pwm_config_t *init)
|
||||||
// Handle outputs - may override the PWM/PPM inputs
|
// Handle outputs - may override the PWM/PPM inputs
|
||||||
if (init->flyingPlatformType == PLATFORM_MULTIROTOR) {
|
if (init->flyingPlatformType == PLATFORM_MULTIROTOR) {
|
||||||
// Multicopter
|
// Multicopter
|
||||||
#ifdef USE_SERVOS
|
|
||||||
if (init->useServoOutputs && (timerHardwarePtr->usageFlags & TIM_USE_MC_SERVO)) {
|
if (init->useServoOutputs && (timerHardwarePtr->usageFlags & TIM_USE_MC_SERVO)) {
|
||||||
type = MAP_TO_SERVO_OUTPUT;
|
type = MAP_TO_SERVO_OUTPUT;
|
||||||
}
|
}
|
||||||
|
@ -187,13 +186,10 @@ pwmIOConfiguration_t *pwmInit(drv_pwm_config_t *init)
|
||||||
type = MAP_TO_SERVO_OUTPUT;
|
type = MAP_TO_SERVO_OUTPUT;
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
#endif
|
|
||||||
if (timerHardwarePtr->usageFlags & TIM_USE_MC_MOTOR) {
|
if (timerHardwarePtr->usageFlags & TIM_USE_MC_MOTOR) {
|
||||||
type = MAP_TO_MOTOR_OUTPUT;
|
type = MAP_TO_MOTOR_OUTPUT;
|
||||||
}
|
}
|
||||||
}
|
} else if (init->flyingPlatformType == PLATFORM_AIRPLANE || init->flyingPlatformType == PLATFORM_HELICOPTER) {
|
||||||
#ifdef USE_SERVOS
|
|
||||||
else if (init->flyingPlatformType == PLATFORM_AIRPLANE || init->flyingPlatformType == PLATFORM_HELICOPTER) {
|
|
||||||
// Fixed wing or HELI (one/two motors and a lot of servos
|
// Fixed wing or HELI (one/two motors and a lot of servos
|
||||||
if (timerHardwarePtr->usageFlags & TIM_USE_FW_SERVO) {
|
if (timerHardwarePtr->usageFlags & TIM_USE_FW_SERVO) {
|
||||||
type = MAP_TO_SERVO_OUTPUT;
|
type = MAP_TO_SERVO_OUTPUT;
|
||||||
|
@ -202,37 +198,10 @@ pwmIOConfiguration_t *pwmInit(drv_pwm_config_t *init)
|
||||||
type = MAP_TO_MOTOR_OUTPUT;
|
type = MAP_TO_MOTOR_OUTPUT;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
#endif
|
|
||||||
|
|
||||||
// If timer not mapped - skip
|
// If timer not mapped - skip
|
||||||
if (type == MAP_TO_NONE)
|
if (type == MAP_TO_NONE)
|
||||||
continue;
|
continue;
|
||||||
/*
|
|
||||||
#ifdef USE_SERVOS
|
|
||||||
if (init->useServos && !init->airplane) {
|
|
||||||
#if defined(SPRACINGF3MINI)
|
|
||||||
// remap PWM6+7 as servos
|
|
||||||
if ((timerIndex == PWM6 || timerIndex == PWM7) && timerHardwarePtr->tim == TIM15)
|
|
||||||
type = MAP_TO_SERVO_OUTPUT;
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#if defined(SINGULARITY)
|
|
||||||
// remap PWM6+7 as servos
|
|
||||||
if (timerIndex == PWM6 || timerIndex == PWM7)
|
|
||||||
type = MAP_TO_SERVO_OUTPUT;
|
|
||||||
#endif
|
|
||||||
}
|
|
||||||
|
|
||||||
#if defined(SPRACINGF3MINI)
|
|
||||||
if (((timerIndex == PWM6 || timerIndex == PWM7) && timerHardwarePtr->tim == TIM15)
|
|
||||||
|| ((timerIndex == PWM8 || timerIndex == PWM9 || timerIndex == PWM10 || timerIndex == PWM11) && timerHardwarePtr->tim == TIM2)) {
|
|
||||||
type = MAP_TO_SERVO_OUTPUT;
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
}
|
|
||||||
|
|
||||||
#endif // USE_SERVOS
|
|
||||||
*/
|
|
||||||
|
|
||||||
if (type == MAP_TO_PPM_INPUT) {
|
if (type == MAP_TO_PPM_INPUT) {
|
||||||
#if defined(USE_RX_PPM)
|
#if defined(USE_RX_PPM)
|
||||||
|
@ -284,7 +253,6 @@ pwmIOConfiguration_t *pwmInit(drv_pwm_config_t *init)
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
|
|
||||||
#ifdef USE_SERVOS
|
|
||||||
if (pwmServoConfig(timerHardwarePtr, pwmIOConfiguration.servoCount, init->servoPwmRate, init->servoCenterPulse, init->enablePWMOutput)) {
|
if (pwmServoConfig(timerHardwarePtr, pwmIOConfiguration.servoCount, init->servoPwmRate, init->servoCenterPulse, init->enablePWMOutput)) {
|
||||||
pwmIOConfiguration.ioConfigurations[pwmIOConfiguration.ioCount].flags = PWM_PF_SERVO | PWM_PF_OUTPUT_PROTOCOL_PWM;
|
pwmIOConfiguration.ioConfigurations[pwmIOConfiguration.ioCount].flags = PWM_PF_SERVO | PWM_PF_OUTPUT_PROTOCOL_PWM;
|
||||||
pwmIOConfiguration.ioConfigurations[pwmIOConfiguration.ioCount].index = pwmIOConfiguration.servoCount;
|
pwmIOConfiguration.ioConfigurations[pwmIOConfiguration.ioCount].index = pwmIOConfiguration.servoCount;
|
||||||
|
@ -298,7 +266,6 @@ pwmIOConfiguration_t *pwmInit(drv_pwm_config_t *init)
|
||||||
addBootlogEvent6(BOOT_EVENT_TIMER_CH_SKIPPED, BOOT_EVENT_FLAGS_WARNING, timerIndex, pwmIOConfiguration.motorCount, pwmIOConfiguration.servoCount, 3);
|
addBootlogEvent6(BOOT_EVENT_TIMER_CH_SKIPPED, BOOT_EVENT_FLAGS_WARNING, timerIndex, pwmIOConfiguration.motorCount, pwmIOConfiguration.servoCount, 3);
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
#endif
|
|
||||||
} else {
|
} else {
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
|
|
|
@ -68,12 +68,10 @@ typedef struct drv_pwm_config_s {
|
||||||
#ifdef USE_RANGEFINDER
|
#ifdef USE_RANGEFINDER
|
||||||
bool useTriggerRangefinder;
|
bool useTriggerRangefinder;
|
||||||
#endif
|
#endif
|
||||||
#ifdef USE_SERVOS
|
|
||||||
bool useServoOutputs;
|
bool useServoOutputs;
|
||||||
bool useChannelForwarding; // configure additional channels as servos
|
bool useChannelForwarding; // configure additional channels as servos
|
||||||
uint16_t servoPwmRate;
|
uint16_t servoPwmRate;
|
||||||
uint16_t servoCenterPulse;
|
uint16_t servoCenterPulse;
|
||||||
#endif
|
|
||||||
uint8_t pwmProtocolType;
|
uint8_t pwmProtocolType;
|
||||||
uint16_t motorPwmRate;
|
uint16_t motorPwmRate;
|
||||||
uint16_t idlePulse; // PWM value to use when initializing the driver. set this to either PULSE_1MS (regular pwm),
|
uint16_t idlePulse; // PWM value to use when initializing the driver. set this to either PULSE_1MS (regular pwm),
|
||||||
|
|
|
@ -50,10 +50,7 @@ typedef struct {
|
||||||
static pwmOutputPort_t pwmOutputPorts[MAX_PWM_OUTPUT_PORTS];
|
static pwmOutputPort_t pwmOutputPorts[MAX_PWM_OUTPUT_PORTS];
|
||||||
|
|
||||||
static pwmOutputPort_t *motors[MAX_PWM_MOTORS];
|
static pwmOutputPort_t *motors[MAX_PWM_MOTORS];
|
||||||
|
|
||||||
#ifdef USE_SERVOS
|
|
||||||
static pwmOutputPort_t *servos[MAX_PWM_SERVOS];
|
static pwmOutputPort_t *servos[MAX_PWM_SERVOS];
|
||||||
#endif
|
|
||||||
|
|
||||||
#ifdef BEEPER_PWM
|
#ifdef BEEPER_PWM
|
||||||
static pwmOutputPort_t beeperPwmPort;
|
static pwmOutputPort_t beeperPwmPort;
|
||||||
|
@ -290,7 +287,6 @@ bool pwmMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, ui
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
#ifdef USE_SERVOS
|
|
||||||
bool pwmServoConfig(const timerHardware_t *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse, bool enableOutput)
|
bool pwmServoConfig(const timerHardware_t *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse, bool enableOutput)
|
||||||
{
|
{
|
||||||
pwmOutputPort_t * port = pwmOutConfigMotor(timerHardware, PWM_TIMER_MHZ, 1000000 / servoPwmRate, servoCenterPulse, enableOutput);
|
pwmOutputPort_t * port = pwmOutConfigMotor(timerHardware, PWM_TIMER_MHZ, 1000000 / servoPwmRate, servoCenterPulse, enableOutput);
|
||||||
|
@ -323,7 +319,6 @@ void pwmWriteServo(uint8_t index, uint16_t value)
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
#endif
|
|
||||||
|
|
||||||
#ifdef BEEPER_PWM
|
#ifdef BEEPER_PWM
|
||||||
void pwmWriteBeeper(bool onoffBeep)
|
void pwmWriteBeeper(bool onoffBeep)
|
||||||
|
|
|
@ -1267,7 +1267,6 @@ static void cliModeColor(char *cmdline)
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifdef USE_SERVOS
|
|
||||||
static void printServo(uint8_t dumpMask, const servoParam_t *servoParam, const servoParam_t *defaultServoParam)
|
static void printServo(uint8_t dumpMask, const servoParam_t *servoParam, const servoParam_t *defaultServoParam)
|
||||||
{
|
{
|
||||||
// print out servo settings
|
// print out servo settings
|
||||||
|
@ -1530,7 +1529,6 @@ static void cliServoMix(char *cmdline)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
#endif // USE_SERVOS
|
|
||||||
|
|
||||||
#ifdef USE_SDCARD
|
#ifdef USE_SDCARD
|
||||||
|
|
||||||
|
@ -2575,7 +2573,6 @@ static void printConfig(const char *cmdline, bool doDiff)
|
||||||
|
|
||||||
printMotorMix(dumpMask, customMotorMixer_CopyArray, customMotorMixer(0));
|
printMotorMix(dumpMask, customMotorMixer_CopyArray, customMotorMixer(0));
|
||||||
|
|
||||||
#ifdef USE_SERVOS
|
|
||||||
// print custom servo mixer if exists
|
// print custom servo mixer if exists
|
||||||
cliPrintHashLine("servo mix");
|
cliPrintHashLine("servo mix");
|
||||||
cliDumpPrintLinef(dumpMask, customServoMixers(0)->rate == 0, "smix reset\r\n");
|
cliDumpPrintLinef(dumpMask, customServoMixers(0)->rate == 0, "smix reset\r\n");
|
||||||
|
@ -2584,7 +2581,6 @@ static void printConfig(const char *cmdline, bool doDiff)
|
||||||
// print servo parameters
|
// print servo parameters
|
||||||
cliPrintHashLine("servo");
|
cliPrintHashLine("servo");
|
||||||
printServo(dumpMask, servoParams_CopyArray, servoParams(0));
|
printServo(dumpMask, servoParams_CopyArray, servoParams(0));
|
||||||
#endif
|
|
||||||
|
|
||||||
cliPrintHashLine("feature");
|
cliPrintHashLine("feature");
|
||||||
printFeature(dumpMask, &featureConfig_Copy, featureConfig());
|
printFeature(dumpMask, &featureConfig_Copy, featureConfig());
|
||||||
|
@ -2755,17 +2751,13 @@ const clicmd_t cmdTable[] = {
|
||||||
#ifdef USE_SERIAL_PASSTHROUGH
|
#ifdef USE_SERIAL_PASSTHROUGH
|
||||||
CLI_COMMAND_DEF("serialpassthrough", "passthrough serial data to port", "<id> [baud] [mode] : passthrough to serial", cliSerialPassthrough),
|
CLI_COMMAND_DEF("serialpassthrough", "passthrough serial data to port", "<id> [baud] [mode] : passthrough to serial", cliSerialPassthrough),
|
||||||
#endif
|
#endif
|
||||||
#ifdef USE_SERVOS
|
|
||||||
CLI_COMMAND_DEF("servo", "configure servos", NULL, cliServo),
|
CLI_COMMAND_DEF("servo", "configure servos", NULL, cliServo),
|
||||||
#endif
|
|
||||||
CLI_COMMAND_DEF("set", "change setting", "[<name>=<value>]", cliSet),
|
CLI_COMMAND_DEF("set", "change setting", "[<name>=<value>]", cliSet),
|
||||||
#ifdef USE_SERVOS
|
|
||||||
CLI_COMMAND_DEF("smix", "servo mixer",
|
CLI_COMMAND_DEF("smix", "servo mixer",
|
||||||
"<rule> <servo> <source> <rate> <speed>\r\n"
|
"<rule> <servo> <source> <rate> <speed>\r\n"
|
||||||
"\treset\r\n"
|
"\treset\r\n"
|
||||||
"\tload <mixer>\r\n"
|
"\tload <mixer>\r\n"
|
||||||
"\treverse <servo> <source> r|n", cliServoMix),
|
"\treverse <servo> <source> r|n", cliServoMix),
|
||||||
#endif
|
|
||||||
#ifdef USE_SDCARD
|
#ifdef USE_SDCARD
|
||||||
CLI_COMMAND_DEF("sd_info", "sdcard info", NULL, cliSdInfo),
|
CLI_COMMAND_DEF("sd_info", "sdcard info", NULL, cliSdInfo),
|
||||||
#endif
|
#endif
|
||||||
|
|
|
@ -524,7 +524,6 @@ void processRx(timeUs_t currentTimeUs)
|
||||||
LED1_OFF;
|
LED1_OFF;
|
||||||
}
|
}
|
||||||
|
|
||||||
#ifdef USE_SERVOS
|
|
||||||
/* Flaperon mode */
|
/* Flaperon mode */
|
||||||
if (IS_RC_MODE_ACTIVE(BOXFLAPERON) && STATE(FLAPERON_AVAILABLE)) {
|
if (IS_RC_MODE_ACTIVE(BOXFLAPERON) && STATE(FLAPERON_AVAILABLE)) {
|
||||||
if (!FLIGHT_MODE(FLAPERON)) {
|
if (!FLIGHT_MODE(FLAPERON)) {
|
||||||
|
@ -533,7 +532,6 @@ void processRx(timeUs_t currentTimeUs)
|
||||||
} else {
|
} else {
|
||||||
DISABLE_FLIGHT_MODE(FLAPERON);
|
DISABLE_FLIGHT_MODE(FLAPERON);
|
||||||
}
|
}
|
||||||
#endif
|
|
||||||
|
|
||||||
#ifdef USE_FLM_TURN_ASSIST
|
#ifdef USE_FLM_TURN_ASSIST
|
||||||
/* Turn assistant mode */
|
/* Turn assistant mode */
|
||||||
|
@ -756,9 +754,7 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
|
||||||
// motors do not spin up while we are trying to arm or disarm.
|
// motors do not spin up while we are trying to arm or disarm.
|
||||||
// Allow yaw control for tricopters if the user wants the servo to move even when unarmed.
|
// Allow yaw control for tricopters if the user wants the servo to move even when unarmed.
|
||||||
if (isUsingSticksForArming() && rcData[THROTTLE] <= rxConfig()->mincheck
|
if (isUsingSticksForArming() && rcData[THROTTLE] <= rxConfig()->mincheck
|
||||||
#ifdef USE_SERVOS
|
|
||||||
&& !((mixerConfig()->mixerMode == MIXER_TRI || mixerConfig()->mixerMode == MIXER_CUSTOM_TRI) && servoConfig()->tri_unarmed_servo)
|
&& !((mixerConfig()->mixerMode == MIXER_TRI || mixerConfig()->mixerMode == MIXER_CUSTOM_TRI) && servoConfig()->tri_unarmed_servo)
|
||||||
#endif
|
|
||||||
&& mixerConfig()->mixerMode != MIXER_AIRPLANE
|
&& mixerConfig()->mixerMode != MIXER_AIRPLANE
|
||||||
&& mixerConfig()->mixerMode != MIXER_FLYING_WING
|
&& mixerConfig()->mixerMode != MIXER_FLYING_WING
|
||||||
&& mixerConfig()->mixerMode != MIXER_CUSTOM_AIRPLANE
|
&& mixerConfig()->mixerMode != MIXER_CUSTOM_AIRPLANE
|
||||||
|
@ -803,7 +799,6 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
|
||||||
|
|
||||||
mixTable();
|
mixTable();
|
||||||
|
|
||||||
#ifdef USE_SERVOS
|
|
||||||
if (isMixerUsingServos()) {
|
if (isMixerUsingServos()) {
|
||||||
servoMixer(dT);
|
servoMixer(dT);
|
||||||
processServoAutotrim();
|
processServoAutotrim();
|
||||||
|
@ -818,7 +813,6 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
|
||||||
if (isServoOutputEnabled()) {
|
if (isServoOutputEnabled()) {
|
||||||
writeServos();
|
writeServos();
|
||||||
}
|
}
|
||||||
#endif
|
|
||||||
|
|
||||||
if (motorControlEnable) {
|
if (motorControlEnable) {
|
||||||
writeMotors();
|
writeMotors();
|
||||||
|
|
|
@ -269,10 +269,8 @@ void init(void)
|
||||||
debugTraceInit();
|
debugTraceInit();
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifdef USE_SERVOS
|
|
||||||
servosInit();
|
servosInit();
|
||||||
mixerUpdateStateFlags(); // This needs to be called early to allow pwm mapper to use information about FIXED_WING state
|
mixerUpdateStateFlags(); // This needs to be called early to allow pwm mapper to use information about FIXED_WING state
|
||||||
#endif
|
|
||||||
|
|
||||||
drv_pwm_config_t pwm_params;
|
drv_pwm_config_t pwm_params;
|
||||||
memset(&pwm_params, 0, sizeof(pwm_params));
|
memset(&pwm_params, 0, sizeof(pwm_params));
|
||||||
|
@ -314,12 +312,10 @@ void init(void)
|
||||||
pwm_params.usePPM = (rxConfig()->receiverType == RX_TYPE_PPM);
|
pwm_params.usePPM = (rxConfig()->receiverType == RX_TYPE_PPM);
|
||||||
pwm_params.useSerialRx = (rxConfig()->receiverType == RX_TYPE_SERIAL);
|
pwm_params.useSerialRx = (rxConfig()->receiverType == RX_TYPE_SERIAL);
|
||||||
|
|
||||||
#ifdef USE_SERVOS
|
|
||||||
pwm_params.useServoOutputs = isMixerUsingServos();
|
pwm_params.useServoOutputs = isMixerUsingServos();
|
||||||
pwm_params.useChannelForwarding = feature(FEATURE_CHANNEL_FORWARDING);
|
pwm_params.useChannelForwarding = feature(FEATURE_CHANNEL_FORWARDING);
|
||||||
pwm_params.servoCenterPulse = servoConfig()->servoCenterPulse;
|
pwm_params.servoCenterPulse = servoConfig()->servoCenterPulse;
|
||||||
pwm_params.servoPwmRate = servoConfig()->servoPwmRate;
|
pwm_params.servoPwmRate = servoConfig()->servoPwmRate;
|
||||||
#endif
|
|
||||||
|
|
||||||
pwm_params.pwmProtocolType = motorConfig()->motorPwmProtocol;
|
pwm_params.pwmProtocolType = motorConfig()->motorPwmProtocol;
|
||||||
#ifndef BRUSHED_MOTORS
|
#ifndef BRUSHED_MOTORS
|
||||||
|
|
|
@ -439,7 +439,6 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
|
|
||||||
#ifdef USE_SERVOS
|
|
||||||
case MSP_SERVO:
|
case MSP_SERVO:
|
||||||
sbufWriteData(dst, &servo, MAX_SUPPORTED_SERVOS * 2);
|
sbufWriteData(dst, &servo, MAX_SUPPORTED_SERVOS * 2);
|
||||||
break;
|
break;
|
||||||
|
@ -466,8 +465,6 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
|
||||||
sbufWriteU8(dst, 0);
|
sbufWriteU8(dst, 0);
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
#endif
|
|
||||||
|
|
||||||
case MSP2_COMMON_MOTOR_MIXER:
|
case MSP2_COMMON_MOTOR_MIXER:
|
||||||
for (uint8_t i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
|
for (uint8_t i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
|
||||||
sbufWriteU16(dst, customMotorMixer(i)->throttle * 1000);
|
sbufWriteU16(dst, customMotorMixer(i)->throttle * 1000);
|
||||||
|
@ -1009,11 +1006,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
|
||||||
sbufWriteU8(dst, 1); // BF: motorConfig()->useUnsyncedPwm
|
sbufWriteU8(dst, 1); // BF: motorConfig()->useUnsyncedPwm
|
||||||
sbufWriteU8(dst, motorConfig()->motorPwmProtocol);
|
sbufWriteU8(dst, motorConfig()->motorPwmProtocol);
|
||||||
sbufWriteU16(dst, motorConfig()->motorPwmRate);
|
sbufWriteU16(dst, motorConfig()->motorPwmRate);
|
||||||
#ifdef USE_SERVOS
|
|
||||||
sbufWriteU16(dst, servoConfig()->servoPwmRate);
|
sbufWriteU16(dst, servoConfig()->servoPwmRate);
|
||||||
#else
|
|
||||||
sbufWriteU16(dst, 0);
|
|
||||||
#endif
|
|
||||||
sbufWriteU8(dst, gyroConfig()->gyroSync);
|
sbufWriteU8(dst, gyroConfig()->gyroSync);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
|
@ -1651,7 +1644,6 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
|
||||||
return MSP_RESULT_ERROR;
|
return MSP_RESULT_ERROR;
|
||||||
break;
|
break;
|
||||||
|
|
||||||
#ifdef USE_SERVOS
|
|
||||||
case MSP_SET_SERVO_CONFIGURATION:
|
case MSP_SET_SERVO_CONFIGURATION:
|
||||||
if (dataSize != (1 + 14)) {
|
if (dataSize != (1 + 14)) {
|
||||||
return MSP_RESULT_ERROR;
|
return MSP_RESULT_ERROR;
|
||||||
|
@ -1671,9 +1663,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
|
||||||
servoComputeScalingFactors(tmp_u8);
|
servoComputeScalingFactors(tmp_u8);
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
#endif
|
|
||||||
|
|
||||||
#ifdef USE_SERVOS
|
|
||||||
case MSP_SET_SERVO_MIX_RULE:
|
case MSP_SET_SERVO_MIX_RULE:
|
||||||
sbufReadU8Safe(&tmp_u8, src);
|
sbufReadU8Safe(&tmp_u8, src);
|
||||||
if ((dataSize >= 8) && (tmp_u8 < MAX_SERVO_RULES)) {
|
if ((dataSize >= 8) && (tmp_u8 < MAX_SERVO_RULES)) {
|
||||||
|
@ -1687,7 +1677,6 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
|
||||||
} else
|
} else
|
||||||
return MSP_RESULT_ERROR;
|
return MSP_RESULT_ERROR;
|
||||||
break;
|
break;
|
||||||
#endif
|
|
||||||
|
|
||||||
case MSP2_COMMON_SET_MOTOR_MIXER:
|
case MSP2_COMMON_SET_MOTOR_MIXER:
|
||||||
sbufReadU8Safe(&tmp_u8, src);
|
sbufReadU8Safe(&tmp_u8, src);
|
||||||
|
@ -1743,11 +1732,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
|
||||||
sbufReadU8(src); // BF: motorConfig()->useUnsyncedPwm
|
sbufReadU8(src); // BF: motorConfig()->useUnsyncedPwm
|
||||||
motorConfigMutable()->motorPwmProtocol = sbufReadU8(src);
|
motorConfigMutable()->motorPwmProtocol = sbufReadU8(src);
|
||||||
motorConfigMutable()->motorPwmRate = sbufReadU16(src);
|
motorConfigMutable()->motorPwmRate = sbufReadU16(src);
|
||||||
#ifdef USE_SERVOS
|
|
||||||
servoConfigMutable()->servoPwmRate = sbufReadU16(src);
|
servoConfigMutable()->servoPwmRate = sbufReadU16(src);
|
||||||
#else
|
|
||||||
sbufReadU16(src);
|
|
||||||
#endif
|
|
||||||
gyroConfigMutable()->gyroSync = sbufReadU8(src);
|
gyroConfigMutable()->gyroSync = sbufReadU8(src);
|
||||||
} else
|
} else
|
||||||
return MSP_RESULT_ERROR;
|
return MSP_RESULT_ERROR;
|
||||||
|
|
|
@ -199,7 +199,6 @@ void initActiveBoxIds(void)
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
#ifdef USE_SERVOS
|
|
||||||
/*
|
/*
|
||||||
* FLAPERON mode active only in case of airplane and custom airplane. Activating on
|
* FLAPERON mode active only in case of airplane and custom airplane. Activating on
|
||||||
* flying wing can cause bad thing
|
* flying wing can cause bad thing
|
||||||
|
@ -207,7 +206,6 @@ void initActiveBoxIds(void)
|
||||||
if (STATE(FLAPERON_AVAILABLE)) {
|
if (STATE(FLAPERON_AVAILABLE)) {
|
||||||
activeBoxIds[activeBoxIdCount++] = BOXFLAPERON;
|
activeBoxIds[activeBoxIdCount++] = BOXFLAPERON;
|
||||||
}
|
}
|
||||||
#endif
|
|
||||||
|
|
||||||
activeBoxIds[activeBoxIdCount++] = BOXBEEPERON;
|
activeBoxIds[activeBoxIdCount++] = BOXBEEPERON;
|
||||||
|
|
||||||
|
|
|
@ -487,7 +487,6 @@ groups:
|
||||||
- name: PG_GIMBAL_CONFIG
|
- name: PG_GIMBAL_CONFIG
|
||||||
type: gimbalConfig_t
|
type: gimbalConfig_t
|
||||||
headers: ["io/gimbal.h"]
|
headers: ["io/gimbal.h"]
|
||||||
condition: USE_SERVOS
|
|
||||||
members:
|
members:
|
||||||
- name: gimbal_mode
|
- name: gimbal_mode
|
||||||
field: mode
|
field: mode
|
||||||
|
@ -582,7 +581,6 @@ groups:
|
||||||
- name: PG_SERVO_CONFIG
|
- name: PG_SERVO_CONFIG
|
||||||
type: servoConfig_t
|
type: servoConfig_t
|
||||||
headers: ["flight/servos.h"]
|
headers: ["flight/servos.h"]
|
||||||
condition: USE_SERVOS
|
|
||||||
members:
|
members:
|
||||||
- name: servo_center_pulse
|
- name: servo_center_pulse
|
||||||
field: servoCenterPulse
|
field: servoCenterPulse
|
||||||
|
@ -880,17 +878,14 @@ groups:
|
||||||
max: 2
|
max: 2
|
||||||
- name: fw_iterm_throw_limit
|
- name: fw_iterm_throw_limit
|
||||||
field: fixedWingItermThrowLimit
|
field: fixedWingItermThrowLimit
|
||||||
condition: USE_SERVOS
|
|
||||||
min: FW_ITERM_THROW_LIMIT_MIN
|
min: FW_ITERM_THROW_LIMIT_MIN
|
||||||
max: FW_ITERM_THROW_LIMIT_MAX
|
max: FW_ITERM_THROW_LIMIT_MAX
|
||||||
- name: fw_reference_airspeed
|
- name: fw_reference_airspeed
|
||||||
field: fixedWingReferenceAirspeed
|
field: fixedWingReferenceAirspeed
|
||||||
condition: USE_SERVOS
|
|
||||||
min: 1
|
min: 1
|
||||||
max: 5000
|
max: 5000
|
||||||
- name: fw_turn_assist_yaw_gain
|
- name: fw_turn_assist_yaw_gain
|
||||||
field: fixedWingCoordinatedYawGain
|
field: fixedWingCoordinatedYawGain
|
||||||
condition: USE_SERVOS
|
|
||||||
min: 0
|
min: 0
|
||||||
max: 2
|
max: 2
|
||||||
- name: dterm_notch_hz
|
- name: dterm_notch_hz
|
||||||
|
|
|
@ -302,7 +302,6 @@ bool isMixerEnabled(mixerMode_e mixerMode)
|
||||||
return (mixer != NULL) ? true : false;
|
return (mixer != NULL) ? true : false;
|
||||||
}
|
}
|
||||||
|
|
||||||
#ifdef USE_SERVOS
|
|
||||||
void mixerUpdateStateFlags(void)
|
void mixerUpdateStateFlags(void)
|
||||||
{
|
{
|
||||||
// set flag that we're on something with wings
|
// set flag that we're on something with wings
|
||||||
|
@ -363,16 +362,6 @@ void mixerUsePWMIOConfiguration(void)
|
||||||
|
|
||||||
mixerResetDisarmedMotors();
|
mixerResetDisarmedMotors();
|
||||||
}
|
}
|
||||||
#else
|
|
||||||
void mixerUsePWMIOConfiguration(void)
|
|
||||||
{
|
|
||||||
motorCount = 4;
|
|
||||||
for (int i = 0; i < motorCount; i++) {
|
|
||||||
currentMixer[i] = mixerQuadX[i];
|
|
||||||
}
|
|
||||||
mixerResetDisarmedMotors();
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
||||||
void mixerLoadMix(int index, motorMixer_t *customMixers)
|
void mixerLoadMix(int index, motorMixer_t *customMixers)
|
||||||
{
|
{
|
||||||
|
|
|
@ -443,7 +443,6 @@ static void pidApplySetpointRateLimiting(pidState_t *pidState, flight_dynamics_i
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
#ifdef USE_SERVOS
|
|
||||||
static void pidApplyFixedWingRateController(pidState_t *pidState, flight_dynamics_index_t axis)
|
static void pidApplyFixedWingRateController(pidState_t *pidState, flight_dynamics_index_t axis)
|
||||||
{
|
{
|
||||||
const float rateError = pidState->rateTarget - pidState->gyroRate;
|
const float rateError = pidState->rateTarget - pidState->gyroRate;
|
||||||
|
@ -485,7 +484,6 @@ static void pidApplyFixedWingRateController(pidState_t *pidState, flight_dynamic
|
||||||
axisPID_Setpoint[axis] = pidState->rateTarget;
|
axisPID_Setpoint[axis] = pidState->rateTarget;
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
#endif
|
|
||||||
|
|
||||||
static void pidApplyMulticopterRateController(pidState_t *pidState, flight_dynamics_index_t axis)
|
static void pidApplyMulticopterRateController(pidState_t *pidState, flight_dynamics_index_t axis)
|
||||||
{
|
{
|
||||||
|
@ -761,15 +759,11 @@ void pidController(void)
|
||||||
// Step 4: Run gyro-driven control
|
// Step 4: Run gyro-driven control
|
||||||
for (int axis = 0; axis < 3; axis++) {
|
for (int axis = 0; axis < 3; axis++) {
|
||||||
// Apply PID setpoint controller
|
// Apply PID setpoint controller
|
||||||
#ifdef USE_SERVOS
|
|
||||||
if (STATE(FIXED_WING)) {
|
if (STATE(FIXED_WING)) {
|
||||||
pidApplyFixedWingRateController(&pidState[axis], axis);
|
pidApplyFixedWingRateController(&pidState[axis], axis);
|
||||||
}
|
}
|
||||||
else {
|
else {
|
||||||
pidApplyMulticopterRateController(&pidState[axis], axis);
|
pidApplyMulticopterRateController(&pidState[axis], axis);
|
||||||
}
|
}
|
||||||
#else
|
|
||||||
pidApplyMulticopterRateController(&pidState[axis], axis);
|
|
||||||
#endif
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
@ -22,8 +22,6 @@
|
||||||
|
|
||||||
#include "platform.h"
|
#include "platform.h"
|
||||||
|
|
||||||
#ifdef USE_SERVOS
|
|
||||||
|
|
||||||
#include "build/debug.h"
|
#include "build/debug.h"
|
||||||
#include "build/build_config.h"
|
#include "build/build_config.h"
|
||||||
|
|
||||||
|
@ -568,5 +566,4 @@ bool isServoOutputEnabled(void)
|
||||||
bool isMixerUsingServos(void)
|
bool isMixerUsingServos(void)
|
||||||
{
|
{
|
||||||
return mixerUsesServos;
|
return mixerUsesServos;
|
||||||
}
|
}
|
||||||
#endif
|
|
|
@ -136,7 +136,6 @@
|
||||||
// Number of available PWM outputs
|
// Number of available PWM outputs
|
||||||
#define MAX_PWM_OUTPUT_PORTS 10
|
#define MAX_PWM_OUTPUT_PORTS 10
|
||||||
#define TARGET_MOTOR_COUNT 10
|
#define TARGET_MOTOR_COUNT 10
|
||||||
#define USE_SERVOS
|
|
||||||
|
|
||||||
// IO - stm32f303cc in 48pin package
|
// IO - stm32f303cc in 48pin package
|
||||||
#define TARGET_IO_PORTA 0xffff
|
#define TARGET_IO_PORTA 0xffff
|
||||||
|
|
|
@ -67,7 +67,6 @@
|
||||||
#define USE_TELEMETRY
|
#define USE_TELEMETRY
|
||||||
#define USE_BLACKBOX
|
#define USE_BLACKBOX
|
||||||
#define USE_SERIAL_RX
|
#define USE_SERIAL_RX
|
||||||
#define USE_SERVOS
|
|
||||||
|
|
||||||
#define BOARD_HAS_VOLTAGE_DIVIDER
|
#define BOARD_HAS_VOLTAGE_DIVIDER
|
||||||
#define USE_ADC
|
#define USE_ADC
|
||||||
|
|
|
@ -21,7 +21,6 @@
|
||||||
#define I2C2_OVERCLOCK false
|
#define I2C2_OVERCLOCK false
|
||||||
#define USE_I2C_PULLUP // Enable built-in pullups on all boards in case external ones are too week
|
#define USE_I2C_PULLUP // Enable built-in pullups on all boards in case external ones are too week
|
||||||
|
|
||||||
#define USE_SERVOS
|
|
||||||
#define USE_CLI
|
#define USE_CLI
|
||||||
|
|
||||||
#define USE_RX_PWM
|
#define USE_RX_PWM
|
||||||
|
|
|
@ -34,7 +34,6 @@
|
||||||
#define USE_TELEMETRY_SMARTPORT
|
#define USE_TELEMETRY_SMARTPORT
|
||||||
#define USE_TELEMETRY_LTM
|
#define USE_TELEMETRY_LTM
|
||||||
#define USE_LED_STRIP
|
#define USE_LED_STRIP
|
||||||
#define USE_SERVOS
|
|
||||||
#define TRANSPONDER
|
#define TRANSPONDER
|
||||||
#define USE_FAKE_GYRO
|
#define USE_FAKE_GYRO
|
||||||
#define USE_VCP
|
#define USE_VCP
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue