@ -19,6 +19,9 @@ uint16_t rcCommand[RC_CHANS]; // Signal to ESC
uint16_t rcData [ RC_CHANS ] ; // Signal from nRF24L01+
int16_t rcSerial [ 8 ] ; // Signal from Serial Port
uint16_t failsafeCnt = 0 ; // To trigger failsave if any issues
int16_t lookupPitchRollRC [ 5 ] ; // lookup table for expo & RC rate PITCH+ROLL
uint16_t lookupThrottleRC [ 11 ] ; // lookup table for expo & mid THROTTLE
int16_t gyroADCprevious [ 3 ] = { 0 , 0 , 0 } ;
int16_t gyroADCinter [ 3 ] ;
uint32_t timeInterleave = 0 ;
@ -507,7 +510,7 @@ void run_tasks(void) {
process_rc ( ) ;
} else {
// Not in RC Loop
process_module s ( ) ;
process_task s ( ) ;
}
// Loop time
@ -568,7 +571,7 @@ void run_tasks(void) {
}
}
# if BARO && (!defined(SURPRESS_BARO_ALTHOLD))
# if USE_ BARO && (!defined(SURPRESS_BARO_ALTHOLD))
/* Smooth alt change routing for slow auto aerophoto modes. It is slowly increase/decrease altitude
* proportional to stick movement ( + / - 100 throttle gives about + / - 50 cm in 1 second with cycle time about 3 - 4 ms )
*/
@ -810,6 +813,7 @@ void run_tasks(void) {
# endif
* * * * * * * * * * * * * * * * * * * */
// TODO: Send to DSHOTS
///// writeMotors();
}
@ -880,6 +884,46 @@ void compute_imu(void) {
acc_common ( ) ;
GYRO_ORIENTATION ( GyX , GyY , GyZ ) ; // Range +/- 8192; +/- 2000 deg/sec?
gyro_common ( ) ;
for ( axis = 0 ; axis < 3 ; axis + + )
gyroADCinter [ axis ] = imu . gyroADC [ axis ] ;
timeInterleave = micros ( ) ;
annexCode ( ) ;
uint8_t t = 0 ;
while ( ( int16_t ) ( micros ( ) - timeInterleave ) < 650 )
t = 1 ; // empirical, interleaving delay between 2 consecutive reads
# ifdef LCD_TELEMETRY
if ( ! t ) annex650_overrun_count + + ;
# endif
GYRO_ORIENTATION ( GyX , GyY , GyZ ) ; // Range +/- 8192; +/- 2000 deg/sec?
gyro_common ( ) ;
for ( axis = 0 ; axis < 3 ; axis + + ) {
gyroADCinter [ axis ] = imu . gyroADC [ axis ] + gyroADCinter [ axis ] ;
// empirical, we take a weighted value of the current and the previous values
imu . gyroData [ axis ] = ( gyroADCinter [ axis ] + gyroADCprevious [ axis ] ) / 3 ;
gyroADCprevious [ axis ] = gyroADCinter [ axis ] > > 1 ;
if ( ! USE_ACC )
imu . accADC [ axis ] = 0 ;
}
# if defined(GYRO_SMOOTHING)
static int16_t gyroSmooth [ 3 ] = { 0 , 0 , 0 } ;
for ( axis = 0 ; axis < 3 ; axis + + ) {
imu . gyroData [ axis ] = ( int16_t ) ( ( ( int32_t ) ( ( int32_t ) gyroSmooth [ axis ] * ( conf . Smoothing [ axis ] - 1 ) ) + imu . gyroData [ axis ] + 1 ) / conf . Smoothing [ axis ] ) ;
gyroSmooth [ axis ] = imu . gyroData [ axis ] ;
}
# elif defined(TRI)
static int16_t gyroYawSmooth = 0 ;
imu . gyroData [ YAW ] = ( gyroYawSmooth * 2 + imu . gyroData [ YAW ] ) / 3 ;
gyroYawSmooth = imu . gyroData [ YAW ] ;
# endif
}
@ -1050,7 +1094,53 @@ void acc_common(void) {
}
void process_modules ( void ) {
void process_tasks ( void ) {
static uint8_t taskOrder = 0 ; // never call all functions in the same loop to avail high delay spikes
switch ( taskOrder ) {
case 0 :
taskOrder + + ;
# if USE_MAG
// if(mag_getadc() != 0) break; // 320us
# endif
break ;
case 1 :
taskOrder + + ;
# if USE_BARO
if ( baro . baro_update ( ) ! = 0 )
break ; // pressure and temperature computation 160us
# endif
break ;
case 2 :
# if USE_BARO
if ( baro . getEstimatedAltitude ( ) ! = 0 )
break ; // 280us
# endif
break ;
case 3 :
taskOrder + + ;
// TODO: GPS will be migrated in once the flight is workable. Not focusing on basic flight logics.
# if GPS
// if(GPS_Compute() != 0) break; // performs computation on new frame only if present
# endif
break ;
case 4 :
taskOrder + + ;
// TODO: SONAR will be migrated in once the flight is workable. Not focusing on basic flight logics.
# if USE_SONAR
// Sonar_update();
# endif
# ifdef LANDING_LIGHTS_DDR
// auto_switch_landing_lights();
# endif
# ifdef VARIOMETER
// if (f.VARIO_MODE) vario_signaling();
# endif
break ;
}
}
@ -1072,4 +1162,299 @@ void writeGlobalSet(uint8_t b) {
}
// this code is excetuted at each loop and won't interfere with control loop if it lasts less than 650 microseconds
void annexCode ( void ) {
static uint32_t calibratedAccTime ;
uint16_t tmp , tmp2 ;
uint8_t axis , prop1 , prop2 ;
// PITCH & ROLL only dynamic PID adjustemnt, depending on throttle value (or collective.pitch value for heli)
# ifdef HELICOPTER
# define DYN_THR_PID_CHANNEL COLLECTIVE_PITCH
# else
# define DYN_THR_PID_CHANNEL THROTTLE
# endif
prop2 = 128 ; // prop2 was 100, is 128 now
if ( rcData [ DYN_THR_PID_CHANNEL ] > 1500 ) { // breakpoint is fix: 1500
if ( rcData [ DYN_THR_PID_CHANNEL ] < 2000 ) {
prop2 - = ( ( uint16_t ) conf . dynThrPID * ( rcData [ DYN_THR_PID_CHANNEL ] - 1500 ) > > 9 ) ; // /512 instead of /500
} else {
prop2 - = conf . dynThrPID ;
}
}
for ( axis = 0 ; axis < 3 ; axis + + ) {
tmp = min ( abs ( rcData [ axis ] - MIDRC ) , 500 ) ;
# if defined(DEADBAND)
if ( tmp > DEADBAND ) {
tmp - = DEADBAND ;
} else {
tmp = 0 ;
}
# endif
if ( axis ! = 2 ) { // ROLL & PITCH
tmp2 = tmp > > 7 ; // 500/128 = 3.9 => range [0;3]
rcCommand [ axis ] = lookupPitchRollRC [ tmp2 ] + ( ( tmp - ( tmp2 < < 7 ) ) * ( lookupPitchRollRC [ tmp2 + 1 ] - lookupPitchRollRC [ tmp2 ] ) > > 7 ) ;
prop1 = 128 - ( ( uint16_t ) conf . rollPitchRate * tmp > > 9 ) ; // prop1 was 100, is 128 now -- and /512 instead of /500
prop1 = ( uint16_t ) prop1 * prop2 > > 7 ; // prop1: max is 128 prop2: max is 128 result prop1: max is 128
dynP8 [ axis ] = ( uint16_t ) conf . pid [ axis ] . P8 * prop1 > > 7 ; // was /100, is /128 now
dynD8 [ axis ] = ( uint16_t ) conf . pid [ axis ] . D8 * prop1 > > 7 ; // was /100, is /128 now
} else { // YAW
rcCommand [ axis ] = tmp ;
}
if ( rcData [ axis ] < MIDRC )
rcCommand [ axis ] = - rcCommand [ axis ] ;
}
tmp = constrain ( rcData [ THROTTLE ] , MINCHECK , 2000 ) ;
tmp = ( uint32_t ) ( tmp - MINCHECK ) * 2559 / ( 2000 - MINCHECK ) ; // [MINCHECK;2000] -> [0;2559]
tmp2 = tmp / 256 ; // range [0;9]
rcCommand [ THROTTLE ] = lookupThrottleRC [ tmp2 ] + ( tmp - tmp2 * 256 ) * ( lookupThrottleRC [ tmp2 + 1 ] - lookupThrottleRC [ tmp2 ] ) / 256 ;
// [0;2559] -> expo -> [conf.minthrottle;MAXTHROTTLE]
# if defined(HEADFREE)
if ( flags . HEADFREE_MODE ) { // to optimize
float radDiff = ( att . heading - headFreeModeHold ) * 0.0174533f ; // where PI/180 ~= 0.0174533
float cosDiff = cos ( radDiff ) ;
float sinDiff = sin ( radDiff ) ;
int16_t rcCommand_PITCH = rcCommand [ PITCH ] * cosDiff + rcCommand [ ROLL ] * sinDiff ;
rcCommand [ ROLL ] = rcCommand [ ROLL ] * cosDiff - rcCommand [ PITCH ] * sinDiff ;
rcCommand [ PITCH ] = rcCommand_PITCH ;
}
# endif
// query at most one multiplexed analog channel per MWii cycle
static uint8_t analogReader = 0 ;
switch ( analogReader + + % ( 3 + VBAT_CELLS_NUM ) ) {
case 0 :
# if defined(POWERMETER_HARD)
static uint32_t lastRead = currentTime ;
static uint8_t ind = 0 ;
static uint16_t pvec [ PSENSOR_SMOOTH ] , psum ;
uint16_t p = analogRead ( PSENSORPIN ) ;
//LCDprintInt16(p); LCDcrlf();
//debug[0] = p;
# if PSENSOR_SMOOTH != 1
psum + = p ;
psum - = pvec [ ind ] ;
pvec [ ind + + ] = p ;
ind % = PSENSOR_SMOOTH ;
p = psum / PSENSOR_SMOOTH ;
# endif // PSENSOR_SMOOTH
powerValue = ( conf . psensornull > p ? conf . psensornull - p : p - conf . psensornull ) ; // do not use abs(), it would induce implicit cast to uint and overrun
analog . amperage = ( ( uint32_t ) powerValue * conf . pint2ma ) / 100 ; // [100mA] old (will overflow for 65A: powerValue * conf.pint2ma; // [1mA]
pMeter [ PMOTOR_SUM ] + = ( ( currentTime - lastRead ) * ( uint32_t ) ( ( uint32_t ) powerValue * conf . pint2ma ) ) / 100000 ;
// [10 mA * msec]
lastRead = currentTime ;
# endif // POWERMETER_HARD
break ;
case 1 :
# if defined(VBAT) && !defined(VBAT_CELLS)
static uint8_t ind = 0 ;
static uint16_t vvec [ VBAT_SMOOTH ] , vsum ;
uint16_t v = analogRead ( V_BATPIN ) ;
# if VBAT_SMOOTH == 1
analog . vbat = ( v * VBAT_PRESCALER ) / conf . vbatscale + VBAT_OFFSET ; // result is Vbatt in 0.1V steps
# else
vsum + = v ;
vsum - = vvec [ ind ] ;
vvec [ ind + + ] = v ;
ind % = VBAT_SMOOTH ;
# if VBAT_SMOOTH == VBAT_PRESCALER
analog . vbat = vsum / conf . vbatscale + VBAT_OFFSET ; // result is Vbatt in 0.1V steps
# elif VBAT_SMOOTH < VBAT_PRESCALER
analog . vbat = ( vsum * ( VBAT_PRESCALER / VBAT_SMOOTH ) ) / conf . vbatscale + VBAT_OFFSET ; // result is Vbatt in 0.1V steps
# else
analog . vbat = ( ( vsum / VBAT_SMOOTH ) * VBAT_PRESCALER ) / conf . vbatscale + VBAT_OFFSET ; // result is Vbatt in 0.1V steps
# endif // VBAT_SMOOTH == VBAT_PRESCALER
# endif // VBAT_SMOOTH == 1
# endif // defined(VBAT)
break ;
case 2 :
# if defined(RX_RSSI)
static uint8_t ind = 0 ;
static uint16_t rvec [ RSSI_SMOOTH ] , rsum , r ;
// http://www.multiwii.com/forum/viewtopic.php?f=8&t=5530
// TODO: Since all RSSI is internal, read from the internal values later
# if defined(RX_RSSI_CHAN)
uint16_t rssi_Input = constrain ( rcData [ RX_RSSI_CHAN ] , 1000 , 2000 ) ;
r = map ( ( uint16_t ) rssi_Input , 1000 , 2000 , 0 , 1023 ) ;
# else
r = analogRead ( RX_RSSI_PIN ) ;
# endif // defined(RX_RSSI_CHAN)
# if RSSI_SMOOTH == 1
analog . rssi = r ;
# else
rsum + = r ;
rsum - = rvec [ ind ] ;
rvec [ ind + + ] = r ;
ind % = RSSI_SMOOTH ;
r = rsum / RSSI_SMOOTH ;
analog . rssi = r ;
# endif // RSSI_SMOOTH == 1
# endif // RX RSSI
break ;
default : // here analogReader >=4, because of ++ in switch()
# if defined(VBAT) && defined(VBAT_CELLS)
if ( ( analogReader < 4 ) | | ( analogReader > 4 + VBAT_CELLS_NUM - 1 ) )
break ;
uint8_t ind = analogReader - 4 ;
static uint16_t vbatcells_pins [ VBAT_CELLS_NUM ] = VBAT_CELLS_PINS ;
static uint8_t vbatcells_offset [ VBAT_CELLS_NUM ] = VBAT_CELLS_OFFSETS ;
static uint8_t vbatcells_div [ VBAT_CELLS_NUM ] = VBAT_CELLS_DIVS ;
uint16_t v = analogRead ( vbatcells_pins [ ind ] ) ;
analog . vbatcells [ ind ] = vbatcells_offset [ ind ] + ( v < < 2 ) / vbatcells_div [ ind ] ;
// result is Vbatt in 0.1V steps
if ( ind = = VBAT_CELLS_NUM - 1 )
analog . vbat = analog . vbatcells [ ind ] ;
# endif // defined(VBAT) && defined(VBAT_CELLS)
break ;
} // end of switch()
# if defined( POWERMETER_HARD ) && (defined(LOG_VALUES) || defined(LCD_TELEMETRY))
if ( analog . amperage > powerValueMaxMAH )
powerValueMaxMAH = analog . amperage ;
# endif
# if defined(WATTS)
analog . watts = ( analog . amperage * analog . vbat ) / 100 ; // [0.1A] * [0.1V] / 100 = [Watt]
# if defined(LOG_VALUES) || defined(LCD_TELEMETRY)
if ( analog . watts > wattsMax )
wattsMax = analog . watts ;
# endif // defined(LOG_VALUES) || defined(LCD_TELEMETRY)
# endif // defined(WATTS)
// TODO: Try to generate buzzer over DSHOT protocol
# if defined(BUZZER)
alarmHandler ( ) ; // external buzzer routine that handles buzzer events globally now
# endif
if ( ( calibratingA > 0 & & USE_ACC ) | | ( calibratingG > 0 ) ) { // Calibration phasis
// LEDPIN_TOGGLE;
} else {
if ( flags . ACC_CALIBRATED ) {
// LEDPIN_OFF;
}
if ( flags . ARMED ) {
// LEDPIN_ON;
}
}
# if defined(LED_RING)
static uint32_t LEDTime ;
if ( currentTime > LEDTime ) {
LEDTime = currentTime + 50000 ;
i2CLedRingState ( ) ;
}
# endif
# if defined(LED_FLASHER)
auto_switch_led_flasher ( ) ;
# endif
if ( currentTime > calibratedAccTime ) {
if ( ! flags . SMALL_ANGLES_25 ) {
// the multi uses ACC and is not calibrated or is too much inclinated
flags . ACC_CALIBRATED = 0 ;
// LEDPIN_TOGGLE;
calibratedAccTime = currentTime + 100000 ;
} else {
flags . ACC_CALIBRATED = 1 ;
}
}
// TODO: Pump through other ways?
// #if !(defined(SERIAL_RX) && defined(PROMINI)) // Only one serial port on ProMini. Skip serial com if SERIAL RX in use. Note: Spek code will auto-call serialCom if GUI data detected on serial0.
// serialCom();
// #endif
# if defined(POWERMETER)
analog . intPowerMeterSum = ( pMeter [ PMOTOR_SUM ] / PLEVELDIV ) ;
intPowerTrigger1 = conf . powerTrigger1 * PLEVELSCALE ;
# endif
# ifdef LCD_TELEMETRY_AUTO
static char telemetryAutoSequence [ ] = LCD_TELEMETRY_AUTO ;
static uint8_t telemetryAutoIndex = 0 ;
static uint16_t telemetryAutoTimer = 0 ;
if (
( telemetry_auto ) & &
( ! ( + + telemetryAutoTimer % LCD_TELEMETRY_AUTO_FREQ ) )
) {
telemetry = telemetryAutoSequence [ + + telemetryAutoIndex % strlen ( telemetryAutoSequence ) ] ;
LCDclear ( ) ; // make sure to clear away remnants
}
# endif
# ifdef LCD_TELEMETRY
static uint16_t telemetryTimer = 0 ;
if ( ! ( + + telemetryTimer % LCD_TELEMETRY_FREQ ) ) {
# if (LCD_TELEMETRY_DEBUG+0 > 0)
telemetry = LCD_TELEMETRY_DEBUG ;
# endif
if ( telemetry )
lcd_telemetry ( ) ;
}
# endif
// TODO: Convert TELEMETRY -> use web sending / nrf24l01+ to send
# ifdef TELEMETRY
run_telemetry ( ) ;
# endif
# if GPS & defined(GPS_LED_INDICATOR) // modified by MIS to use STABLEPIN LED for number of sattelites indication
static uint32_t GPSLEDTime ; // - No GPS FIX -> LED blink at speed of incoming GPS frames
static uint8_t blcnt ; // - Fix and sat no. bellow 5 -> LED off
if ( currentTime > GPSLEDTime ) { // - Fix and sat no. >= 5 -> LED blinks, one blink for 5 sat, two blinks for 6 sat, three for 7 ...
if ( f . GPS_FIX & & GPS_numSat > = 5 ) {
if ( + + blcnt > 2 * GPS_numSat )
blcnt = 0 ;
GPSLEDTime = currentTime + 150000 ;
if ( blcnt > = 10 & & ( ( blcnt % 2 ) = = 0 ) ) {
STABLEPIN_ON ;
} else {
STABLEPIN_OFF ;
}
} else {
if ( ( GPS_update = = 1 ) & & ! f . GPS_FIX ) {
STABLEPIN_ON ;
} else {
STABLEPIN_OFF ;
}
blcnt = 0 ;
}
}
# endif
# if defined(LOG_VALUES) && (LOG_VALUES >= 2)
if ( cycleTime > cycleTimeMax )
cycleTimeMax = cycleTime ; // remember highscore
if ( cycleTime < cycleTimeMin )
cycleTimeMin = cycleTime ; // remember lowscore
# endif
if ( flags . ARMED ) {
# if defined(LCD_TELEMETRY) || defined(ARMEDTIMEWARNING) || defined(LOG_PERMANENT) || defined (TELEMETRY)
armedTime + = ( uint32_t ) cycleTime ;
# endif // defined(LCD_TELEMETRY)
# if defined(VBAT)
if ( ( analog . vbat > NO_VBAT ) & & ( analog . vbat < vbatMin ) )
vbatMin = analog . vbat ;
# endif // defined(VBAT)
# ifdef LCD_TELEMETRY
# if USE_BARO
if ( ( alt . EstAlt > BAROaltMax ) )
BAROaltMax = alt . EstAlt ;
# endif // BARO
# if GPS
if ( ( GPS_speed > GPS_speedMax ) )
GPS_speedMax = GPS_speed ;
# endif // GPS
# endif // LCD_TELEMETRY
}
}