static long cmsx_menuMiscOnEnter(void)
{
motorConfig_minthrottle = motorConfig()->minthrottle;
motorConfig_digitalIdleOffsetValue = motorConfig()->digitalIdleOffsetValue / 10;
voltageSensorADCConfig_vbatscale = voltageSensorADCConfig(VOLTAGE_SENSOR_ADC_VBAT)->vbatscale;
batteryConfig_vbatmaxcellvoltage = batteryConfig()->vbatmaxcellvoltage;
return 0;
}
static void applyFixedWingLaunchIdleLogic(void)
{
// Until motors are started don't use PID I-term
pidResetErrorAccumulators();
// Throttle control logic
if (navConfig()->fw.launch_idle_throttle <= motorConfig()->minthrottle) {
ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE); // If MOTOR_STOP is enabled mixer will keep motor stopped
rcCommand[THROTTLE] = motorConfig()->minthrottle; // If MOTOR_STOP is disabled, motors will spin at minthrottle
}
else {
rcCommand[THROTTLE] = navConfig()->fw.launch_idle_throttle;
}
}
void generateThrottleCurve(const controlRateConfig_t *controlRateConfig)
{
lookupThrottleRCMid = motorConfig()->minthrottle + (int32_t)(motorConfig()->maxthrottle - motorConfig()->minthrottle) * controlRateConfig->throttle.rcMid8 / 100; // [MINTHROTTLE;MAXTHROTTLE]
for (int i = 0; i < THROTTLE_LOOKUP_LENGTH; i++) {
const int16_t tmp = 10 * i - controlRateConfig->throttle.rcMid8;
uint8_t y = 1;
if (tmp > 0)
y = 100 - controlRateConfig->throttle.rcMid8;
if (tmp < 0)
y = controlRateConfig->throttle.rcMid8;
lookupThrottleRC[i] = 10 * controlRateConfig->throttle.rcMid8 + tmp * (100 - controlRateConfig->throttle.rcExpo8 + (int32_t) controlRateConfig->throttle.rcExpo8 * (tmp * tmp) / (y * y)) / 10;
lookupThrottleRC[i] = motorConfig()->minthrottle + (int32_t) (motorConfig()->maxthrottle - motorConfig()->minthrottle) * lookupThrottleRC[i] / 1000; // [MINTHROTTLE;MAXTHROTTLE]
}
}
uint16_t rcLookupThrottle(uint16_t absoluteDeflection)
{
if (absoluteDeflection > 999)
return motorConfig()->maxthrottle;
const uint8_t lookupStep = absoluteDeflection / 100;
return lookupThrottleRC[lookupStep] + (absoluteDeflection - lookupStep * 100) * (lookupThrottleRC[lookupStep + 1] - lookupThrottleRC[lookupStep]) / 100;
}
// All PWM motor scaling is done to standard PWM range of 1000-2000 for easier tick conversion with legacy code / configurator
// DSHOT scaling is done to the actual dshot range
void initEscEndpoints(void)
{
// Can't use 'isMotorProtocolDshot()' here since motors haven't been initialised yet
switch (motorConfig()->dev.motorPwmProtocol) {
#ifdef USE_DSHOT
case PWM_TYPE_PROSHOT1000:
case PWM_TYPE_DSHOT1200:
case PWM_TYPE_DSHOT600:
case PWM_TYPE_DSHOT300:
case PWM_TYPE_DSHOT150:
disarmMotorOutput = DSHOT_DISARM_COMMAND;
if (feature(FEATURE_3D)) {
motorOutputLow = DSHOT_MIN_THROTTLE + ((DSHOT_3D_DEADBAND_LOW - DSHOT_MIN_THROTTLE) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue);
} else {
motorOutputLow = DSHOT_MIN_THROTTLE + ((DSHOT_MAX_THROTTLE - DSHOT_MIN_THROTTLE) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue);
}
motorOutputHigh = DSHOT_MAX_THROTTLE;
deadbandMotor3dHigh = DSHOT_3D_DEADBAND_HIGH + ((DSHOT_MAX_THROTTLE - DSHOT_3D_DEADBAND_HIGH) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue);
deadbandMotor3dLow = DSHOT_3D_DEADBAND_LOW;
break;
#endif
default:
if (feature(FEATURE_3D)) {
disarmMotorOutput = flight3DConfig()->neutral3d;
motorOutputLow = flight3DConfig()->limit3d_low;
motorOutputHigh = flight3DConfig()->limit3d_high;
deadbandMotor3dHigh = flight3DConfig()->deadband3d_high;
deadbandMotor3dLow = flight3DConfig()->deadband3d_low;
} else {
disarmMotorOutput = motorConfig()->mincommand;
motorOutputLow = motorConfig()->minthrottle;
motorOutputHigh = motorConfig()->maxthrottle;
}
break;
}
rcCommandThrottleRange = PWM_RANGE_MAX - rxConfig()->mincheck;
rcCommand3dDeadBandLow = rxConfig()->midrc - flight3DConfig()->deadband3d_throttle;
rcCommand3dDeadBandHigh = rxConfig()->midrc + flight3DConfig()->deadband3d_throttle;
rcCommandThrottleRange3dLow = rcCommand3dDeadBandLow - PWM_RANGE_MIN;
rcCommandThrottleRange3dHigh = PWM_RANGE_MAX - rcCommand3dDeadBandHigh;
}
int16_t calculateMotorOff(void) {
int16_t motorOff = motorConfig()->mincommand;
#ifndef SKIP_3D_FLIGHT
if (feature(FEATURE_3D)) {
motorOff = motor3DConfig()->neutral3d;
}
#endif
return motorOff;
}
void init(void)
{
drv_pwm_config_t pwm_params;
printfSupportInit();
initEEPROM();
ensureEEPROMContainsValidData();
readEEPROM();
systemState |= SYSTEM_STATE_CONFIG_LOADED;
#ifdef STM32F303
// start fpu
SCB->CPACR = (0x3 << (10*2)) | (0x3 << (11*2));
#endif
#ifdef STM32F303xC
SetSysClock();
#endif
#ifdef STM32F10X
// Configure the System clock frequency, HCLK, PCLK2 and PCLK1 prescalers
// Configure the Flash Latency cycles and enable prefetch buffer
SetSysClock(systemConfig()->emf_avoidance);
#endif
i2cSetOverclock(systemConfig()->i2c_highspeed);
systemInit();
#ifdef USE_HARDWARE_REVISION_DETECTION
detectHardwareRevision();
#endif
// Latch active features to be used for feature() in the remainder of init().
latchActiveFeatures();
// initialize IO (needed for all IO operations)
IOInitGlobal();
debugMode = debugConfig()->debug_mode;
#ifdef USE_EXTI
EXTIInit();
#endif
#ifdef ALIENFLIGHTF3
if (hardwareRevision == AFF3_REV_1) {
ledInit(false);
} else {
ledInit(true);
}
#else
ledInit(false);
#endif
#ifdef BEEPER
beeperConfig_t beeperConfig = {
.gpioPeripheral = BEEP_PERIPHERAL,
.gpioPin = BEEP_PIN,
.gpioPort = BEEP_GPIO,
#ifdef BEEPER_INVERTED
.gpioMode = Mode_Out_PP,
.isInverted = true
#else
.gpioMode = Mode_Out_OD,
.isInverted = false
#endif
};
#ifdef NAZE
if (hardwareRevision >= NAZE32_REV5) {
// naze rev4 and below used opendrain to PNP for buzzer. Rev5 and above use PP to NPN.
beeperConfig.gpioMode = Mode_Out_PP;
beeperConfig.isInverted = true;
}
#endif
beeperInit(&beeperConfig);
#endif
#ifdef BUTTONS
buttonsInit();
if (!isMPUSoftReset()) {
buttonsHandleColdBootButtonPresses();
}
#endif
#ifdef SPEKTRUM_BIND
if (feature(FEATURE_RX_SERIAL)) {
switch (rxConfig()->serialrx_provider) {
case SERIALRX_SPEKTRUM1024:
case SERIALRX_SPEKTRUM2048:
// Spektrum satellite binding if enabled on startup.
// Must be called before that 100ms sleep so that we don't lose satellite's binding window after startup.
// The rest of Spektrum initialization will happen later - via spektrumInit()
spektrumBind(rxConfig());
break;
}
}
#endif
delay(100);
timerInit(); // timer must be initialized before any channel is allocated
dmaInit();
serialInit(feature(FEATURE_SOFTSERIAL));
mixerInit(customMotorMixer(0));
#ifdef USE_SERVOS
mixerInitServos(customServoMixer(0));
#endif
memset(&pwm_params, 0, sizeof(pwm_params));
#ifdef SONAR
const sonarHardware_t *sonarHardware = NULL;
sonarGPIOConfig_t sonarGPIOConfig;
if (feature(FEATURE_SONAR)) {
bool usingCurrentMeterIOPins = (feature(FEATURE_AMPERAGE_METER) && batteryConfig()->amperageMeterSource == AMPERAGE_METER_ADC);
sonarHardware = sonarGetHardwareConfiguration(usingCurrentMeterIOPins);
sonarGPIOConfig.triggerGPIO = sonarHardware->trigger_gpio;
sonarGPIOConfig.triggerPin = sonarHardware->trigger_pin;
sonarGPIOConfig.echoGPIO = sonarHardware->echo_gpio;
sonarGPIOConfig.echoPin = sonarHardware->echo_pin;
pwm_params.sonarGPIOConfig = &sonarGPIOConfig;
}
#endif
// when using airplane/wing mixer, servo/motor outputs are remapped
if (mixerConfig()->mixerMode == MIXER_AIRPLANE || mixerConfig()->mixerMode == MIXER_FLYING_WING || mixerConfig()->mixerMode == MIXER_CUSTOM_AIRPLANE)
pwm_params.airplane = true;
else
pwm_params.airplane = false;
#if defined(USE_UART2) && defined(STM32F10X)
pwm_params.useUART2 = doesConfigurationUsePort(SERIAL_PORT_UART2);
#endif
#if defined(USE_UART3)
pwm_params.useUART3 = doesConfigurationUsePort(SERIAL_PORT_UART3);
#endif
#if defined(USE_UART4)
pwm_params.useUART4 = doesConfigurationUsePort(SERIAL_PORT_UART4);
#endif
#if defined(USE_UART5)
pwm_params.useUART5 = doesConfigurationUsePort(SERIAL_PORT_UART5);
#endif
pwm_params.useVbat = feature(FEATURE_VBAT);
pwm_params.useSoftSerial = feature(FEATURE_SOFTSERIAL);
pwm_params.useParallelPWM = feature(FEATURE_RX_PARALLEL_PWM);
pwm_params.useRSSIADC = feature(FEATURE_RSSI_ADC);
pwm_params.useCurrentMeterADC = (
feature(FEATURE_AMPERAGE_METER)
&& batteryConfig()->amperageMeterSource == AMPERAGE_METER_ADC
);
pwm_params.useLEDStrip = feature(FEATURE_LED_STRIP);
pwm_params.usePPM = feature(FEATURE_RX_PPM);
pwm_params.useSerialRx = feature(FEATURE_RX_SERIAL);
#ifdef SONAR
pwm_params.useSonar = feature(FEATURE_SONAR);
#endif
#ifdef USE_SERVOS
pwm_params.useServos = isMixerUsingServos();
pwm_params.useChannelForwarding = feature(FEATURE_CHANNEL_FORWARDING);
pwm_params.servoCenterPulse = servoConfig()->servoCenterPulse;
pwm_params.servoPwmRate = servoConfig()->servo_pwm_rate;
#endif
pwm_params.useOneshot = feature(FEATURE_ONESHOT125);
pwm_params.motorPwmRate = motorConfig()->motor_pwm_rate;
pwm_params.idlePulse = calculateMotorOff();
if (pwm_params.motorPwmRate > 500)
pwm_params.idlePulse = 0; // brushed motors
pwmRxInit();
// pwmInit() needs to be called as soon as possible for ESC compatibility reasons
pwmIOConfiguration_t *pwmIOConfiguration = pwmInit(&pwm_params);
mixerUsePWMIOConfiguration(pwmIOConfiguration);
#ifdef DEBUG_PWM_CONFIGURATION
debug[2] = pwmIOConfiguration->pwmInputCount;
debug[3] = pwmIOConfiguration->ppmInputCount;
#endif
if (!feature(FEATURE_ONESHOT125))
motorControlEnable = true;
systemState |= SYSTEM_STATE_MOTORS_READY;
#ifdef INVERTER
initInverter();
#endif
#ifdef USE_SPI
spiInit(SPI1);
spiInit(SPI2);
#ifdef STM32F303xC
#ifdef ALIENFLIGHTF3
if (hardwareRevision == AFF3_REV_2) {
spiInit(SPI3);
}
#else
spiInit(SPI3);
#endif
#endif
#endif
#ifdef USE_HARDWARE_REVISION_DETECTION
updateHardwareRevision();
#endif
#if defined(NAZE)
if (hardwareRevision == NAZE32_SP) {
serialRemovePort(SERIAL_PORT_SOFTSERIAL2);
} else {
serialRemovePort(SERIAL_PORT_UART3);
}
#endif
#if defined(SPRACINGF3) && defined(SONAR) && defined(USE_SOFTSERIAL2)
if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) {
serialRemovePort(SERIAL_PORT_SOFTSERIAL2);
}
#endif
#if defined(SPRACINGF3MINI) && defined(SONAR) && defined(USE_SOFTSERIAL1)
if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) {
serialRemovePort(SERIAL_PORT_SOFTSERIAL1);
}
#endif
#ifdef USE_I2C
#if defined(NAZE)
if (hardwareRevision != NAZE32_SP) {
i2cInit(I2C_DEVICE);
} else {
if (!doesConfigurationUsePort(SERIAL_PORT_UART3)) {
i2cInit(I2C_DEVICE);
}
}
#elif defined(CC3D)
if (!doesConfigurationUsePort(SERIAL_PORT_UART3)) {
i2cInit(I2C_DEVICE);
}
#else
i2cInit(I2C_DEVICE);
#endif
#endif
#ifdef USE_ADC
drv_adc_config_t adc_params;
adc_params.channelMask = 0;
#ifdef ADC_BATTERY
adc_params.channelMask = (feature(FEATURE_VBAT) ? ADC_CHANNEL_MASK(ADC_BATTERY) : 0);
#endif
#ifdef ADC_RSSI
adc_params.channelMask |= (feature(FEATURE_RSSI_ADC) ? ADC_CHANNEL_MASK(ADC_RSSI) : 0);
#endif
#ifdef ADC_AMPERAGE
adc_params.channelMask |= (feature(FEATURE_AMPERAGE_METER) ? ADC_CHANNEL_MASK(ADC_AMPERAGE) : 0);
#endif
#ifdef ADC_POWER_12V
adc_params.channelMask |= ADC_CHANNEL_MASK(ADC_POWER_12V);
#endif
#ifdef ADC_POWER_5V
adc_params.channelMask |= ADC_CHANNEL_MASK(ADC_POWER_5V);
#endif
#ifdef ADC_POWER_3V
adc_params.channelMask |= ADC_CHANNEL_MASK(ADC_POWER_3V);
#endif
#ifdef NAZE
// optional ADC5 input on rev.5 hardware
adc_params.channelMask |= (hardwareRevision >= NAZE32_REV5) ? ADC_CHANNEL_MASK(ADC_EXTERNAL) : 0;
#endif
adcInit(&adc_params);
#endif
initBoardAlignment();
#ifdef DISPLAY
if (feature(FEATURE_DISPLAY)) {
displayInit();
}
#endif
#ifdef NAZE
if (hardwareRevision < NAZE32_REV5) {
gyroConfig()->gyro_sync = 0;
}
#endif
if (!sensorsAutodetect()) {
// if gyro was not detected due to whatever reason, we give up now.
failureMode(FAILURE_MISSING_ACC);
}
systemState |= SYSTEM_STATE_SENSORS_READY;
flashLedsAndBeep();
mspInit();
mspSerialInit();
const uint16_t pidPeriodUs = US_FROM_HZ(gyro.sampleFrequencyHz);
pidSetTargetLooptime(pidPeriodUs * gyroConfig()->pid_process_denom);
pidInitFilters(pidProfile());
#ifdef USE_SERVOS
mixerInitialiseServoFiltering(targetPidLooptime);
#endif
imuInit();
#ifdef USE_CLI
cliInit();
#endif
failsafeInit();
rxInit(modeActivationProfile()->modeActivationConditions);
#ifdef GPS
if (feature(FEATURE_GPS)) {
gpsInit();
navigationInit(pidProfile());
}
#endif
#ifdef SONAR
if (feature(FEATURE_SONAR)) {
sonarInit(sonarHardware);
}
#endif
#ifdef LED_STRIP
ledStripInit();
if (feature(FEATURE_LED_STRIP)) {
ledStripEnable();
}
#endif
#ifdef TELEMETRY
if (feature(FEATURE_TELEMETRY)) {
telemetryInit();
}
#endif
#ifdef USB_CABLE_DETECTION
usbCableDetectInit();
#endif
#ifdef TRANSPONDER
if (feature(FEATURE_TRANSPONDER)) {
transponderInit(transponderConfig()->data);
transponderEnable();
transponderStartRepeating();
systemState |= SYSTEM_STATE_TRANSPONDER_ENABLED;
}
#endif
#ifdef USE_FLASHFS
#ifdef NAZE
if (hardwareRevision == NAZE32_REV5) {
m25p16_init();
}
#elif defined(USE_FLASH_M25P16)
m25p16_init();
#endif
flashfsInit();
#endif
#ifdef USE_SDCARD
bool sdcardUseDMA = false;
sdcardInsertionDetectInit();
#ifdef SDCARD_DMA_CHANNEL_TX
#if defined(LED_STRIP) && defined(WS2811_DMA_CHANNEL)
// Ensure the SPI Tx DMA doesn't overlap with the led strip
sdcardUseDMA = !feature(FEATURE_LED_STRIP) || SDCARD_DMA_CHANNEL_TX != WS2811_DMA_CHANNEL;
#else
sdcardUseDMA = true;
#endif
#endif
sdcard_init(sdcardUseDMA);
afatfs_init();
#endif
#ifdef BLACKBOX
initBlackbox();
#endif
if (mixerConfig()->mixerMode == MIXER_GIMBAL) {
accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
}
gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES);
#ifdef BARO
baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES);
#endif
// start all timers
// TODO - not implemented yet
timerStart();
ENABLE_STATE(SMALL_ANGLE);
DISABLE_ARMING_FLAG(PREVENT_ARMING);
#ifdef SOFTSERIAL_LOOPBACK
// FIXME this is a hack, perhaps add a FUNCTION_LOOPBACK to support it properly
loopbackPort = (serialPort_t*)&(softSerialPorts[0]);
if (!loopbackPort->vTable) {
loopbackPort = openSoftSerial(0, NULL, 19200, SERIAL_NOT_INVERTED);
}
serialPrint(loopbackPort, "LOOPBACK\r\n");
#endif
if (feature(FEATURE_VBAT)) {
// Now that everything has powered up the voltage and cell count be determined.
voltageMeterInit();
batteryInit();
}
if (feature(FEATURE_AMPERAGE_METER)) {
amperageMeterInit();
}
#ifdef DISPLAY
if (feature(FEATURE_DISPLAY)) {
#ifdef USE_OLED_GPS_DEBUG_PAGE_ONLY
displayShowFixedPage(PAGE_GPS);
#else
displayResetPageCycling();
displayEnablePageCycling();
#endif
}
#endif
#ifdef CJMCU
LED2_ON;
#endif
// Latch active features AGAIN since some may be modified by init().
latchActiveFeatures();
motorControlEnable = true;
systemState |= SYSTEM_STATE_READY;
}
#ifdef SOFTSERIAL_LOOPBACK
void processLoopback(void) {
if (loopbackPort) {
uint8_t bytesWaiting;
while ((bytesWaiting = serialRxBytesWaiting(loopbackPort))) {
uint8_t b = serialRead(loopbackPort);
serialWrite(loopbackPort, b);
};
}
}
#else
#define processLoopback()
#endif
void configureScheduler(void)
{
schedulerInit();
setTaskEnabled(TASK_SYSTEM, true);
uint16_t gyroPeriodUs = US_FROM_HZ(gyro.sampleFrequencyHz);
rescheduleTask(TASK_GYRO, gyroPeriodUs);
setTaskEnabled(TASK_GYRO, true);
rescheduleTask(TASK_PID, gyroPeriodUs);
setTaskEnabled(TASK_PID, true);
if (sensors(SENSOR_ACC)) {
setTaskEnabled(TASK_ACCEL, true);
}
setTaskEnabled(TASK_ATTITUDE, sensors(SENSOR_ACC));
setTaskEnabled(TASK_SERIAL, true);
#ifdef BEEPER
setTaskEnabled(TASK_BEEPER, true);
#endif
setTaskEnabled(TASK_BATTERY, feature(FEATURE_VBAT) || feature(FEATURE_AMPERAGE_METER));
setTaskEnabled(TASK_RX, true);
#ifdef GPS
setTaskEnabled(TASK_GPS, feature(FEATURE_GPS));
#endif
#ifdef MAG
setTaskEnabled(TASK_COMPASS, sensors(SENSOR_MAG));
#if defined(MPU6500_SPI_INSTANCE) && defined(USE_MAG_AK8963)
// fixme temporary solution for AK6983 via slave I2C on MPU9250
rescheduleTask(TASK_COMPASS, 1000000 / 40);
#endif
#endif
#ifdef BARO
setTaskEnabled(TASK_BARO, sensors(SENSOR_BARO));
#endif
#ifdef SONAR
setTaskEnabled(TASK_SONAR, sensors(SENSOR_SONAR));
#endif
#if defined(BARO) || defined(SONAR)
setTaskEnabled(TASK_ALTITUDE, sensors(SENSOR_BARO) || sensors(SENSOR_SONAR));
#endif
#ifdef DISPLAY
setTaskEnabled(TASK_DISPLAY, feature(FEATURE_DISPLAY));
#endif
#ifdef TELEMETRY
setTaskEnabled(TASK_TELEMETRY, feature(FEATURE_TELEMETRY));
#endif
#ifdef LED_STRIP
setTaskEnabled(TASK_LEDSTRIP, feature(FEATURE_LED_STRIP));
#endif
#ifdef TRANSPONDER
setTaskEnabled(TASK_TRANSPONDER, feature(FEATURE_TRANSPONDER));
#endif
}
CMS_Menu cmsx_menuRcPreview = {
.GUARD_text = "XRCPREV",
.GUARD_type = OME_MENU,
.onEnter = NULL,
.onExit = cmsx_menuRcConfirmBack,
.onGlobalExit = NULL,
.entries = cmsx_menuRcEntries
};
static OSD_Entry menuMiscEntries[]=
{
{ "-- MISC --", OME_Label, NULL, NULL, 0 },
{ "MIN THR", OME_UINT16, NULL, &(OSD_UINT16_t){ &motorConfig()->minthrottle, 1000, 2000, 1 }, 0 },
{ "VBAT SCALE", OME_UINT8, NULL, &(OSD_UINT8_t) { &batteryConfig()->vbatscale, 1, 250, 1 }, 0 },
{ "VBAT CLMAX", OME_UINT8, NULL, &(OSD_UINT8_t) { &batteryConfig()->vbatmaxcellvoltage, 10, 50, 1 }, 0 },
{ "RC PREV", OME_Submenu, cmsMenuChange, &cmsx_menuRcPreview, 0},
{ "BACK", OME_Back, NULL, NULL, 0},
{ NULL, OME_END, NULL, NULL, 0}
};
CMS_Menu cmsx_menuMisc = {
.GUARD_text = "XMISC",
.GUARD_type = OME_MENU,
.onEnter = NULL,
.onExit = NULL,
.onGlobalExit = NULL,
.entries = menuMiscEntries
void FlemonsSpineModelGoal::setup(tgWorld& world)
{
// This is basically a manual setup of a model.
// There are things that do this for us
/// @todo: reference the things that do this for us
// Rod and Muscle configuration
// Note: This needs to be high enough or things fly apart...
const double density = 4.2/300.0;
const double radius = 0.5;
const double friction = 0.5;
const double rollFriction = 0.0;
const double restitution = 0.0;
const tgRod::Config rodConfig(radius, density, friction, rollFriction, restitution);
const double elasticity = 1000.0;
const double damping = 10.0;
const double pretension = 0.0;
const bool history = true;
const double maxTens = 7000.0;
const double maxSpeed = 12.0;
const double mRad = 1.0;
const double motorFriction = 10.0;
const double motorInertia = 1.0;
const bool backDrivable = false;
tgKinematicActuator::Config motorConfig(elasticity, damping, pretension,
mRad, motorFriction, motorInertia, backDrivable,
history, maxTens, maxSpeed);
// Calculations for the flemons spine model
double v_size = 10.0;
// Create the tetrahedra
tgStructure tetra;
tetra.addNode(0.0, 0.0, 0.0); // center
tetra.addNode( v_size, v_size, v_size); // front
tetra.addNode( v_size, -v_size, -v_size); // right
tetra.addNode(-v_size, v_size, -v_size); // back
tetra.addNode(-v_size, -v_size, v_size); // left
tetra.addPair(0, 1, "front rod");
tetra.addPair(0, 2, "right rod");
tetra.addPair(0, 3, "back rod");
tetra.addPair(0, 4, "left rod");
// Move the first one so we can create a longer snake.
// Or you could move the snake at the end, up to you.
tetra.move(btVector3(0.0,15.0,50.0));
// Create our snake segments
tgStructure snake;
/// @todo: there seems to be an issue with Muscle2P connections if the front
/// of a tetra is inside the next one.
btVector3 offset(0.0, 0.0, -v_size * 1.15);
for (std::size_t i = 0; i < m_segments; i++)
{
/// @todo: the snake is a temporary variable --
/// will its destructor be called?
/// If not, where do we delete its children?
tgStructure* const p = new tgStructure(tetra);
p->addTags(tgString("segment num", i + 1));
p->move((i + 1.0) * offset);
snake.addChild(p); // Add a child to the snake
}
//conditionally compile for debugging
#if (1)
// Add muscles that connect the segments
// Tag the muscles with their segment numbers so CPGs can find
// them.
std::vector<tgStructure*> children = snake.getChildren();
for (std::size_t i = 1; i < children.size(); i++)
{
tgNodes n0 = children[i - 1]->getNodes();
tgNodes n1 = children[i]->getNodes();
snake.addPair(n0[1], n1[1],
tgString("outer front muscle seg", i));
snake.addPair(n0[2], n1[2],
tgString("outer right muscle seg", i));
snake.addPair(n0[3], n1[3],
tgString("outer back muscle seg", i));
snake.addPair(n0[4], n1[4],
tgString("outer top muscle seg", i));
snake.addPair(n0[2], n1[1],
tgString("inner front muscle seg", i));
snake.addPair(n0[2], n1[4],
tgString("inner right muscle seg", i));
snake.addPair(n0[3], n1[1],
tgString("inner left muscle seg", i));
snake.addPair(n0[3], n1[4],
tgString("inner back muscle seg", i));
}
#endif
btVector3 fixedPoint(0.0, 0.0, 0.0);
btVector3 axis(0.0, 1.0, 0.0);
snake.addRotation(fixedPoint, axis, m_startAngle);
// Create the build spec that uses tags to turn the structure into a real model
tgBuildSpec spec;
spec.addBuilder("rod", new tgRodInfo(rodConfig));
#if (1)
spec.addBuilder("muscle", new tgKinematicContactCableInfo(motorConfig));
#else
spec.addBuilder("muscle", new tgBasicContactCableInfo(motorConfig));
#endif
// Create your structureInfo
tgStructureInfo structureInfo(snake, spec);
// Use the structureInfo to build ourselves
structureInfo.buildInto(*this, world);
// Setup vectors for control
m_allMuscles = tgCast::filter<tgModel, tgSpringCableActuator> (getDescendants());
m_allSegments = this->find<tgModel> ("segment");
//addMarkers(snake, *this, m_segments);
#if (0)
// Debug printing
std::cout << "StructureInfo:" << std::endl;
std::cout << structureInfo << std::endl;
std::cout << "Model: " << std::endl;
std::cout << *this << std::endl;
std::cout << "Spine Length: " << getSpineLength() << std::endl;
#endif
std::vector<tgBaseRigid*> myRigids = this->getAllRigids();
#if (1)
for (int i =0; i < myRigids.size(); i++)
{
std::cout << myRigids[i]->mass() << " " <<myRigids[i]->getPRigidBody() << std::endl;
}
#endif
children.clear();
// Actually setup the children, notify controller
BaseSpineModelGoal::setup(world);
}
void mixTable(void)
{
uint32_t i;
bool isFailsafeActive = failsafeIsActive();
if (motorCount >= 4 && mixerConfig()->yaw_jump_prevention_limit < YAW_JUMP_PREVENTION_LIMIT_HIGH) {
// prevent "yaw jump" during yaw correction
axisPID[FD_YAW] = constrain(axisPID[FD_YAW], -mixerConfig()->yaw_jump_prevention_limit - ABS(rcCommand[YAW]), mixerConfig()->yaw_jump_prevention_limit + ABS(rcCommand[YAW]));
}
if (rcModeIsActive(BOXAIRMODE)) {
// Initial mixer concept by bdoiron74 reused and optimized for Air Mode
int16_t rollPitchYawMix[MAX_SUPPORTED_MOTORS];
int16_t rollPitchYawMixMax = 0; // assumption: symetrical about zero.
int16_t rollPitchYawMixMin = 0;
// Find roll/pitch/yaw desired output
for (i = 0; i < motorCount; i++) {
rollPitchYawMix[i] =
axisPID[FD_PITCH] * currentMixer[i].pitch +
axisPID[FD_ROLL] * currentMixer[i].roll +
-mixerConfig()->yaw_motor_direction * axisPID[FD_YAW] * currentMixer[i].yaw;
if (rollPitchYawMix[i] > rollPitchYawMixMax) rollPitchYawMixMax = rollPitchYawMix[i];
if (rollPitchYawMix[i] < rollPitchYawMixMin) rollPitchYawMixMin = rollPitchYawMix[i];
}
// Scale roll/pitch/yaw uniformly to fit within throttle range
int16_t rollPitchYawMixRange = rollPitchYawMixMax - rollPitchYawMixMin;
int16_t throttleRange, throttle;
int16_t throttleMin, throttleMax;
#ifndef SKIP_3D_FLIGHT
static int16_t throttlePrevious = 0; // Store the last throttle direction for deadband transitions in 3D.
// Find min and max throttle based on condition. Use rcData for 3D to prevent loss of power due to min_check
if (feature(FEATURE_3D)) {
if (!ARMING_FLAG(ARMED)) throttlePrevious = rxConfig()->midrc; // When disarmed set to mid_rc. It always results in positive direction after arming.
if ((rcData[THROTTLE] <= (rxConfig()->midrc - rcControlsConfig()->deadband3d_throttle))) { // Out of band handling
throttleMax = motor3DConfig()->deadband3d_low;
throttleMin = motorConfig()->minthrottle;
throttlePrevious = throttle = rcData[THROTTLE];
} else if (rcData[THROTTLE] >= (rxConfig()->midrc + rcControlsConfig()->deadband3d_throttle)) { // Positive handling
throttleMax = motorConfig()->maxthrottle;
throttleMin = motor3DConfig()->deadband3d_high;
throttlePrevious = throttle = rcData[THROTTLE];
} else if ((throttlePrevious <= (rxConfig()->midrc - rcControlsConfig()->deadband3d_throttle))) { // Deadband handling from negative to positive
throttle = throttleMax = motor3DConfig()->deadband3d_low;
throttleMin = motorConfig()->minthrottle;
} else { // Deadband handling from positive to negative
throttleMax = motorConfig()->maxthrottle;
throttle = throttleMin = motor3DConfig()->deadband3d_high;
}
} else {
#endif
throttle = rcCommand[THROTTLE];
throttleMin = motorConfig()->minthrottle;
throttleMax = motorConfig()->maxthrottle;
#ifndef SKIP_3D_FLIGHT
}
#endif
throttleRange = throttleMax - throttleMin;
if (rollPitchYawMixRange > throttleRange) {
motorLimitReached = true;
float mixReduction = (float) throttleRange / rollPitchYawMixRange;
for (i = 0; i < motorCount; i++) {
rollPitchYawMix[i] = lrintf((float) rollPitchYawMix[i] * mixReduction);
}
// Get the maximum correction by setting throttle offset to center.
throttleMin = throttleMax = throttleMin + (throttleRange / 2);
} else {
motorLimitReached = false;
throttleMin = throttleMin + (rollPitchYawMixRange / 2);
throttleMax = throttleMax - (rollPitchYawMixRange / 2);
}
// Now add in the desired throttle, but keep in a range that doesn't clip adjusted
// roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips.
for (i = 0; i < motorCount; i++) {
motor[i] = rollPitchYawMix[i] + constrain(throttle * currentMixer[i].throttle, throttleMin, throttleMax);
motor[i] += triGetMotorCorrection(i);
if (isFailsafeActive) {
motor[i] = mixConstrainMotorForFailsafeCondition(i);
} else
#ifndef SKIP_3D_FLIGHT
if (feature(FEATURE_3D)) {
if (throttlePrevious <= (rxConfig()->midrc - rcControlsConfig()->deadband3d_throttle)) {
motor[i] = constrain(motor[i], motorConfig()->minthrottle, motor3DConfig()->deadband3d_low);
} else {
motor[i] = constrain(motor[i], motor3DConfig()->deadband3d_high, motorConfig()->maxthrottle);
}
} else
#endif
{
motor[i] = constrain(motor[i], motorConfig()->minthrottle, motorConfig()->maxthrottle);
}
}
} else {
// motors for non-servo mixes
for (i = 0; i < motorCount; i++) {
motor[i] =
rcCommand[THROTTLE] * currentMixer[i].throttle +
axisPID[FD_PITCH] * currentMixer[i].pitch +
axisPID[FD_ROLL] * currentMixer[i].roll +
-mixerConfig()->yaw_motor_direction * axisPID[FD_YAW] * currentMixer[i].yaw + triGetMotorCorrection(i);
}
// Find the maximum motor output.
int16_t maxMotor = motor[0];
for (i = 1; i < motorCount; i++) {
// If one motor is above the maxthrottle threshold, we reduce the value
// of all motors by the amount of overshoot. That way, only one motor
// is at max and the relative power of each motor is preserved.
if (motor[i] > maxMotor) {
maxMotor = motor[i];
}
}
int16_t maxThrottleDifference = 0;
if (maxMotor > motorConfig()->maxthrottle) {
maxThrottleDifference = maxMotor - motorConfig()->maxthrottle;
}
for (i = 0; i < motorCount; i++) {
// this is a way to still have good gyro corrections if at least one motor reaches its max.
motor[i] -= maxThrottleDifference;
#ifndef SKIP_3D_FLIGHT
if (feature(FEATURE_3D)) {
if (mixerConfig()->pid_at_min_throttle
|| rcData[THROTTLE] <= rxConfig()->midrc - rcControlsConfig()->deadband3d_throttle
|| rcData[THROTTLE] >= rxConfig()->midrc + rcControlsConfig()->deadband3d_throttle) {
if (rcData[THROTTLE] > rxConfig()->midrc) {
motor[i] = constrain(motor[i], motor3DConfig()->deadband3d_high, motorConfig()->maxthrottle);
} else {
motor[i] = constrain(motor[i], motorConfig()->mincommand, motor3DConfig()->deadband3d_low);
}
} else {
if (rcData[THROTTLE] > rxConfig()->midrc) {
motor[i] = motor3DConfig()->deadband3d_high;
} else {
motor[i] = motor3DConfig()->deadband3d_low;
}
}
} else
#endif
{
if (isFailsafeActive) {
motor[i] = mixConstrainMotorForFailsafeCondition(i);
} else {
// If we're at minimum throttle and FEATURE_MOTOR_STOP enabled,
// do not spin the motors.
motor[i] = constrain(motor[i], motorConfig()->minthrottle, motorConfig()->maxthrottle);
if ((rcData[THROTTLE]) < rxConfig()->mincheck) {
if (feature(FEATURE_MOTOR_STOP)) {
motor[i] = motorConfig()->mincommand;
} else if (mixerConfig()->pid_at_min_throttle == 0) {
motor[i] = motorConfig()->minthrottle;
}
}
}
}
}
}
/* Disarmed for all mixers */
if (!ARMING_FLAG(ARMED)) {
for (i = 0; i < motorCount; i++) {
motor[i] = motor_disarmed[i];
}
}
// motor outputs are used as sources for servo mixing, so motors must be calculated before servos.
#if !defined(USE_QUAD_MIXER_ONLY) && defined(USE_SERVOS)
servoMixTable();
#endif
}
uint16_t mixConstrainMotorForFailsafeCondition(uint8_t motorIndex)
{
return constrain(motor[motorIndex], motorConfig()->mincommand, motorConfig()->maxthrottle);
}
uint16_t getCurrentMinthrottle(void)
{
return motorConfig()->minthrottle;
}
void BigPuppy::setup(tgWorld& world)
{
//Rod and Muscle configuration. Todo: make these into structs in a namespace block!
const double density = 4.2/300.0; //Note: this needs to be high enough or things fly apart...
const double radius = 0.5;
const double rod_space = 10.0;
const double friction = 0.5;
const double rollFriction = 0.0;
const double restitution = 0.0;
const tgRod::Config rodConfig(radius, density, friction, rollFriction, restitution);
const double radius2 = 0.15;
const double density2 = 1; // Note: This needs to be high enough or things fly apart...
const tgRod::Config rodConfig2(radius2, density2);
const double stiffness = 1000.0;
const double damping = .01*stiffness;
const double pretension = 0.0;
const bool history = false;
const double maxTens = 7000.0;
const double maxSpeed = 12.0;
const double passivePretension = 700; // 5 N
#ifdef USE_KINEMATIC
const double mRad = 1.0;
const double motorFriction = 10.0;
const double motorInertia = 1.0;
const bool backDrivable = false;
tgKinematicActuator::Config motorConfig(2000, 20, passivePretension,
mRad, motorFriction, motorInertia, backDrivable,
history, maxTens, maxSpeed);
#else
const tgSpringCableActuator::Config stringConfig(stiffness, damping, pretension, false, 7000, 24);
tgSpringCableActuator::Config muscleConfig(2000, 20, passivePretension);
#endif
// Calculations for the flemons spine model
double v_size = 10.0;
//Todo: make separate functions for node, rod, and muscle placement! Do for each type of segment.
//Foot:
tgStructure foot;
//Foot nodes. Todo: make into separate function
foot.addNode(8,0,8);//0
foot.addNode(8,0,-8);//1
foot.addNode(-8,0,-8);//2
foot.addNode(-8,0,8);//3
foot.addNode(4,rod_space/2,0);//4
foot.addNode(0,rod_space/2,-4);//5
foot.addNode(-4,rod_space/2,0);//6
foot.addNode(0,rod_space/2,4);//7
//Foot rods. Todo: make into separate function
foot.addPair(0,6,"rod");
foot.addPair(1,7,"rod");
foot.addPair(2,4,"rod");
foot.addPair(3,5,"rod");
//Create basic unit for right leg. Todo: make just one basic unit for right and left legs, since they're now the same.
tgStructure rightLeg;
//Right Leg nodes:
rightLeg.addNode(0,0,0); //0: Bottom Center of lower leg segment
rightLeg.addNode(0,10,0); //1: Center of lower leg segment
rightLeg.addNode(10,10,0); //2: Right of lower leg segment
rightLeg.addNode(-10,10,0); //3: Left of lower leg segment
rightLeg.addNode(0,20,0); //4: Top of lower leg segment
rightLeg.addNode(0,-4,0); //5: was z=3; was y=-3
//rightLeg.addNode(0,-3,-3); //6
//rightLeg.addNode(3,-3,0); //7
//rightLeg.addNode(-3,-3,0); //8
//Add rods for right leg:
rightLeg.addPair(0,1,"rod");
rightLeg.addPair(1,2,"rod");
rightLeg.addPair(1,3,"rod");
rightLeg.addPair(1,4,"rod");
rightLeg.addPair(0,5,"rod");
//rightLeg.addPair(0,6,"rod");
//rightLeg.addPair(0,7,"rod");
//rightLeg.addPair(0,8,"rod");
//Create basic unit for left leg
tgStructure leftLeg;
//Left Leg nodes:
leftLeg.addNode(0,0,0); //0: Bottom Center of lower leg segment
leftLeg.addNode(0,10,0); //1: Center of lower leg segment
leftLeg.addNode(10,10,0); //2: Right of lower leg segment
leftLeg.addNode(-10,10,0); //3: Left of lower leg segment
leftLeg.addNode(0,20,0); //4: Top of lower leg segment
leftLeg.addNode(0,-4,0); //5: was z=3; was y=-3
//leftLeg.addNode(0,-3,-3); //6
//leftLeg.addNode(3,-3,0); //7
//leftLeg.addNode(-3,-3,0); //8
//Add rods for left leg:
leftLeg.addPair(0,1,"rod");
leftLeg.addPair(1,2,"rod");
leftLeg.addPair(1,3,"rod");
leftLeg.addPair(1,4,"rod");
leftLeg.addPair(0,5,"rod");
//leftLeg.addPair(0,6,"rod");
//leftLeg.addPair(0,7,"rod");
//leftLeg.addPair(0,8,"rod");
//Create the basic unit of the spine
tgStructure tetra;
//Add the nodes
tetra.addNode(0,0,0); //Node 0
tetra.addNode(v_size, 0, v_size); //Node 1
tetra.addNode(v_size, 0, -v_size); //Node 2
tetra.addNode(-v_size, 0, -v_size); //Node 3
tetra.addNode(-v_size, 0, v_size); //Node 4
tetra.addPair(0,1,"rod");
tetra.addPair(0,2,"rod");
tetra.addPair(0,3,"rod");
tetra.addPair(0,4,"rod");
//Create the basic unit for the hips/shoulders:
tgStructure lHip;
lHip.addNode(0,0,0); //Node 0
lHip.addNode(0, v_size, v_size); //Node 1
lHip.addNode(0, -v_size, -v_size); //Node 2
lHip.addNode(0, -v_size, v_size); //Node 3
lHip.addPair(0,1,"rod");
lHip.addPair(0,2,"rod");
lHip.addPair(0,3,"rod");
tgStructure rHip;
rHip.addNode(0,0,0); //Node 0
rHip.addNode(0, v_size, -v_size); //Node 1
rHip.addNode(0, -v_size, -v_size); //Node 2
rHip.addNode(0, -v_size, v_size); //Node 3
rHip.addPair(0,1,"rod");
rHip.addPair(0,2,"rod");
rHip.addPair(0,3,"rod");
//Build the spine
tgStructure spine;
const double offsetDist = v_size + 1; //So rod ends don't touch, may need to adjust
const double offsetDist2 = v_size*5 + 5 + 3.3;
const double offsetDist3 = v_size*6;
const double yOffset_leg = -21.0;
const double yOffset_foot = -26.0;
std::size_t m_segments = 6;
std::size_t m_hips = 4;
std::size_t m_legs = 4;
std::size_t m_feet = 4;
//Vertebrae
btVector3 offset(offsetDist,0.0,0);
//Hips
btVector3 offset1(offsetDist*2,0.0,offsetDist);
btVector3 offset2(offsetDist2,0.0,offsetDist);
btVector3 offset3(offsetDist*2,0.0,-offsetDist);
btVector3 offset4(offsetDist2,0.0,-offsetDist);
//Lower legs
btVector3 offset5(offsetDist3,yOffset_leg,offsetDist);
btVector3 offset6(offsetDist3,yOffset_leg,-offsetDist);
btVector3 offset7(v_size*2,yOffset_leg,offsetDist);
btVector3 offset8(v_size*2,yOffset_leg,-offsetDist);
//Feet
btVector3 offset9(offsetDist3+1,yOffset_foot,offsetDist);
btVector3 offset10(offsetDist3+1,yOffset_foot,-offsetDist);
btVector3 offset11(v_size*2+1,yOffset_foot,offsetDist);
btVector3 offset12(v_size*2+1,yOffset_foot,-offsetDist);
for(std::size_t i = 0; i < m_segments; i++) { //Connect segments for spine
tgStructure* t = new tgStructure (tetra);
t->addTags(tgString("segment num", i + 1));
t->move((i + 1)*offset);
if (i % 2 == 1) {
t->addRotation(btVector3((i + 1) * offsetDist, 0.0, 0.0), btVector3(1, 0, 0), 0.0);
}
else {
t->addRotation(btVector3((i + 1) * offsetDist, 0.0, 0.0), btVector3(1, 0, 0), M_PI/2.0);
}
spine.addChild(t); //Add a segment to the spine
}
for(std::size_t i = m_segments; i < (m_segments + 2); i++) {//deal with right hip and shoulder first
tgStructure* t = new tgStructure (rHip);
t->addTags(tgString("segment num", i + 1));
if(i % 2 == 0) {
t->move(offset1);
t->addRotation(btVector3(offsetDist*2, 0.0, offsetDist), btVector3(1, 0, 0), 0.0);
}
else {
t->move(offset2);
t->addRotation(btVector3(offsetDist2, 0.0, offsetDist), btVector3(0, 0, 1), M_PI*1/8);
}
spine.addChild(t); //Add a segment to the spine
}
for(std::size_t i = (m_segments + 2); i < (m_segments + m_hips); i++) {//deal with left hip and shoulder now
tgStructure* t = new tgStructure (lHip);
t->addTags(tgString("segment num", i + 1));
if(i % 2 == 0) {
t->move(offset3);
t->addRotation(btVector3(offsetDist*2, 0.0, -offsetDist), btVector3(1, 0, 0), 0.0);
}
else {
t->move(offset4);
t->addRotation(btVector3(offsetDist2, 0.0, -offsetDist), btVector3(0, 0, 1), M_PI*1/8);
}
spine.addChild(t); //Add a segment to the spine
}
for(std::size_t i = (m_segments + m_hips); i < (m_segments + m_hips + 2); i++) {//right front and back legs
tgStructure* t = new tgStructure (rightLeg);
t->addTags(tgString("segment num", i + 1));
if(i % 2 == 0) {
t->move(offset7);
t->addRotation(btVector3(v_size*2, yOffset_leg, offsetDist), btVector3(0, 1, 0), M_PI);
}
else {
t->move(offset5);
t->addRotation(btVector3(offsetDist3, yOffset_leg, offsetDist), btVector3(0, 1, 0), M_PI);
}
spine.addChild(t); //Add a segment to the spine
}
for(std::size_t i = (m_segments + m_hips + 2); i < (m_segments + m_hips + m_legs); i++) {//left front and back legs
tgStructure* t = new tgStructure (leftLeg);
t->addTags(tgString("segment num", i + 1));
if(i % 2 == 0) {
t->move(offset8);
t->addRotation(btVector3(v_size*2, yOffset_leg, -offsetDist), btVector3(0, 1, 0), M_PI);
}
else {
t->move(offset6);
t->addRotation(btVector3(offsetDist3, yOffset_leg, -offsetDist), btVector3(0, 1, 0), M_PI);
}
spine.addChild(t); //Add a segment to the spine
}
for(std::size_t i = (m_segments + m_hips + m_legs); i < (m_segments + m_hips + m_legs + 2); i++) {//right front and back feet
tgStructure* t = new tgStructure (foot);
t->addTags(tgString("segment num", i + 1));
if(i % 2 == 0) {
t->move(offset11);
t->addRotation(btVector3(v_size*2+1, yOffset_foot, offsetDist), btVector3(0, 1, 0), 0.0);
}
else {
t->move(offset9);
t->addRotation(btVector3(offsetDist3+1, yOffset_foot, offsetDist), btVector3(0, 1, 0), 0.0);
}
spine.addChild(t); //Add a segment to the spine
}
for(std::size_t i = (m_segments + m_hips + m_legs + 2); i < (m_segments + m_hips + m_legs + m_feet); i++) {//left front and back feet
tgStructure* t = new tgStructure (foot);
t->addTags(tgString("segment num", i + 1));
if(i % 2 == 0) {
t->move(offset12);
t->addRotation(btVector3(v_size*2+1, yOffset_foot, -offsetDist), btVector3(0, 1, 0), 0.0);
}
else {
t->move(offset10);
t->addRotation(btVector3(offsetDist3+1, yOffset_foot, -offsetDist), btVector3(0, 1, 0), 0.0);
}
spine.addChild(t); //Add a segment to the spine
}
#ifdef SMALL_HILLS
spine.move(btVector3(0.0,-yOffset_foot+5,0.0));
#endif
#ifdef LARGE_HILLS
spine.move(btVector3(0.0,-yOffset_foot+12,0.0));
#endif
#ifdef FLAT_GROUND
spine.move(btVector3(0.0,-yOffset_foot,0.0));
#endif
#ifdef BLOCKY_GROUND
spine.move(btVector3(0.0,10.0,0.0));
#endif
#ifdef STAIRS
spine.move(btVector3(0.0,0.0,0.0));
#endif
std::vector<tgStructure*> children = spine.getChildren();
for(std::size_t i = 2; i < (children.size() - (m_hips + m_legs + m_feet)); i++) {
tgNodes n0 = children[i-2]->getNodes();
tgNodes n1 = children[i-1]->getNodes();
tgNodes n2 = children[i]->getNodes();
if(i==2) {
//Extra muscles, to keep front vertebra from swinging.
spine.addPair(n0[3], n1[3], tgString("spine front upper right muscle seg", i-2) + tgString(" seg", i-1));
spine.addPair(n0[3], n1[4], tgString("spine front upper left muscle seg", i-2) + tgString(" seg", i-1));
}
//Add muscles to the spine
if(i < 3) {
if(i % 2 == 0) { //front
spine.addPair(n0[1], n1[3], tgString("spine front lower right muscle seg", i-2) + tgString(" seg", i-1));
spine.addPair(n0[1], n1[4], tgString("spine front lower left muscle seg", i-2) + tgString(" seg", i-1));
spine.addPair(n0[2], n1[3], tgString("spine front upper right muscle seg", i-2) + tgString(" seg", i-1));
spine.addPair(n0[2], n1[4], tgString("spine front upper left muscle seg", i-2) + tgString(" seg", i-1));
}
else { //rear
spine.addPair(n0[1], n1[3], tgString("spine rear upper left muscle seg", i-2) + tgString(" seg", i-1));
spine.addPair(n0[1], n1[4], tgString("spine rear lower left muscle seg", i-2) + tgString(" seg", i-1));
spine.addPair(n0[2], n1[3], tgString("spine rear upper right muscle seg", i-2) + tgString(" seg", i-1));
spine.addPair(n0[2], n1[4], tgString("spine rear lower right muscle seg", i-2) + tgString(" seg", i-1));
}
}
if(i < 6) {
if(i % 2 == 0) {
spine.addPair(n0[1], n2[4], tgString("spine bottom muscle seg", i-2) + tgString(" seg", i-1));
spine.addPair(n0[2], n2[3], tgString("spine top muscle seg", i-2) + tgString(" seg", i-1));
}
else {
spine.addPair(n0[1], n2[4], tgString("spine lateral left muscle seg", i-2) + tgString(" seg", i-1));
spine.addPair(n0[2], n2[3], tgString("spine lateral right muscle seg", i-2) + tgString(" seg", i-1));
}
}
if(i > 0 && i < 5) {
if(i % 2 == 0) { //rear
spine.addPair(n1[1], n2[3], tgString("spine rear upper left muscle seg", i-1) + tgString(" seg", i));
spine.addPair(n1[1], n2[4], tgString("spine rear lower left muscle seg", i-1) + tgString(" seg", i));
spine.addPair(n1[2], n2[3], tgString("spine rear upper right muscle seg", i-1) + tgString(" seg", i));
spine.addPair(n1[2], n2[4], tgString("spine rear lower right muscle seg", i-1) + tgString(" seg", i));
}
else { //front
spine.addPair(n1[1], n2[3], tgString("spine front lower right muscle seg", i-1) + tgString(" seg", i));
spine.addPair(n1[1], n2[4], tgString("spine front lower left muscle seg", i-1) + tgString(" seg", i));
spine.addPair(n1[2], n2[3], tgString("spine front upper right muscle seg", i-1) + tgString(" seg", i));
spine.addPair(n1[2], n2[4], tgString("spine front upper left muscle seg", i-1) + tgString(" seg", i));
}
}
if(i == 5) {
//rear
spine.addPair(n1[1], n2[1], tgString("spine rear lower left muscle seg", i-1) + tgString(" seg", i));
spine.addPair(n1[1], n2[2], tgString("spine rear lower right muscle seg", i-1) + tgString(" seg", i));
spine.addPair(n1[2], n2[1], tgString("spine rear upper left muscle seg", i-1) + tgString(" seg", i));
spine.addPair(n1[2], n2[2], tgString("spine rear upper right muscle seg", i-1) + tgString(" seg", i));
//front
spine.addPair(n1[1], n2[3], tgString("spine front lower right muscle seg", i-1) + tgString(" seg", i));
spine.addPair(n1[1], n2[4], tgString("spine front lower left muscle seg", i-1) + tgString(" seg", i));
spine.addPair(n1[2], n2[3], tgString("spine front upper right muscle seg", i-1) + tgString(" seg", i));
spine.addPair(n1[2], n2[4], tgString("spine front upper left muscle seg", i-1) + tgString(" seg", i));
}
}
//Now add muscles to hips....
tgNodes n0 = children[0]->getNodes();
tgNodes n1 = children[1]->getNodes();
tgNodes n2 = children[2]->getNodes();
tgNodes n3 = children[3]->getNodes();
tgNodes n4 = children[4]->getNodes();
tgNodes n5 = children[5]->getNodes();
tgNodes n6 = children[6]->getNodes();
tgNodes n7 = children[7]->getNodes();
tgNodes n8 = children[8]->getNodes();
tgNodes n9 = children[9]->getNodes();
tgNodes n10 = children[10]->getNodes();
tgNodes n11 = children[11]->getNodes();
tgNodes n12 = children[12]->getNodes();
tgNodes n13 = children[13]->getNodes();
//Left shoulder muscles
spine.addPair(n6[1], n1[1], tgString("left shoulder rear upper muscle seg", 6) + tgString(" seg", 1));
spine.addPair(n6[1], n1[4], tgString("left shoulder front upper muscle seg", 6) + tgString(" seg", 1));
spine.addPair(n6[1], n0[2], tgString("left shoulder front top muscle seg", 6) + tgString(" seg", 0));
spine.addPair(n6[1], n2[3], tgString("left shoulder rear top muscle seg", 6) + tgString(" seg", 2));
spine.addPair(n6[2], n1[1], tgString("left shoulder rear lower muscle seg", 6) + tgString(" seg", 1));
spine.addPair(n6[2], n1[4], tgString("left shoulder front lower muscle seg", 6) + tgString(" seg", 1));
spine.addPair(n6[2], n0[1], tgString("left shoulder front bottom muscle seg", 6) + tgString(" seg", 0));
spine.addPair(n6[2], n2[4], tgString("left shoulder rear bottom muscle seg", 6) + tgString(" seg", 2));
//Extra muscles, to move left shoulder forward and back:
spine.addPair(n6[0], n1[1], tgString("left shoulder rear mid muscle seg", 6) + tgString(" seg", 1));
spine.addPair(n6[0], n1[4], tgString("left shoulder front mid muscle seg", 6) + tgString(" seg", 1));
//Left hip muscles
spine.addPair(n7[1], n5[1], tgString("left hip rear upper muscle seg", 7) + tgString(" seg", 5));
spine.addPair(n7[1], n5[4], tgString("left hip front upper muscle seg", 7) + tgString(" seg", 5));
spine.addPair(n7[1], n4[2], tgString("left hip rear top muscle seg", 7) + tgString(" seg", 4));
spine.addPair(n7[1], n4[3], tgString("left hip front top muscle seg", 7) + tgString(" seg", 4));
spine.addPair(n7[2], n5[1], tgString("left hip rear lower muscle seg", 7) + tgString(" seg", 5));
spine.addPair(n7[2], n5[4], tgString("left hip front lower muscle seg", 7) + tgString(" seg", 5));
spine.addPair(n7[2], n4[1], tgString("left hip bottom muscle seg", 7) + tgString(" seg", 4));
//Extra muscles, to move left hip forward and back:
spine.addPair(n7[0], n3[1], tgString("left hip rear mid muscle seg", 7) + tgString(" seg", 3)); //could also be n3[3]
spine.addPair(n7[0], n5[4], tgString("left hip front mid muscle seg", 7) + tgString(" seg", 5));
//Inter-hip connector muscle
spine.addPair(n7[2], n9[3], tgString("inter-hip bottom muscle seg", 7) + tgString(" seg", 9)); //inter-hip bottom muscle
//Right shoulder muscles
spine.addPair(n8[1], n1[2], tgString("right shoulder rear upper muscle seg", 8) + tgString(" seg", 1));
spine.addPair(n8[1], n1[3], tgString("right shoulder front upper muscle seg", 8) + tgString(" seg", 1));
spine.addPair(n8[1], n0[2], tgString("right shoulder front top muscle seg", 8) + tgString(" seg", 0));
spine.addPair(n8[1], n2[3], tgString("right shoulder rear top muscle seg", 8) + tgString(" seg", 2));
spine.addPair(n8[3], n1[2], tgString("right shoulder rear lower muscle seg", 8) + tgString(" seg", 1));
spine.addPair(n8[3], n1[3], tgString("right shoulder front lower muscle seg", 8) + tgString(" seg", 1));
spine.addPair(n8[3], n0[1], tgString("right shoulder front bottom muscle seg", 8) + tgString(" seg", 0));
spine.addPair(n8[3], n2[4], tgString("right shoulder rear bottom muscle seg", 8) + tgString(" seg", 2));
//Extra muscles, to move right shoulder forward and back:
spine.addPair(n8[0], n1[2], tgString("right shoulder rear mid muscle seg", 8) + tgString(" seg", 1));
spine.addPair(n8[0], n1[3], tgString("right shoulder front mid muscle seg", 8) + tgString(" seg", 1));
//Right hip muscles
spine.addPair(n9[1], n5[2], tgString("right hip rear upper muscle seg", 9) + tgString(" seg", 5));
spine.addPair(n9[1], n5[3], tgString("right hip front upper muscle seg", 9) + tgString(" seg", 5));
spine.addPair(n9[1], n4[2], tgString("right hip rear top muscle seg", 9) + tgString(" seg", 4));
spine.addPair(n9[1], n4[3], tgString("right hip front top muscle seg", 9) + tgString(" seg", 4));
spine.addPair(n9[3], n5[2], tgString("right hip rear lower muscle seg", 9) + tgString(" seg", 5));
spine.addPair(n9[3], n5[3], tgString("right hip front lower muscle seg", 9) + tgString(" seg", 5));
spine.addPair(n9[3], n4[1], tgString("right hip bottom muscle seg", 9) + tgString(" seg", 4));
//Extra muscles, to move right hip forward and back:
spine.addPair(n9[0], n3[2], tgString("right hip rear mid muscle seg", 9) + tgString(" seg", 3)); //could also be n3[3]
spine.addPair(n9[0], n5[3], tgString("right hip front mid muscle seg", 9) + tgString(" seg", 5));
//Leg/hip connections:
//Right front leg/shoulder
spine.addPair(n10[4], n6[2], tgString("right outer bicep muscle seg", 10) + tgString(" seg", 6));
spine.addPair(n10[4], n6[3], tgString("right inner bicep muscle seg", 10) + tgString(" seg", 6));
spine.addPair(n10[4], n1[4], tgString("right front abdomen connection muscle seg", 10) + tgString(" seg", 1));
spine.addPair(n10[3], n6[2], tgString("right outer tricep muscle seg", 10) + tgString(" seg", 6));
spine.addPair(n10[3], n6[3], tgString("right inner tricep muscle seg", 10) + tgString(" seg", 6));
spine.addPair(n10[2], n6[2], tgString("right outer front tricep muscle seg", 10) + tgString(" seg", 6));
spine.addPair(n10[2], n6[3], tgString("right inner front tricep muscle seg", 10) + tgString(" seg", 6));
//Adding muscle to pull up on right front leg:
spine.addPair(n10[4], n6[1], tgString("right mid bicep muscle seg", 10) + tgString(" seg", 6));
//Left front leg/shoulder
spine.addPair(n12[4], n8[2], tgString("left inner bicep muscle seg", 12) + tgString(" seg", 8));
spine.addPair(n12[4], n8[3], tgString("left outer bicep muscle seg", 12) + tgString(" seg", 8));
spine.addPair(n12[4], n1[3], tgString("left front abdomen connection muscle seg", 12) + tgString(" seg", 1)); //Was n1[2]
spine.addPair(n12[3], n8[2], tgString("left inner tricep muscle seg", 12) + tgString(" seg", 8));
spine.addPair(n12[3], n8[3], tgString("left outer tricep muscle seg", 12) + tgString(" seg", 8));
spine.addPair(n12[2], n8[2], tgString("left inner front tricep muscle seg", 12) + tgString(" seg", 8));
spine.addPair(n12[2], n8[3], tgString("left outer front tricep muscle seg", 12) + tgString(" seg", 8));
//Adding muscle to pull up on left front leg:
spine.addPair(n12[4], n8[1], tgString("left mid bicep muscle seg", 12) + tgString(" seg", 8));
//Right rear leg/hip
spine.addPair(n11[4], n7[2], tgString("right outer thigh muscle seg", 11) + tgString(" seg", 7));
spine.addPair(n11[4], n7[3], tgString("right inner thigh muscle seg", 11) + tgString(" seg", 7));
spine.addPair(n11[4], n3[4],tgString("right rear abdomen connection muscle seg", 11) + tgString(" seg", 3));
spine.addPair(n11[3], n5[1],tgString("right rear abdomen connection muscle seg", 11) + tgString(" seg", 5));
spine.addPair(n11[3], n7[2], tgString("right outer calf muscle seg", 11) + tgString(" seg", 7));
spine.addPair(n11[3], n7[3], tgString("right inner calf muscle seg", 11) + tgString(" seg", 7));
spine.addPair(n11[2], n7[2], tgString("right outer front calf muscle seg", 11) + tgString(" seg", 7));
spine.addPair(n11[2], n7[3], tgString("right inner front calf muscle seg", 11) + tgString(" seg", 7));
//Adding muscle to pull rear right leg up:
spine.addPair(n11[4], n7[1], tgString("right central thigh muscle seg", 11) + tgString(" seg", 7));
//Left rear leg/hip
spine.addPair(n13[4], n9[2], tgString("left inner thigh muscle seg", 13) + tgString(" seg", 9));
spine.addPair(n13[4], n9[3], tgString("left outer thigh muscle seg", 13) + tgString(" seg", 9));
spine.addPair(n13[4], n3[3], tgString("left rear abdomen connection muscle seg", 13) + tgString(" seg", 3));
spine.addPair(n13[3], n5[2], tgString("left rear abdomen connection muscle seg", 13) + tgString(" seg", 5));
spine.addPair(n13[3], n9[2], tgString("left inner calf muscle seg", 13) + tgString(" seg", 9));
spine.addPair(n13[3], n9[3], tgString("left outer calf muscle seg", 13) + tgString(" seg", 9));
spine.addPair(n13[2], n9[2], tgString("left inner front calf muscle seg", 13) + tgString(" seg", 9));
spine.addPair(n13[2], n9[3], tgString("left outer front calf muscle seg", 13) + tgString(" seg", 9));
//Adding muscle to pull rear left leg up:
spine.addPair(n13[4], n9[1], tgString("left central thigh muscle seg", 13) + tgString(" seg", 9));
//Populate feet with muscles. Todo: think up names to differentiate each!
for(std::size_t i = (m_segments + m_hips + m_legs); i < children.size(); i++) {
tgNodes ni = children[i]->getNodes();
tgNodes ni4 = children[i-4]->getNodes(); //Think of a nicer name for this!
spine.addPair(ni[0],ni[1],tgString("foot muscle seg", i));
spine.addPair(ni[0],ni[3],tgString("foot muscle seg", i));
spine.addPair(ni[1],ni[2],tgString("foot muscle seg", i));
spine.addPair(ni[2],ni[3],tgString("foot muscle seg", i));
spine.addPair(ni[0],ni[7],tgString("foot muscle seg", i));
spine.addPair(ni[1],ni[4],tgString("foot muscle seg", i));
spine.addPair(ni[2],ni[5],tgString("foot muscle seg", i));
spine.addPair(ni[3],ni[6],tgString("foot muscle seg", i));
spine.addPair(ni[4],ni[5],tgString("foot muscle seg", i));
spine.addPair(ni[4],ni[7],tgString("foot muscle seg", i));
spine.addPair(ni[5],ni[6],tgString("foot muscle seg", i));
spine.addPair(ni[6],ni[7],tgString("foot muscle seg", i));
//Connecting feet to legs:
//spine.addPair(ni4[5],ni[1],tgString("foot muscle seg", i) + tgString(" seg", i-4));
//spine.addPair(ni4[5],ni[2],tgString("foot muscle seg", i) + tgString(" seg", i-4));
//spine.addPair(ni4[5],ni[5],tgString("foot muscle seg", i) + tgString(" seg", i-4));
//spine.addPair(ni4[6],ni[0],tgString("foot muscle seg", i) + tgString(" seg", i-4));
//spine.addPair(ni4[6],ni[3],tgString("foot muscle seg", i) + tgString(" seg", i-4));
//spine.addPair(ni4[6],ni[7],tgString("foot muscle seg", i) + tgString(" seg", i-4));
//spine.addPair(ni4[7],ni[0],tgString("foot muscle seg", i) + tgString(" seg", i-4));
//spine.addPair(ni4[7],ni[1],tgString("foot muscle seg", i) + tgString(" seg", i-4));
//spine.addPair(ni4[7],ni[4],tgString("foot muscle seg", i) + tgString(" seg", i-4));
//spine.addPair(ni4[8],ni[2],tgString("foot muscle seg", i) + tgString(" seg", i-4));
//spine.addPair(ni4[8],ni[3],tgString("foot muscle seg", i) + tgString(" seg", i-4));
//spine.addPair(ni4[8],ni[6],tgString("foot muscle seg", i) + tgString(" seg", i-4));
//Trying out these for foot:
spine.addPair(ni4[5],ni[0],tgString("foot muscle seg", i) + tgString(" seg", i-4));
spine.addPair(ni4[5],ni[1],tgString("foot muscle seg", i) + tgString(" seg", i-4));
spine.addPair(ni4[5],ni[2],tgString("foot muscle seg", i) + tgString(" seg", i-4));
spine.addPair(ni4[5],ni[3],tgString("foot muscle seg", i) + tgString(" seg", i-4));
spine.addPair(ni4[0],ni[4],tgString("foot muscle seg", i) + tgString(" seg", i-4));
spine.addPair(ni4[0],ni[5],tgString("foot muscle seg", i) + tgString(" seg", i-4));
spine.addPair(ni4[0],ni[6],tgString("foot muscle seg", i) + tgString(" seg", i-4));
spine.addPair(ni4[0],ni[7],tgString("foot muscle seg", i) + tgString(" seg", i-4));
}
//Don't forget muscles connecting hips to feet!
// Create the build spec that uses tags to turn the structure into a real model
tgBuildSpec spec;
spec.addBuilder("rod", new tgRodInfo(rodConfig));
#ifdef USE_KINEMATIC
spec.addBuilder("muscle", new tgKinematicContactCableInfo(motorConfig));
#else
spec.addBuilder("muscle", new tgBasicActuatorInfo(muscleConfig));
#endif
// Create your structureInfo
tgStructureInfo structureInfo(spine, spec);
// Use the structureInfo to build ourselves
structureInfo.buildInto(*this, world);
// We could now use tgCast::filter or similar to pull out the
// models (e.g. muscles) that we want to control.
allActuators = tgCast::filter<tgModel, tgSpringCableActuator> (getDescendants());
// Notify controllers that setup has finished.
notifySetup();
// Actually setup the children
tgModel::setup(world);
children.clear();
}
// This is basically a manual setup of a model.
// There are things that do this for us (@todo: reference the things that do this for us)
void TetraSpineGoal::setup(tgWorld& world)
{
const double edge = 3.8 * scaleFactor;
const double height = tgUtil::round(std::sqrt(3.0)/2 * edge);
std::cout << "edge: " << edge << "; height: " << height << std::endl;
// Create the tetrahedra
tgStructure tetra;
addNodes(tetra, edge, height, scaleFactor);
addPairs(tetra);
// Move the first one so we can create a longer snake.
// Or you could move the snake at the end, up to you.
tetra.move(btVector3(0.0, 8.0, 10.0));
// Create our snake segments
tgStructure snake;
addSegments(snake, tetra, -2.30 * scaleFactor, m_segments);
addMuscles(snake);
// Create the build spec that uses tags to turn the structure into a real model
// Note: This needs to be high enough or things fly apart...
// Params for In Won
const double radius = 0.635 * scaleFactor / 10.0;
const double sphereRadius = 0.635 * scaleFactor / (10.0);
const double density = 2.0 *.0201 / (pow(radius, 2) * M_PI * edge); // Mass divided by volume... should there be a way to set this automatically??
const double friction = 0.5;
const tgRod::Config rodConfig(radius, density, friction);
tgBuildSpec spec;
spec.addBuilder("rod", new tgRodInfo(rodConfig));
// 1000 is so the units below can be in grams
const double sphereVolume1 = 1000.0 * 4.0 / 3.0 * M_PI * pow(sphereRadius, 3);
const double sphereVolume2 = 1000.0 * 4.0 / 3.0 * M_PI * pow(sphereRadius, 3);
const double baseCornerMidD = 180.0 / sphereVolume1;
const tgSphere::Config baseCornerMidConfig(sphereRadius, baseCornerMidD, friction);
spec.addBuilder("base", new tgSphereInfo(baseCornerMidConfig));
const double tipCornerMidD = 120.0 / sphereVolume1;
const tgSphere::Config tipCornerMidConfig(sphereRadius, tipCornerMidD, friction);
spec.addBuilder("tip", new tgSphereInfo(tipCornerMidConfig));
const double PCBD = 70.0 / sphereVolume2;
const tgSphere::Config PCB_1_Config(radius, PCBD, friction);
spec.addBuilder("PCB", new tgSphereInfo(PCB_1_Config));
const double elasticity = 1000.0;
const double damping = 10.0;
const double pretension = 0.0;
const bool history = false;
const double maxTens = 7000.0;
const double maxSpeed = 12.0;
const double mRad = 1.0;
const double motorFriction = 10.0;
const double motorInertia = 1.0;
const bool backDrivable = false;
tgKinematicActuator::Config motorConfig(elasticity, damping, pretension,
mRad, motorFriction, motorInertia, backDrivable,
history, maxTens, maxSpeed);
spec.addBuilder("muscle", new tgKinematicContactCableInfo(motorConfig));
// Create your structureInfo
tgStructureInfo structureInfo(snake, spec);
// Use the structureInfo to build ourselves
structureInfo.buildInto(*this, world);
// We could now use tgCast::filter or similar to pull out the models (e.g. muscles)
// that we want to control.
m_allMuscles = this->find<tgSpringCableActuator> ("muscle");
m_allSegments = this->find<tgModel> ("segment");
mapMuscles(m_muscleMap, *this);
//addMarkers(snake, *this);
#if (0)
trace(structureInfo, *this);
#endif
// Actually setup the children
BaseSpineModelGoal::setup(world);
}
void applyFixedWingLaunchController(timeUs_t currentTimeUs)
{
// Called at PID rate
if (launchState.launchDetected) {
bool applyLaunchIdleLogic = true;
if (launchState.motorControlAllowed) {
// If launch detected we are in launch procedure - control airplane
const float timeElapsedSinceLaunchMs = US2MS(currentTimeUs - launchState.launchStartedTime);
// If user moves the stick - finish the launch
if ((ABS(rcCommand[ROLL]) > rcControlsConfig()->pos_hold_deadband) || (ABS(rcCommand[PITCH]) > rcControlsConfig()->pos_hold_deadband)) {
launchState.launchFinished = true;
}
// Abort launch after a pre-set time
if (timeElapsedSinceLaunchMs >= navConfig()->fw.launch_timeout) {
launchState.launchFinished = true;
}
// Motor control enabled
if (timeElapsedSinceLaunchMs >= navConfig()->fw.launch_motor_timer) {
// Don't apply idle logic anymore
applyLaunchIdleLogic = false;
// Increase throttle gradually over `launch_motor_spinup_time` milliseconds
if (navConfig()->fw.launch_motor_spinup_time > 0) {
const float timeSinceMotorStartMs = constrainf(timeElapsedSinceLaunchMs - navConfig()->fw.launch_motor_timer, 0.0f, navConfig()->fw.launch_motor_spinup_time);
const uint16_t minIdleThrottle = MAX(motorConfig()->minthrottle, navConfig()->fw.launch_idle_throttle);
rcCommand[THROTTLE] = scaleRangef(timeSinceMotorStartMs,
0.0f, navConfig()->fw.launch_motor_spinup_time,
minIdleThrottle, navConfig()->fw.launch_throttle);
}
else {
rcCommand[THROTTLE] = navConfig()->fw.launch_throttle;
}
}
}
if (applyLaunchIdleLogic) {
// Launch idle logic - low throttle and zero out PIDs
applyFixedWingLaunchIdleLogic();
}
}
else {
// We are waiting for launch - update launch detector
updateFixedWingLaunchDetector(currentTimeUs);
// Launch idle logic - low throttle and zero out PIDs
applyFixedWingLaunchIdleLogic();
}
// Control beeper
if (!launchState.launchFinished) {
beeper(BEEPER_LAUNCH_MODE_ENABLED);
}
// Lock out controls
rcCommand[ROLL] = 0;
rcCommand[PITCH] = pidAngleToRcCommand(-DEGREES_TO_DECIDEGREES(navConfig()->fw.launch_climb_angle), pidProfile()->max_angle_inclination[FD_PITCH]);
rcCommand[YAW] = 0;
}
static void validateAndFixConfig(void)
{
#if !defined(USE_QUAD_MIXER_ONLY) && !defined(USE_OSD_SLAVE)
// Reset unsupported mixer mode to default.
// This check will be gone when motor/servo mixers are loaded dynamically
// by configurator as a part of configuration procedure.
mixerMode_e mixerMode = mixerConfigMutable()->mixerMode;
if (!(mixerMode == MIXER_CUSTOM || mixerMode == MIXER_CUSTOM_AIRPLANE || mixerMode == MIXER_CUSTOM_TRI)) {
if (mixers[mixerMode].motorCount && mixers[mixerMode].motor == NULL)
mixerConfigMutable()->mixerMode = MIXER_CUSTOM;
#ifdef USE_SERVOS
if (mixers[mixerMode].useServo && servoMixers[mixerMode].servoRuleCount == 0)
mixerConfigMutable()->mixerMode = MIXER_CUSTOM_AIRPLANE;
#endif
}
#endif
#ifndef USE_OSD_SLAVE
if (systemConfig()->activeRateProfile >= CONTROL_RATE_PROFILE_COUNT) {
systemConfigMutable()->activeRateProfile = 0;
}
setControlRateProfile(systemConfig()->activeRateProfile);
if (systemConfig()->pidProfileIndex >= MAX_PROFILE_COUNT) {
systemConfigMutable()->pidProfileIndex = 0;
}
setPidProfile(systemConfig()->pidProfileIndex);
// Prevent invalid notch cutoff
if (currentPidProfile->dterm_notch_cutoff >= currentPidProfile->dterm_notch_hz) {
currentPidProfile->dterm_notch_hz = 0;
}
if ((motorConfig()->dev.motorPwmProtocol == PWM_TYPE_BRUSHED) && (motorConfig()->mincommand < 1000)) {
motorConfigMutable()->mincommand = 1000;
}
if ((motorConfig()->dev.motorPwmProtocol == PWM_TYPE_STANDARD) && (motorConfig()->dev.motorPwmRate > BRUSHLESS_MOTORS_PWM_RATE)) {
motorConfigMutable()->dev.motorPwmRate = BRUSHLESS_MOTORS_PWM_RATE;
}
validateAndFixGyroConfig();
if (!(featureConfigured(FEATURE_RX_PARALLEL_PWM) || featureConfigured(FEATURE_RX_PPM) || featureConfigured(FEATURE_RX_SERIAL) || featureConfigured(FEATURE_RX_MSP) || featureConfigured(FEATURE_RX_SPI))) {
featureSet(DEFAULT_RX_FEATURE);
}
if (featureConfigured(FEATURE_RX_PPM)) {
featureClear(FEATURE_RX_SERIAL | FEATURE_RX_PARALLEL_PWM | FEATURE_RX_MSP | FEATURE_RX_SPI);
}
if (featureConfigured(FEATURE_RX_MSP)) {
featureClear(FEATURE_RX_SERIAL | FEATURE_RX_PARALLEL_PWM | FEATURE_RX_PPM | FEATURE_RX_SPI);
}
if (featureConfigured(FEATURE_RX_SERIAL)) {
featureClear(FEATURE_RX_PARALLEL_PWM | FEATURE_RX_MSP | FEATURE_RX_PPM | FEATURE_RX_SPI);
}
#ifdef USE_RX_SPI
if (featureConfigured(FEATURE_RX_SPI)) {
featureClear(FEATURE_RX_SERIAL | FEATURE_RX_PARALLEL_PWM | FEATURE_RX_PPM | FEATURE_RX_MSP);
}
#endif // USE_RX_SPI
if (featureConfigured(FEATURE_RX_PARALLEL_PWM)) {
featureClear(FEATURE_RX_SERIAL | FEATURE_RX_MSP | FEATURE_RX_PPM | FEATURE_RX_SPI);
#if defined(STM32F10X)
// rssi adc needs the same ports
featureClear(FEATURE_RSSI_ADC);
// current meter needs the same ports
if (batteryConfig()->currentMeterSource == CURRENT_METER_ADC) {
batteryConfigMutable()->currentMeterSource = CURRENT_METER_NONE;
}
#endif // STM32F10X
// software serial needs free PWM ports
featureClear(FEATURE_SOFTSERIAL);
}
#ifdef USE_SOFTSPI
if (featureConfigured(FEATURE_SOFTSPI)) {
featureClear(FEATURE_RX_PPM | FEATURE_RX_PARALLEL_PWM | FEATURE_SOFTSERIAL);
batteryConfigMutable()->voltageMeterSource = VOLTAGE_METER_NONE;
#if defined(STM32F10X)
featureClear(FEATURE_LED_STRIP);
// rssi adc needs the same ports
featureClear(FEATURE_RSSI_ADC);
// current meter needs the same ports
if (batteryConfig()->currentMeterSource == CURRENT_METER_ADC) {
batteryConfigMutable()->currentMeterSource = CURRENT_METER_NONE;
}
#endif // STM32F10X
}
#endif // USE_SOFTSPI
#endif // USE_OSD_SLAVE
if (!isSerialConfigValid(serialConfig())) {
pgResetFn_serialConfig(serialConfigMutable());
}
// clear features that are not supported.
// I have kept them all here in one place, some could be moved to sections of code above.
#ifndef USE_PPM
featureClear(FEATURE_RX_PPM);
#endif
#ifndef USE_SERIAL_RX
featureClear(FEATURE_RX_SERIAL);
#endif
#if !defined(USE_SOFTSERIAL1) && !defined(USE_SOFTSERIAL2)
featureClear(FEATURE_SOFTSERIAL);
#endif
#ifndef USE_GPS
featureClear(FEATURE_GPS);
#endif
#ifndef USE_RANGEFINDER
featureClear(FEATURE_RANGEFINDER);
#endif
#ifndef USE_TELEMETRY
featureClear(FEATURE_TELEMETRY);
#endif
#ifndef USE_PWM
featureClear(FEATURE_RX_PARALLEL_PWM);
#endif
#ifndef USE_RX_MSP
featureClear(FEATURE_RX_MSP);
#endif
#ifndef USE_LED_STRIP
featureClear(FEATURE_LED_STRIP);
#endif
#ifndef USE_DASHBOARD
featureClear(FEATURE_DASHBOARD);
#endif
#ifndef USE_OSD
featureClear(FEATURE_OSD);
#endif
#ifndef USE_SERVOS
featureClear(FEATURE_SERVO_TILT | FEATURE_CHANNEL_FORWARDING);
#endif
#ifndef USE_TRANSPONDER
featureClear(FEATURE_TRANSPONDER);
#endif
#ifndef USE_RX_SPI
featureClear(FEATURE_RX_SPI);
#endif
#ifndef USE_SOFTSPI
featureClear(FEATURE_SOFTSPI);
#endif
#ifndef USE_ESC_SENSOR
featureClear(FEATURE_ESC_SENSOR);
#endif
#ifndef USE_GYRO_DATA_ANALYSE
featureClear(FEATURE_DYNAMIC_FILTER);
#endif
#if defined(TARGET_VALIDATECONFIG)
targetValidateConfiguration();
#endif
}
void validateAndFixGyroConfig(void)
{
// Prevent invalid notch cutoff
if (gyroConfig()->gyro_soft_notch_cutoff_1 >= gyroConfig()->gyro_soft_notch_hz_1) {
gyroConfigMutable()->gyro_soft_notch_hz_1 = 0;
}
if (gyroConfig()->gyro_soft_notch_cutoff_2 >= gyroConfig()->gyro_soft_notch_hz_2) {
gyroConfigMutable()->gyro_soft_notch_hz_2 = 0;
}
if (gyroConfig()->gyro_lpf != GYRO_LPF_256HZ && gyroConfig()->gyro_lpf != GYRO_LPF_NONE) {
pidConfigMutable()->pid_process_denom = 1; // When gyro set to 1khz always set pid speed 1:1 to sampling speed
gyroConfigMutable()->gyro_sync_denom = 1;
gyroConfigMutable()->gyro_use_32khz = false;
}
if (gyroConfig()->gyro_use_32khz) {
// F1 and F3 can't handle high sample speed.
#if defined(STM32F1)
gyroConfigMutable()->gyro_sync_denom = MAX(gyroConfig()->gyro_sync_denom, 16);
#elif defined(STM32F3)
gyroConfigMutable()->gyro_sync_denom = MAX(gyroConfig()->gyro_sync_denom, 4);
#endif
} else {
#if defined(STM32F1)
gyroConfigMutable()->gyro_sync_denom = MAX(gyroConfig()->gyro_sync_denom, 3);
#endif
}
float samplingTime;
switch (gyroMpuDetectionResult()->sensor) {
case ICM_20649_SPI:
samplingTime = 1.0f / 9000.0f;
break;
case BMI_160_SPI:
samplingTime = 0.0003125f;
break;
default:
samplingTime = 0.000125f;
break;
}
if (gyroConfig()->gyro_lpf != GYRO_LPF_256HZ && gyroConfig()->gyro_lpf != GYRO_LPF_NONE) {
switch (gyroMpuDetectionResult()->sensor) {
case ICM_20649_SPI:
samplingTime = 1.0f / 1100.0f;
break;
default:
samplingTime = 0.001f;
break;
}
}
if (gyroConfig()->gyro_use_32khz) {
samplingTime = 0.00003125;
}
// check for looptime restrictions based on motor protocol. Motor times have safety margin
float motorUpdateRestriction;
switch (motorConfig()->dev.motorPwmProtocol) {
case PWM_TYPE_STANDARD:
motorUpdateRestriction = 1.0f / BRUSHLESS_MOTORS_PWM_RATE;
break;
case PWM_TYPE_ONESHOT125:
motorUpdateRestriction = 0.0005f;
break;
case PWM_TYPE_ONESHOT42:
motorUpdateRestriction = 0.0001f;
break;
#ifdef USE_DSHOT
case PWM_TYPE_DSHOT150:
motorUpdateRestriction = 0.000250f;
break;
case PWM_TYPE_DSHOT300:
motorUpdateRestriction = 0.0001f;
break;
#endif
default:
motorUpdateRestriction = 0.00003125f;
break;
}
if (motorConfig()->dev.useUnsyncedPwm) {
// Prevent overriding the max rate of motors
if ((motorConfig()->dev.motorPwmProtocol <= PWM_TYPE_BRUSHED) && (motorConfig()->dev.motorPwmProtocol != PWM_TYPE_STANDARD)) {
const uint32_t maxEscRate = lrintf(1.0f / motorUpdateRestriction);
motorConfigMutable()->dev.motorPwmRate = MIN(motorConfig()->dev.motorPwmRate, maxEscRate);
}
} else {
const float pidLooptime = samplingTime * gyroConfig()->gyro_sync_denom * pidConfig()->pid_process_denom;
if (pidLooptime < motorUpdateRestriction) {
const uint8_t minPidProcessDenom = constrain(motorUpdateRestriction / (samplingTime * gyroConfig()->gyro_sync_denom), 1, MAX_PID_PROCESS_DENOM);
pidConfigMutable()->pid_process_denom = MAX(pidConfigMutable()->pid_process_denom, minPidProcessDenom);
}
}
}
// Default settings
STATIC_UNIT_TESTED void resetConf(void)
{
pgResetAll(MAX_PROFILE_COUNT);
setProfile(0);
pgActivateProfile(0);
setControlRateProfile(0);
parseRcChannels("AETR1234", rxConfig());
featureClearAll();
featureSet(DEFAULT_RX_FEATURE | FEATURE_FAILSAFE | FEATURE_BLACKBOX);
#ifdef DEFAULT_FEATURES
featureSet(DEFAULT_FEATURES);
#endif
#ifdef BOARD_HAS_VOLTAGE_DIVIDER
// only enable the feature by default if the board has supporting hardware
featureSet(FEATURE_VBAT);
#endif
#ifdef BOARD_HAS_AMPERAGE_METER
// only enable the feature by default if the board has supporting hardware
featureSet(FEATURE_AMPERAGE_METER);
batteryConfig()->amperageMeterSource = AMPERAGE_METER_ADC;
#endif
// alternative defaults settings for ALIENFLIGHTF1 and ALIENFLIGHTF3 targets
#ifdef ALIENFLIGHT
#ifdef ALIENFLIGHTF3
serialConfig()->portConfigs[2].functionMask = FUNCTION_RX_SERIAL;
getVoltageMeterConfig(ADC_BATTERY)->vbatscale = 20;
sensorSelectionConfig()->mag_hardware = MAG_NONE; // disabled by default
# else
serialConfig()->portConfigs[1].functionMask = FUNCTION_RX_SERIAL;
# endif
rxConfig()->serialrx_provider = SERIALRX_SPEKTRUM2048;
rxConfig()->spektrum_sat_bind = 5;
motorConfig()->minthrottle = 1000;
motorConfig()->maxthrottle = 2000;
motorConfig()->motor_pwm_rate = 32000;
failsafeConfig()->failsafe_delay = 2;
failsafeConfig()->failsafe_off_delay = 0;
mixerConfig()->yaw_jump_prevention_limit = YAW_JUMP_PREVENTION_LIMIT_HIGH;
currentControlRateProfile->rcRate8 = 100;
currentControlRateProfile->rates[PITCH] = 20;
currentControlRateProfile->rates[ROLL] = 20;
currentControlRateProfile->rates[YAW] = 20;
parseRcChannels("TAER1234", rxConfig());
*customMotorMixer(0) = (motorMixer_t){ 1.0f, -0.414178f, 1.0f, -1.0f }; // REAR_R
*customMotorMixer(1) = (motorMixer_t){ 1.0f, -0.414178f, -1.0f, 1.0f }; // FRONT_R
*customMotorMixer(2) = (motorMixer_t){ 1.0f, 0.414178f, 1.0f, 1.0f }; // REAR_L
*customMotorMixer(3) = (motorMixer_t){ 1.0f, 0.414178f, -1.0f, -1.0f }; // FRONT_L
*customMotorMixer(4) = (motorMixer_t){ 1.0f, -1.0f, -0.414178f, -1.0f }; // MIDFRONT_R
*customMotorMixer(5) = (motorMixer_t){ 1.0f, 1.0f, -0.414178f, 1.0f }; // MIDFRONT_L
*customMotorMixer(6) = (motorMixer_t){ 1.0f, -1.0f, 0.414178f, 1.0f }; // MIDREAR_R
*customMotorMixer(7) = (motorMixer_t){ 1.0f, 1.0f, 0.414178f, -1.0f }; // MIDREAR_L
#endif
// copy first profile into remaining profile
PG_FOREACH_PROFILE(reg) {
for (int i = 1; i < MAX_PROFILE_COUNT; i++) {
memcpy(reg->address + i * pgSize(reg), reg->address, pgSize(reg));
}
}
for (int i = 1; i < MAX_PROFILE_COUNT; i++) {
configureRateProfileSelection(i, i % MAX_CONTROL_RATE_PROFILE_COUNT);
}
}
void BigPuppySymmetricArching::setup(tgWorld& world)
{
//Rod and Muscle configuration.
const double density = 4.2/300.0; //Note: this needs to be high enough or things fly apart...
const double radius = 0.5;
const double rod_space = 10.0;
const double rod_space2 = 8.0;
const double friction = 0.5;
const double rollFriction = 0.0;
const double restitution = 0.0;
const tgRod::Config rodConfig(radius, density, friction, rollFriction, restitution);
const double stiffness = 1000.0;
const double stiffnessPassive = 4000.0;
const double damping = .01*stiffness;
const double pretension = 0.0;
const bool history = true;
const double maxTens = 7000.0;
const double maxSpeed = 12.0;
const double passivePretension = 1000;
const double passivePretension2 = 3500;
const double passivePretension3 = 3500;
#ifdef USE_KINEMATIC
const double mRad = 1.0;
const double motorFriction = 10.0;
const double motorInertia = 1.0;
const bool backDrivable = false;
#ifdef PASSIVE_STRUCTURE
tgKinematicActuator::Config motorConfig(2000, 20, passivePretension,
mRad, motorFriction, motorInertia, backDrivable,
history, maxTens, maxSpeed);
#else
tgKinematicActuator::Config motorConfigSpine(stiffness, damping, pretension,
mRad, motorFriction, motorInertia, backDrivable,
history, maxTens, maxSpeed);
tgKinematicActuator::Config motorConfigOther(stiffnessPassive, damping, passivePretension2,
mRad, motorFriction, motorInertia, backDrivable,
history, maxTens, maxSpeed);
tgKinematicActuator::Config motorConfigFeet(stiffnessPassive, damping, passivePretension,
mRad, motorFriction, motorInertia, backDrivable,
history, maxTens, maxSpeed);
tgKinematicActuator::Config motorConfigLegs(stiffnessPassive, damping, passivePretension3,
mRad, motorFriction, motorInertia, backDrivable,
history, maxTens, maxSpeed);
#endif
#else
#ifdef PASSIVE_STRUCTURE
tgSpringCableActuator::Config muscleConfig(2000, 20, passivePretension);
#else
tgSpringCableActuator::Config muscleConfigSpine(stiffness, damping, pretension, history, maxTens, 2*maxSpeed);
tgSpringCableActuator::Config muscleConfigOther(stiffnessPassive, damping, passivePretension2);
tgSpringCableActuator::Config muscleConfigFeet(stiffnessPassive, damping, passivePretension);
tgSpringCableActuator::Config muscleConfigLegs(stiffnessPassive, damping, passivePretension3);
#endif
#endif
//Foot:
tgStructure foot;
addNodesFoot(foot,rod_space,rod_space2);
addRodsFoot(foot);
//Leg:
tgStructure leg;
addNodesLeg(leg,rod_space);
addRodsLeg(leg);
//Create the basic unit of the puppy
tgStructure vertebra;
addNodesVertebra(vertebra,rod_space);
addRodsVertebra(vertebra);
//Create the basic unit for the hips/shoulders.
tgStructure hip;
addNodesHip(hip,rod_space);
addRodsHip(hip);
//Build the puppy
tgStructure puppy;
const double yOffset_foot = -(2*rod_space+6);
addSegments(puppy,vertebra,hip,leg,foot,rod_space); //,m_segments,m_hips,m_legs,m_feet
puppy.move(btVector3(0.0,-yOffset_foot,0.0));
addMuscles(puppy); //,m_segments,m_hips,m_legs,m_feet
//Time to add the muscles to the structure. Todo: make this a function; also try to clean this up.
std::vector<tgStructure*> children = puppy.getChildren();
// Create the build spec that uses tags to turn the structure into a real model
tgBuildSpec spec;
spec.addBuilder("rod", new tgRodInfo(rodConfig));
#ifdef USE_KINEMATIC
#ifdef PASSIVE_STRUCTURE
spec.addBuilder("muscle", new tgKinematicContactCableInfo(motorConfig));
#else
spec.addBuilder("muscleAct", new tgKinematicContactCableInfo(motorConfigSpine));
spec.addBuilder("muscle ", new tgKinematicContactCableInfo(motorConfigOther));
spec.addBuilder("muscle2 ", new tgKinematicContactCableInfo(motorConfigFeet));
spec.addBuilder("muscle3 ", new tgKinematicContactCableInfo(motorConfigLegs));
#endif
#else
#ifdef PASSIVE_STRUCTURE
spec.addBuilder("muscle", new tgBasicActuatorInfo(muscleConfig));
#else
spec.addBuilder("muscleAct" , new tgBasicActuatorInfo(muscleConfigSpine));
spec.addBuilder("muscle " , new tgBasicActuatorInfo(muscleConfigOther));
spec.addBuilder("muscle2 " , new tgBasicActuatorInfo(muscleConfigFeet));
spec.addBuilder("muscle3 " , new tgBasicActuatorInfo(muscleConfigLegs));
#endif
#endif
// Create your structureInfo
tgStructureInfo structureInfo(puppy, spec);
// Use the structureInfo to build ourselves
structureInfo.buildInto(*this, world);
// We could now use tgCast::filter or similar to pull out the
// models (e.g. muscles) that we want to control.
m_allMuscles = tgCast::filter<tgModel, tgSpringCableActuator> (getDescendants());
m_allSegments = this->find<tgModel> ("segment");
// Actually setup the children, notify controller that the setup has finished
BaseQuadModelLearning::setup(world);
children.clear();
}
int calcEscRpm(int erpm)
{
return (erpm * 100) / (motorConfig()->motorPoleCount / 2);
}