float convertExternalToMotor(uint16_t externalValue)
{
uint16_t motorValue;
switch ((int)isMotorProtocolDshot()) {
#ifdef USE_DSHOT
case true:
externalValue = constrain(externalValue, PWM_RANGE_MIN, PWM_RANGE_MAX);
if (feature(FEATURE_3D)) {
if (externalValue == PWM_RANGE_MID) {
motorValue = DSHOT_DISARM_COMMAND;
} else if (externalValue < PWM_RANGE_MID) {
motorValue = scaleRange(externalValue, PWM_RANGE_MIN, PWM_RANGE_MID - 1, DSHOT_3D_DEADBAND_LOW, DSHOT_MIN_THROTTLE);
} else {
motorValue = scaleRange(externalValue, PWM_RANGE_MID + 1, PWM_RANGE_MAX, DSHOT_3D_DEADBAND_HIGH, DSHOT_MAX_THROTTLE);
}
} else {
motorValue = (externalValue == PWM_RANGE_MIN) ? DSHOT_DISARM_COMMAND : scaleRange(externalValue, PWM_RANGE_MIN + 1, PWM_RANGE_MAX, DSHOT_MIN_THROTTLE, DSHOT_MAX_THROTTLE);
}
break;
case false:
#endif
default:
motorValue = externalValue;
break;
}
return (float)motorValue;
}
uint16_t convertMotorToExternal(float motorValue)
{
uint16_t externalValue;
switch ((int)isMotorProtocolDshot()) {
#ifdef USE_DSHOT
case true:
if (feature(FEATURE_3D)) {
if (motorValue == DSHOT_DISARM_COMMAND || motorValue < DSHOT_MIN_THROTTLE) {
externalValue = PWM_RANGE_MID;
} else if (motorValue <= DSHOT_3D_DEADBAND_LOW) {
externalValue = scaleRange(motorValue, DSHOT_MIN_THROTTLE, DSHOT_3D_DEADBAND_LOW, PWM_RANGE_MID - 1, PWM_RANGE_MIN);
} else {
externalValue = scaleRange(motorValue, DSHOT_3D_DEADBAND_HIGH, DSHOT_MAX_THROTTLE, PWM_RANGE_MID + 1, PWM_RANGE_MAX);
}
} else {
externalValue = (motorValue < DSHOT_MIN_THROTTLE) ? PWM_RANGE_MIN : scaleRange(motorValue, DSHOT_MIN_THROTTLE, DSHOT_MAX_THROTTLE, PWM_RANGE_MIN + 1, PWM_RANGE_MAX);
}
break;
case false:
#endif
default:
externalValue = motorValue;
break;
}
return externalValue;
}
//scales a number using different scales depending if num is greater or less than the zero point oldZero.
//if hardCutoff is true, it will not exceed the extrema
double SensorOutput::scaleWeightedRange(double num, double oldMin, double oldZero, double oldMax, double newMin, double newMax, bool hardCutoff) {
double newZero = scaleRange(0.5, 0, 1, newMin, newMax);
if (num > oldZero)
return scaleRange(num, oldZero, oldMax, newZero, newMax, hardCutoff);
else
return scaleRange(num, oldMin, oldZero, newMin, newZero, hardCutoff);
}
static void applyLedFixedLayers()
{
for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) {
const ledConfig_t *ledConfig = &ledStripConfig()->ledConfigs[ledIndex];
hsvColor_t color = *getSC(LED_SCOLOR_BACKGROUND);
int fn = ledGetFunction(ledConfig);
int hOffset = HSV_HUE_MAX;
switch (fn) {
case LED_FUNCTION_COLOR:
color = ledStripConfig()->colors[ledGetColor(ledConfig)];
break;
case LED_FUNCTION_FLIGHT_MODE:
for (unsigned i = 0; i < ARRAYLEN(flightModeToLed); i++)
if (!flightModeToLed[i].flightMode || FLIGHT_MODE(flightModeToLed[i].flightMode)) {
const hsvColor_t *directionalColor = getDirectionalModeColor(ledIndex, &ledStripConfig()->modeColors[flightModeToLed[i].ledMode]);
if (directionalColor) {
color = *directionalColor;
}
break; // stop on first match
}
break;
case LED_FUNCTION_ARM_STATE:
color = ARMING_FLAG(ARMED) ? *getSC(LED_SCOLOR_ARMED) : *getSC(LED_SCOLOR_DISARMED);
break;
case LED_FUNCTION_BATTERY:
color = HSV(RED);
hOffset += scaleRange(calculateBatteryPercentage(), 0, 100, -30, 120);
break;
case LED_FUNCTION_RSSI:
color = HSV(RED);
hOffset += scaleRange(rssi * 100, 0, 1023, -30, 120);
break;
default:
break;
}
if (ledGetOverlayBit(ledConfig, LED_OVERLAY_THROTTLE)) {
hOffset += scaledAux;
}
color.h = (color.h + hOffset) % (HSV_HUE_MAX + 1);
setLedHsv(ledIndex, &color);
}
}
void ledStripUpdate(timeUs_t currentTimeUs)
{
if (!(ledStripInitialised && isWS2811LedStripReady())) {
return;
}
if (IS_RC_MODE_ACTIVE(BOXLEDLOW) && !(ledStripConfig()->ledstrip_visual_beeper && isBeeperOn())) {
if (ledStripEnabled) {
ledStripDisable();
ledStripEnabled = false;
}
return;
}
ledStripEnabled = true;
const uint32_t now = currentTimeUs;
// test all led timers, setting corresponding bits
uint32_t timActive = 0;
for (timId_e timId = 0; timId < timTimerCount; timId++) {
// sanitize timer value, so that it can be safely incremented. Handles inital timerVal value.
const timeDelta_t delta = cmpTimeUs(now, timerVal[timId]);
// max delay is limited to 5s
if (delta < 0 && delta > -MAX_TIMER_DELAY)
continue; // not ready yet
timActive |= 1 << timId;
if (delta >= 100 * 1000 || delta < 0) {
timerVal[timId] = now;
}
}
if (!timActive)
return; // no change this update, keep old state
// apply all layers; triggered timed functions has to update timers
scaledThrottle = ARMING_FLAG(ARMED) ? scaleRange(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX, 0, 100) : 0;
scaledAux = scaleRange(rcData[ledStripConfig()->ledstrip_aux_channel], PWM_RANGE_MIN, PWM_RANGE_MAX, 0, HSV_HUE_MAX + 1);
applyLedFixedLayers();
for (timId_e timId = 0; timId < ARRAYLEN(layerTable); timId++) {
uint32_t *timer = &timerVal[timId];
bool updateNow = timActive & (1 << timId);
(*layerTable[timId])(updateNow, timer);
}
ws2811UpdateStrip();
}
void mavlinkSendRCChannelsAndRSSI(void)
{
uint16_t msgLength;
mavlink_msg_rc_channels_raw_pack(0, 200, &mavMsg,
// time_boot_ms Timestamp (milliseconds since system boot)
millis(),
// port Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos.
0,
// chan1_raw RC channel 1 value, in microseconds
(rxRuntimeConfig.channelCount >= 1) ? rcData[0] : 0,
// chan2_raw RC channel 2 value, in microseconds
(rxRuntimeConfig.channelCount >= 2) ? rcData[1] : 0,
// chan3_raw RC channel 3 value, in microseconds
(rxRuntimeConfig.channelCount >= 3) ? rcData[2] : 0,
// chan4_raw RC channel 4 value, in microseconds
(rxRuntimeConfig.channelCount >= 4) ? rcData[3] : 0,
// chan5_raw RC channel 5 value, in microseconds
(rxRuntimeConfig.channelCount >= 5) ? rcData[4] : 0,
// chan6_raw RC channel 6 value, in microseconds
(rxRuntimeConfig.channelCount >= 6) ? rcData[5] : 0,
// chan7_raw RC channel 7 value, in microseconds
(rxRuntimeConfig.channelCount >= 7) ? rcData[6] : 0,
// chan8_raw RC channel 8 value, in microseconds
(rxRuntimeConfig.channelCount >= 8) ? rcData[7] : 0,
// rssi Receive signal strength indicator, 0: 0%, 255: 100%
scaleRange(rssi, 0, 1023, 0, 255));
msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavMsg);
mavlinkSerialWrite(mavBuffer, msgLength);
}
void osdUpdateFCState(void)
{
fcStatus.vbat = vbat;
fcStatus.fcState = packFlightModeFlags();
fcStatus.rssi = scaleRange(rssi, 0, 1023, 0, 1000);
}
static char osdGetBatterySymbol(int cellVoltage)
{
if (getBatteryState() == BATTERY_CRITICAL) {
return SYM_MAIN_BATT; // FIXME: currently the BAT- symbol, ideally replace with a battery with exclamation mark
} else {
// Calculate a symbol offset using cell voltage over full cell voltage range
const int symOffset = scaleRange(cellVoltage, batteryConfig()->vbatmincellvoltage * 10, batteryConfig()->vbatmaxcellvoltage * 10, 0, 7);
return SYM_BATT_EMPTY - constrain(symOffset, 0, 6);
}
}
uint8_t LeftRightTiltColorGenerator::chooseColorIndex(int16_t axisValue, int axisMin, int axisMax, uint8_t colorsInPalette) {
//Serial.println(sensorDataStore.sampledAccelerationData.x, DEC);
int16_t limitedAxisValue = axisValue;
if (limitedAxisValue < axisMin) {
limitedAxisValue = axisMin;
} else if (limitedAxisValue > axisMax) {
limitedAxisValue = axisMax;
}
return (uint8_t) scaleRange(limitedAxisValue, axisMin, axisMax, 0, colorsInPalette - 1);
}
STATIC_UNIT_TESTED uint16_t applyRxChannelRangeConfiguraton(int sample, rxChannelRangeConfiguration_t range)
{
// Avoid corruption of channel with a value of PPM_RCVR_TIMEOUT
if (sample == PPM_RCVR_TIMEOUT) {
return PPM_RCVR_TIMEOUT;
}
sample = scaleRange(sample, range.min, range.max, PWM_RANGE_MIN, PWM_RANGE_MAX);
sample = MIN(MAX(PWM_PULSE_MIN, sample), PWM_PULSE_MAX);
return sample;
}
void osdElementRender_motors(const element_t *element, elementDataProviderFn dataFn)
{
uint16_t *motors = (uint16_t *) dataFn();
const int maxMotors = 4; // just quad for now
for (int i = 0; i < maxMotors; i++) {
if (!motors[i]) {
continue; // skip unused/uninitialsed motors.
}
int percent = scaleRange(motors[i], 1000, 2000, 0, 100); // FIXME should use min/max command as used by the FC.
osdHardwareDisplayMotor(quadMotorCoordinateOffsets[i].x + element->x, quadMotorCoordinateOffsets[i].y + element->y, percent);
}
}
void MovingRainbowAnimation::frameBegin(void) {
xOffset = 0;
yOffset = 0;
int16_t limitedAxisValue = sensorDataStore.sampledAccelerationData.x;
if (limitedAxisValue < AXIS_X_MIN) {
limitedAxisValue = AXIS_X_MIN;
} else if (limitedAxisValue > AXIS_X_MAX) {
limitedAxisValue = AXIS_X_MAX;
}
int temp = scaleRange(limitedAxisValue, AXIS_X_MIN, AXIS_X_MAX, 0 - MAX_RAINBOW_FRAME_DELAY_MS, MAX_RAINBOW_FRAME_DELAY_MS);
additionalFrameDelay = (MAX_RAINBOW_FRAME_DELAY_MS - abs(temp)) * 1000L;
framePercent = ((uint32_t)frameCounter * 100) / totalFrames;
}
void updateLedStrip(void)
{
if (!(ledStripInitialised && isWS2811LedStripReady())) {
return;
}
if (IS_RC_MODE_ACTIVE(BOXLEDLOW) && !(masterConfig.ledstrip_visual_beeper && isBeeperOn())) {
if (ledStripEnabled) {
ledStripDisable();
ledStripEnabled = false;
}
return;
}
ledStripEnabled = true;
uint32_t now = micros();
// test all led timers, setting corresponding bits
uint32_t timActive = 0;
for (timId_e timId = 0; timId < timTimerCount; timId++) {
// sanitize timer value, so that it can be safely incremented. Handles inital timerVal value.
// max delay is limited to 5s
int32_t delta = cmp32(now, timerVal[timId]);
if (delta < 0 && delta > -LED_STRIP_MS(5000))
continue; // not ready yet
timActive |= 1 << timId;
if (delta >= LED_STRIP_MS(100) || delta < 0) {
timerVal[timId] = now;
}
}
if (!timActive)
return; // no change this update, keep old state
// apply all layers; triggered timed functions has to update timers
scaledThrottle = ARMING_FLAG(ARMED) ? scaleRange(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX, 10, 100) : 10;
applyLedFixedLayers();
for (timId_e timId = 0; timId < ARRAYLEN(layerTable); timId++) {
uint32_t *timer = &timerVal[timId];
bool updateNow = timActive & (1 << timId);
(*layerTable[timId])(updateNow, timer);
}
ws2811UpdateStrip();
}
void sendRcDataToHid(void)
{
int8_t report[8];
for (unsigned i = 0; i < USB_CDC_HID_NUM_AXES; i++) {
const uint8_t channel = hidChannelMapping[i];
report[i] = scaleRange(constrain(rcData[channel], PWM_RANGE_MIN, PWM_RANGE_MAX), PWM_RANGE_MIN, PWM_RANGE_MAX, USB_CDC_HID_RANGE_MIN, USB_CDC_HID_RANGE_MAX);
if (i == 1) {
// For some reason ROLL is inverted in Windows
report[i] = -report[i];
}
}
#if defined(STM32F4)
USBD_HID_SendReport(&USB_OTG_dev, (uint8_t*)report, sizeof(report));
#elif defined(STM32F7)
USBD_HID_SendReport(&USBD_Device, (uint8_t*)report, sizeof(report));
#else
# error "MCU does not support USB HID."
#endif
}
void applyLedThrottleLayer()
{
const ledConfig_t *ledConfig;
hsvColor_t color;
uint8_t ledIndex;
for (ledIndex = 0; ledIndex < ledCount; ledIndex++) {
ledConfig = &ledConfigs[ledIndex];
if (!(ledConfig->flags & LED_FUNCTION_THROTTLE)) {
continue;
}
getLedHsv(ledIndex, &color);
int scaled = scaleRange(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX, -60, +60);
scaled += HSV_HUE_MAX;
color.h = (color.h + scaled) % HSV_HUE_MAX;
setLedHsv(ledIndex, &color);
}
}
// return positive for ACK, negative on error
int mspClientReplyHandler(mspPacket_t *reply)
{
sbuf_t * src = &reply->buf;
int len = sbufBytesRemaining(src);
mspClientStatus.lastReplyAt = micros();
UNUSED(len);
switch (reply->cmd) {
case MSP_STATUS:
fcStatus.cycleTime = sbufReadU16(src);
fcStatus.i2cErrors = sbufReadU16(src);
fcStatus.sensors = sbufReadU16(src);
fcStatus.fcState = sbufReadU32(src);
fcStatus.profile = sbufReadU8(src);
break;
case MSP_ANALOG:
fcStatus.vbat = sbufReadU8(src);
fcStatus.mAhDrawn = sbufReadU16(src);
fcStatus.rssi = scaleRange(sbufReadU16(src), 0, 1023, 0, 1000);
fcStatus.amperage = sbufReadU16(src);
break;
case MSP_MOTOR:
for (unsigned i = 0; i < 8 && i < OSD_MAX_MOTORS; i++) {
fcMotors[i] = sbufReadU16(src);
}
break;
default:
// we do not know how to handle the message
return -1;
}
return 1;
}
void Animator::calculateValueAxisPositionsForSource(ValueAxisSource *valueAxisSource, int16_t rawValue, int16_t rangeMin, int16_t rangeMax) {
#ifdef DEBUG_VALUE_AXIS_POSITIONS
Serial.print("Value Axis Positions (index,low,high) (value,position): ");
#endif
int16_t clampedValue = max(rangeMin, min(rangeMax, rawValue));
for (uint8_t valueAxisIndex = 0; valueAxisIndex < valueAxisCount; valueAxisIndex++) {
ValueAxis *valueAxis = valueAxes[valueAxisIndex];
int8_t position = scaleRange(clampedValue, rangeMin, rangeMax, valueAxis->valueAxisLowValue, valueAxis->valueAxisHighValue);
valueAxisSource->applyValueAxisPosition(valueAxisIndex, position);
#ifdef DEBUG_VALUE_AXIS_POSITIONS
if (valueAxisIndex != 0) {
Serial.print(", ");
}
Serial.print("(");
Serial.print(valueAxisIndex, DEC);
Serial.print(",");
Serial.print(valueAxis->valueAxisLowValue, DEC);
Serial.print(",");
Serial.print(valueAxis->valueAxisLowValue, DEC);
Serial.print(") (");
Serial.print(value, DEC);
Serial.print(",");
Serial.print(valueAxisPositions[valueAxisIndex], DEC);
Serial.print(")");
#endif
}
#ifdef DEBUG_VALUE_AXIS_POSITIONS
Serial.println();
#endif
}
VOID OnPaint(HWND hWnd, HDC hdc)
{
Graphics graphics(hdc);
Pen pen(Color::Red);
Pen axis(Color::Black);
Pen power(Color::Blue);
Pen green(Color::Green);
memblock* mem = w.next(audio.framesAvailable());
audio.fillBuffer();
//graphics.Clear(Color::White);
if (w.hasNext() && beats[(mem->p-w.get_data_p())/(w.getH()->wBitsPerSample/8)/(w.getH()->nChannels)/bd.getPrecision()] != false)
{
RECT rect;
rect.left = 0;
rect.top = 0;
rect.right = scrW;
rect.bottom = scrH;
DrawText(hdc, _T("BEAT"), -1, &rect, DT_CENTER);
}
else graphics.Clear(Color::White);
if (mem->nBytes == 0) return;
return; //Comment this to enable sound wave rendering
graphics.Clear(Color::White);
graphics.DrawLine(&axis, 0, scrH / 2, scrW, scrH / 2);
graphics.DrawLine(&axis, scrW / 2, 0, scrW / 2, scrH);
int bps = (w.getH()->wBitsPerSample / 8);
byte* u = (byte*)mem->p;
int px = 0;
double py;
int newy;
for (unsigned int i = 0; i < mem->nBytes; i+=w.getH()->nChannels*bps)
{
switch (bps)
{
case 1:
newy = (int)(scrH / 2 + scaleRange(*(int8_t*)(u + i), INT8_MIN, INT8_MAX, -scrH/2, scrH/2));
break;
case 2:
newy = (int)(scrH / 2 + scaleRange(*(int16_t*)(u + i), INT16_MIN, INT16_MAX, -scrH / 2, scrH / 2));
break;
case 3:
newy = (int)(scrH / 2 + scaleRange((*(int24*)(u + i)).data, -pow(2, 23), pow(2, 23), -scrH / 2, scrH / 2));
break;
case 4:
newy = (int)(scrH / 2 + scaleRange(*(int32_t*)(u + i), INT32_MIN, INT32_MAX, -scrH / 2, scrH / 2));
break;
}
newy += scrH / 2;
if (i == 0) py = newy;
graphics.DrawLine(&pen, px, (int)py, px + 1, newy);
py = newy;
px++;
}
//memblock bpmmem;
//bpmmem.p = (uintptr_t)w.get_data_p();
//bpmmem.nBytes = w.get_data_size();
//int bpm = w.detectBPM(bpmmem, 60, 180, 10);
//std::wstringstream sstream;
//sstream << bpm;
//MessageBox(hWnd, sstream.str().c_str(), L"BPM", MB_SYSTEMMODAL);
//w.DFTWindow(*mem, Wave::DFTWindowType::Hanning);
//DFTransform* dft = w.DFT(*mem);
//DFTransform::DFTChannelResult* dftr;
// FFTransform* fft = w.FFT(*mem);
//
// FFTransform::DFTChannelResult* dftr;
//
// int prev = 0;
// int previ = 0;
//
// for (int i = 0; i < scrW; i++)
// {
// //dftr = &(dft->next(DFTransform::nextResult::STEREO)->stereo);
// dftr = &(fft->next(FFTransform::nextResult::STEREO)->stereo);
// if (dftr == nullptr) return;
// if (i == 1)
// {
// freqdiff = dftr->freq;
//
// }
//
//
// //Spectral magnitude in dB
// //even function
// int val = 1 * (dftr->dbmag);
// if (i != 0)graphics.DrawLine(&power, i - 1, prev, i, scrH/2 - val);
// else graphics.DrawLine(&power, i - 1, scrH/2 - val, i, scrH/2 - val);
// prev = scrH/2 - val;
//
// //angle
// //odd function
// int vali = radiansToDegrees(dftr->angle);
// if (i != 0)graphics.DrawLine(&green, i - 1, previ, i, scrH / 2 - vali);
// else graphics.DrawLine(&green, i - 1, scrH / 2 - previ, i, scrH / 2 - vali);
// previ = scrH / 2 - vali;
//
///*
// graphics.DrawLine(&power, i, scrH, i, scrH - 100*dftr->real);
// graphics.DrawLine(&green, i, scrH, i, scrH - 100*dftr->imag);*/
// }
// delete fft;
// //delete dft;
}
//sets the input value, and
//calculates the output value based on slider and cuttoff settings
//value should ALWAYS be between 0 and 1
void SensorOutput::setValue(double val) {
//DEBUG_PRINT_NUM("SensorOutput::setValue ", val)
//invert
if (isInvert)
val = 1.0 - val;
//save cur raw val
_inputValue = val;
_outputValue = val;
//calculate values!!!!!!
//if (inRange == NULL)
// return;
// scale raw data into acceptable values between 0 and 1.
_outputValue = scaleRange(_outputValue, inRange.low, inRange.high, 0., 1.);
// boost that f****r
if (isLogarithmic) {
_outputValue = log10(_outputValue) + 1.0;
}
// now cut it down
if (cutoffTypeVal == CUTOFF_TYPE_VAL_HARD) {
if (_outputValue > cutoffRange.high)
_outputValue = 1.0;
else if (_outputValue < cutoffRange.low)
_outputValue = 0.0;
}
else if ( cutoffTypeVal == (CUTOFF_TYPE_VAL_NULLABLE) ||
cutoffTypeVal == (CUTOFF_TYPE_VAL_NULLABLE_LOW)) {
if (_outputValue > cutoffRange.high ) {
// special case if we want to capture the hard hits
if (cutoffTypeVal == (CUTOFF_TYPE_VAL_NULLABLE_LOW)) {
_outputValue = cutoffRange.high;
}
else {
_outputValue = SensorizerServer::SENSOR_VALUE_NULL;
}
}
else if (_outputValue < cutoffRange.low)
_outputValue = SensorizerServer::SENSOR_VALUE_NULL;
}
else if (cutoffTypeVal == (CUTOFF_TYPE_VAL_CLIP)) {
if (_outputValue > cutoffRange.high)
_outputValue = cutoffRange.high;
else if (_outputValue < cutoffRange.low)
_outputValue = cutoffRange.low;
}
else { //NONE
}
//DEBUG_PRINT_NUM("SensorOutput::.outputFiltersCurlength ", outputFiltersCurlength)
//synchronized(outputFilters) {
for (int i = 0; i < outputFiltersCurlength; i++) {
//if (outputFilters[i] != NULL) {
outputFilters[i]->setValue(_outputValue);
_outputValue = outputFilters[i]->value();
//}
}
//}
//TODO: equaitons should go here but out meters need to accept any num
//EQUATIONS
if (_outputValue != SensorizerServer::SENSOR_VALUE_NULL) {
_outputValue = scaleRange(_outputValue, 0., 1., outRange.low, outRange.high);
// for midi, it doesn't make sense to go above 1, but we'll leave this here for legacy support.
_outputValue = _outputValue * multiplyVal + addVal;
}
//DEBUG_PRINT_NUM("DONE SensorOutput::setValue ", _outputValue)
}
//scales a number to fit within the new extrema.
double SensorOutput::scaleRange(double num, double oldMin, double oldMax, double newMin, double newMax) {
return scaleRange(num, oldMin, oldMax, newMin, newMax, false);
}
STATIC_UNIT_TESTED void servoMixer(void)
{
int16_t input[INPUT_SOURCE_COUNT]; // Range [-500:+500]
static int16_t currentOutput[MAX_SERVO_RULES];
uint8_t i;
if (FLIGHT_MODE(PASSTHRU_MODE)) {
// Direct passthru from RX
input[INPUT_STABILIZED_ROLL] = rcCommand[ROLL];
input[INPUT_STABILIZED_PITCH] = rcCommand[PITCH];
input[INPUT_STABILIZED_YAW] = rcCommand[YAW];
} else {
// Assisted modes (gyro only or gyro+acc according to AUX configuration in Gui
input[INPUT_STABILIZED_ROLL] = axisPID[ROLL];
input[INPUT_STABILIZED_PITCH] = axisPID[PITCH];
input[INPUT_STABILIZED_YAW] = axisPID[YAW];
// Reverse yaw servo when inverted in 3D mode
if (feature(FEATURE_3D) && (rcData[THROTTLE] < rxConfig->midrc)) {
input[INPUT_STABILIZED_YAW] *= -1;
}
}
input[INPUT_GIMBAL_PITCH] = scaleRange(attitude.values.pitch, -1800, 1800, -500, +500);
input[INPUT_GIMBAL_ROLL] = scaleRange(attitude.values.roll, -1800, 1800, -500, +500);
input[INPUT_STABILIZED_THROTTLE] = motor[0] - 1000 - 500; // Since it derives from rcCommand or mincommand and must be [-500:+500]
// center the RC input value around the RC middle value
// by subtracting the RC middle value from the RC input value, we get:
// data - middle = input
// 2000 - 1500 = +500
// 1500 - 1500 = 0
// 1000 - 1500 = -500
input[INPUT_RC_ROLL] = rcData[ROLL] - rxConfig->midrc;
input[INPUT_RC_PITCH] = rcData[PITCH] - rxConfig->midrc;
input[INPUT_RC_YAW] = rcData[YAW] - rxConfig->midrc;
input[INPUT_RC_THROTTLE] = rcData[THROTTLE] - rxConfig->midrc;
input[INPUT_RC_AUX1] = rcData[AUX1] - rxConfig->midrc;
input[INPUT_RC_AUX2] = rcData[AUX2] - rxConfig->midrc;
input[INPUT_RC_AUX3] = rcData[AUX3] - rxConfig->midrc;
input[INPUT_RC_AUX4] = rcData[AUX4] - rxConfig->midrc;
for (i = 0; i < MAX_SUPPORTED_SERVOS; i++)
servo[i] = 0;
// mix servos according to rules
for (i = 0; i < servoRuleCount; i++) {
// consider rule if no box assigned or box is active
if (currentServoMixer[i].box == 0 || IS_RC_MODE_ACTIVE(BOXSERVO1 + currentServoMixer[i].box - 1)) {
uint8_t target = currentServoMixer[i].targetChannel;
uint8_t from = currentServoMixer[i].inputSource;
uint16_t servo_width = servoConf[target].max - servoConf[target].min;
int16_t min = currentServoMixer[i].min * servo_width / 100 - servo_width / 2;
int16_t max = currentServoMixer[i].max * servo_width / 100 - servo_width / 2;
if (currentServoMixer[i].speed == 0)
currentOutput[i] = input[from];
else {
if (currentOutput[i] < input[from])
currentOutput[i] = constrain(currentOutput[i] + currentServoMixer[i].speed, currentOutput[i], input[from]);
else if (currentOutput[i] > input[from])
currentOutput[i] = constrain(currentOutput[i] - currentServoMixer[i].speed, input[from], currentOutput[i]);
}
servo[target] += servoDirection(target, from) * constrain(((int32_t)currentOutput[i] * currentServoMixer[i].rate) / 100, min, max);
} else {
currentOutput[i] = 0;
}
}
for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
servo[i] = ((int32_t)servoConf[i].rate * servo[i]) / 100L;
servo[i] += determineServoMiddleOrForwardFromChannel(i);
}
}
void updateLedStrip(void)
{
if (!(ledStripInitialised && isWS2811LedStripReady())) {
return;
}
if (IS_RC_MODE_ACTIVE(BOXLEDLOW)) {
if (ledStripEnabled) {
ledStripDisable();
ledStripEnabled = false;
}
} else {
ledStripEnabled = true;
}
if (!ledStripEnabled){
return;
}
uint32_t now = micros();
bool indicatorFlashNow = (int32_t)(now - nextIndicatorFlashAt) >= 0L;
bool warningFlashNow = (int32_t)(now - nextWarningFlashAt) >= 0L;
bool rotationUpdateNow = (int32_t)(now - nextRotationUpdateAt) >= 0L;
#ifdef USE_LED_ANIMATION
bool animationUpdateNow = (int32_t)(now - nextAnimationUpdateAt) >= 0L;
#endif
if (!(
indicatorFlashNow ||
rotationUpdateNow ||
warningFlashNow
#ifdef USE_LED_ANIMATION
|| animationUpdateNow
#endif
)) {
return;
}
static uint8_t indicatorFlashState = 0;
// LAYER 1
applyLedModeLayer();
applyLedThrottleLayer();
// LAYER 2
if (warningFlashNow) {
nextWarningFlashAt = now + LED_STRIP_10HZ;
}
applyLedWarningLayer(warningFlashNow);
// LAYER 3
if (indicatorFlashNow) {
uint8_t rollScale = ABS(rcCommand[ROLL]) / 50;
uint8_t pitchScale = ABS(rcCommand[PITCH]) / 50;
uint8_t scale = MAX(rollScale, pitchScale);
nextIndicatorFlashAt = now + (LED_STRIP_5HZ / MAX(1, scale));
if (indicatorFlashState == 0) {
indicatorFlashState = 1;
} else {
indicatorFlashState = 0;
}
}
applyLedIndicatorLayer(indicatorFlashState);
#ifdef USE_LED_ANIMATION
if (animationUpdateNow) {
nextAnimationUpdateAt = now + LED_STRIP_20HZ;
updateLedAnimationState();
}
applyLedAnimationLayer();
#endif
if (rotationUpdateNow) {
applyLedThrustRingLayer();
uint8_t animationSpeedScale = 1;
if (ARMING_FLAG(ARMED)) {
animationSpeedScale = scaleRange(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX, 1, 10);
}
nextRotationUpdateAt = now + LED_STRIP_5HZ/animationSpeedScale;
}
ws2811UpdateStrip();
}
void mavlinkSendHUDAndHeartbeat(void)
{
uint16_t msgLength;
float mavAltitude = 0;
float mavGroundSpeed = 0;
float mavAirSpeed = 0;
float mavClimbRate = 0;
#if defined(GPS)
// use ground speed if source available
if (sensors(SENSOR_GPS)) {
mavGroundSpeed = GPS_speed / 100.0f;
}
#endif
// select best source for altitude
#if defined(BARO) || defined(SONAR)
if (sensors(SENSOR_SONAR) || sensors(SENSOR_BARO)) {
// Baro or sonar generally is a better estimate of altitude than GPS MSL altitude
mavAltitude = altitudeHoldGetEstimatedAltitude() / 100.0;
}
#if defined(GPS)
else if (sensors(SENSOR_GPS)) {
// No sonar or baro, just display altitude above MLS
mavAltitude = GPS_altitude;
}
#endif
#elif defined(GPS)
if (sensors(SENSOR_GPS)) {
// No sonar or baro, just display altitude above MLS
mavAltitude = GPS_altitude;
}
#endif
mavlink_msg_vfr_hud_pack(0, 200, &mavMsg,
// airspeed Current airspeed in m/s
mavAirSpeed,
// groundspeed Current ground speed in m/s
mavGroundSpeed,
// heading Current heading in degrees, in compass units (0..360, 0=north)
DECIDEGREES_TO_DEGREES(attitude.values.yaw),
// throttle Current throttle setting in integer percent, 0 to 100
scaleRange(constrain(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX), PWM_RANGE_MIN, PWM_RANGE_MAX, 0, 100),
// alt Current altitude (MSL), in meters, if we have sonar or baro use them, otherwise use GPS (less accurate)
mavAltitude,
// climb Current climb rate in meters/second
mavClimbRate);
msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavMsg);
mavlinkSerialWrite(mavBuffer, msgLength);
uint8_t mavModes = MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
if (ARMING_FLAG(ARMED))
mavModes |= MAV_MODE_FLAG_SAFETY_ARMED;
uint8_t mavSystemType;
switch(mixerConfig()->mixerMode)
{
case MIXER_TRI:
mavSystemType = MAV_TYPE_TRICOPTER;
break;
case MIXER_QUADP:
case MIXER_QUADX:
case MIXER_Y4:
case MIXER_VTAIL4:
mavSystemType = MAV_TYPE_QUADROTOR;
break;
case MIXER_Y6:
case MIXER_HEX6:
case MIXER_HEX6X:
mavSystemType = MAV_TYPE_HEXAROTOR;
break;
case MIXER_OCTOX8:
case MIXER_OCTOFLATP:
case MIXER_OCTOFLATX:
mavSystemType = MAV_TYPE_OCTOROTOR;
break;
case MIXER_FLYING_WING:
case MIXER_AIRPLANE:
case MIXER_CUSTOM_AIRPLANE:
mavSystemType = MAV_TYPE_FIXED_WING;
break;
case MIXER_HELI_120_CCPM:
case MIXER_HELI_90_DEG:
mavSystemType = MAV_TYPE_HELICOPTER;
break;
default:
mavSystemType = MAV_TYPE_GENERIC;
break;
}
// Custom mode for compatibility with APM OSDs
uint8_t mavCustomMode = 1; // Acro by default
if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {
mavCustomMode = 0; //Stabilize
mavModes |= MAV_MODE_FLAG_STABILIZE_ENABLED;
}
if (FLIGHT_MODE(BARO_MODE) || FLIGHT_MODE(SONAR_MODE))
mavCustomMode = 2; //Alt Hold
if (FLIGHT_MODE(GPS_HOME_MODE))
mavCustomMode = 6; //Return to Launch
if (FLIGHT_MODE(GPS_HOLD_MODE))
mavCustomMode = 16; //Position Hold (Earlier called Hybrid)
uint8_t mavSystemState = 0;
if (ARMING_FLAG(ARMED)) {
if (failsafeIsActive()) {
mavSystemState = MAV_STATE_CRITICAL;
}
else {
mavSystemState = MAV_STATE_ACTIVE;
}
}
else {
mavSystemState = MAV_STATE_STANDBY;
}
mavlink_msg_heartbeat_pack(0, 200, &mavMsg,
// type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM)
mavSystemType,
// autopilot Autopilot type / class. defined in MAV_AUTOPILOT ENUM
MAV_AUTOPILOT_GENERIC,
// base_mode System mode bitfield, see MAV_MODE_FLAGS ENUM in mavlink/include/mavlink_types.h
mavModes,
// custom_mode A bitfield for use for autopilot-specific flags.
mavCustomMode,
// system_status System status flag, see MAV_STATE ENUM
mavSystemState);
msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavMsg);
mavlinkSerialWrite(mavBuffer, msgLength);
}
static void applyLedFixedLayers(void)
{
for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) {
const ledConfig_t *ledConfig = &ledStripConfig()->ledConfigs[ledIndex];
hsvColor_t color = *getSC(LED_SCOLOR_BACKGROUND);
int fn = ledGetFunction(ledConfig);
int hOffset = HSV_HUE_MAX + 1;
switch (fn) {
case LED_FUNCTION_COLOR:
color = ledStripConfig()->colors[ledGetColor(ledConfig)];
hsvColor_t nextColor = ledStripConfig()->colors[(ledGetColor(ledConfig) + 1 + LED_CONFIGURABLE_COLOR_COUNT) % LED_CONFIGURABLE_COLOR_COUNT];
hsvColor_t previousColor = ledStripConfig()->colors[(ledGetColor(ledConfig) - 1 + LED_CONFIGURABLE_COLOR_COUNT) % LED_CONFIGURABLE_COLOR_COUNT];
if (ledGetOverlayBit(ledConfig, LED_OVERLAY_THROTTLE)) { //smooth fade with selected Aux channel of all HSV values from previousColor through color to nextColor
int centerPWM = (PWM_RANGE_MIN + PWM_RANGE_MAX) / 2;
if (auxInput < centerPWM) {
color.h = scaleRange(auxInput, PWM_RANGE_MIN, centerPWM, previousColor.h, color.h);
color.s = scaleRange(auxInput, PWM_RANGE_MIN, centerPWM, previousColor.s, color.s);
color.v = scaleRange(auxInput, PWM_RANGE_MIN, centerPWM, previousColor.v, color.v);
} else {
color.h = scaleRange(auxInput, centerPWM, PWM_RANGE_MAX, color.h, nextColor.h);
color.s = scaleRange(auxInput, centerPWM, PWM_RANGE_MAX, color.s, nextColor.s);
color.v = scaleRange(auxInput, centerPWM, PWM_RANGE_MAX, color.v, nextColor.v);
}
}
break;
case LED_FUNCTION_FLIGHT_MODE:
for (unsigned i = 0; i < ARRAYLEN(flightModeToLed); i++)
if (!flightModeToLed[i].flightMode || FLIGHT_MODE(flightModeToLed[i].flightMode)) {
const hsvColor_t *directionalColor = getDirectionalModeColor(ledIndex, &ledStripConfig()->modeColors[flightModeToLed[i].ledMode]);
if (directionalColor) {
color = *directionalColor;
}
break; // stop on first match
}
break;
case LED_FUNCTION_ARM_STATE:
color = ARMING_FLAG(ARMED) ? *getSC(LED_SCOLOR_ARMED) : *getSC(LED_SCOLOR_DISARMED);
break;
case LED_FUNCTION_BATTERY:
color = HSV(RED);
hOffset += scaleRange(calculateBatteryPercentageRemaining(), 0, 100, -30, 120);
break;
case LED_FUNCTION_RSSI:
color = HSV(RED);
hOffset += scaleRange(getRssi() * 100, 0, 1023, -30, 120);
break;
default:
break;
}
if ((fn != LED_FUNCTION_COLOR) && ledGetOverlayBit(ledConfig, LED_OVERLAY_THROTTLE)) {
hOffset += scaleRange(auxInput, PWM_RANGE_MIN, PWM_RANGE_MAX, 0, HSV_HUE_MAX + 1);
}
color.h = (color.h + hOffset) % (HSV_HUE_MAX + 1);
setLedHsv(ledIndex, &color);
}
}
void Animator::readAndSetColour(uint16_t ledIndex) {
uint8_t red = animationReader->readUnsignedByte();
uint8_t green = animationReader->readUnsignedByte();
uint8_t blue = animationReader->readUnsignedByte();
#ifdef DEBUG_ANIMATOR_CODEC_LED_COLOURS
if (DEBUG_LED_INDEX_TEST) {
Serial.print("led colors (ledIndex) (r,g,b): (");
Serial.print(ledIndex, DEC);
Serial.print(") (0x");
Serial.print(red, HEX);
Serial.print(",0x");
Serial.print(green, HEX);
Serial.print(",0x");
Serial.print(blue, HEX);
Serial.println(")");
}
#endif
#ifdef HACK_MIRROR_LEDS
uint8_t row = ledIndex / (LEDS_PER_STRIP / 2);
uint8_t column = ledIndex % (LEDS_PER_STRIP / 2);
uint16_t mirrorIndex = (row * LEDS_PER_STRIP) + column;
#endif
if (hasBackgroundColour &&
(
red == backgroundColourRed &&
green == backgroundColourGreen &&
blue == backgroundColourBlue
)
) {
#ifdef HACK_MIRROR_LEDS
leds.setPixel(mirrorIndex, red, green, blue);
leds.setPixel(mirrorIndex + (LEDS_PER_STRIP / 2), red, green, blue);
#else
leds.setPixel(ledIndex, red, green, blue);
#endif
return;
}
int32_t redIncrement = 0;
int32_t greenIncrement = 0;
int32_t blueIncrement = 0;
for (uint8_t valueAxisIndex = 0; valueAxisIndex < valueAxisCount; valueAxisIndex++) {
ValueAxis *currentValueAxis = valueAxes[valueAxisIndex];
int8_t valueAxisPosition = determineValueAxisPosition(currentValueAxis, valueAxisIndex);
int8_t start = 0;
int8_t end = 0;
if (valueAxisPosition < 0) {
start = valueAxisPosition;
end = currentValueAxis->valueAxisCentreValue;
} else if (valueAxisPosition > 0) {
start = currentValueAxis->valueAxisCentreValue + 1;
end = valueAxisPosition + 1;
}
#ifdef DEBUG_ANIMATOR_CODEC_VALUE_AXIS
if (DEBUG_LED_INDEX_TEST) {
Serial.print("led valueAxis (index,position) (start,end): (");
Serial.print(valueAxisIndex, DEC);
Serial.print(",");
Serial.print(valueAxisPosition, DEC);
Serial.print(") (");
Serial.print(start, DEC);
Serial.print(",");
Serial.print(end, DEC);
Serial.println(")");
Serial.print("led increments (functionIndex) (r,g,b): ");
}
#endif
for (int8_t valueAxisValue = start; valueAxisValue < end; valueAxisValue++) {
uint8_t functionIndex = currentValueAxis->retrieveFunctionIndex(ledIndex, valueAxisValue);
int32_t *componentFunctionData = functionData[functionIndex];
redIncrement += componentFunctionData[0];
greenIncrement += componentFunctionData[1];
blueIncrement += componentFunctionData[2];
#ifdef DEBUG_ANIMATOR_CODEC_VALUE_AXIS
if (DEBUG_LED_INDEX_TEST) {
if (valueAxisValue != start) {
Serial.print(", ");
}
Serial.print("(");
Serial.print(functionIndex, DEC);
Serial.print(") (");
Serial.print(redIncrement, DEC);
Serial.print(",");
Serial.print(greenIncrement, DEC);
Serial.print(",");
Serial.print(blueIncrement, DEC);
Serial.print(")");
}
#endif
}
#ifdef DEBUG_ANIMATOR_CODEC_VALUE_AXIS
if (DEBUG_LED_INDEX_TEST) {
Serial.println();
}
#endif
#ifdef DEBUG_ANIMATOR_CODEC_FINAL_INCREMENTS
if (DEBUG_LED_INDEX_TEST) {
Serial.print("led pre-fixed increments (r,g,b): (");
Serial.print(redIncrement, DEC);
Serial.print(",");
Serial.print(greenIncrement, DEC);
Serial.print(",");
Serial.print(blueIncrement, DEC);
Serial.println(")");
}
#endif
redIncrement = fixIncrement(redIncrement);
greenIncrement = fixIncrement(greenIncrement);
blueIncrement = fixIncrement(blueIncrement);
#ifdef DEBUG_ANIMATOR_CODEC_FINAL_INCREMENTS
if (DEBUG_LED_INDEX_TEST) {
Serial.print("led post-fixed increments (r,g,b): (");
Serial.print(redIncrement, DEC);
Serial.print(",");
Serial.print(greenIncrement, DEC);
Serial.print(",");
Serial.print(blueIncrement, DEC);
Serial.println(")");
}
#endif
}
red = applyIncrement(red, redIncrement);
green = applyIncrement(green, greenIncrement);
blue = applyIncrement(blue, blueIncrement);
#ifdef DEBUG_ANIMATOR_CODEC_LED_COLOURS
if (DEBUG_LED_INDEX_TEST) {
Serial.print("led final color: (ledIndex) (r,g,b): (");
Serial.print(ledIndex, DEC);
Serial.print(") (0x");
Serial.print(red, HEX);
Serial.print(",0x");
Serial.print(green, HEX);
Serial.print(",0x");
Serial.print(blue, HEX);
Serial.println(")");
}
#endif
#ifdef APPLY_BRIGHTNESS_HACK
red = scaleRange(red, 0x00, 0xff, 0x00, BRIGHTNESS_MAX);
green = scaleRange(green, 0x00, 0xff, 0x00, BRIGHTNESS_MAX);
blue = scaleRange(blue, 0x00, 0xff, 0x00, BRIGHTNESS_MAX);
#endif
#ifdef HACK_MIRROR_LEDS
leds.setPixel(mirrorIndex, red, green, blue);
leds.setPixel(mirrorIndex + (LEDS_PER_STRIP / 2), red, green, blue);
#else
leds.setPixel(ledIndex, red, green, blue);
#endif
}
