Browse Source

Minor commits

master
Englebert 3 years ago
parent
commit
f6d8538b2a
  1. 2
      src/config.h
  2. 391
      src/main.cpp
  3. 7
      src/main.h

2
src/config.h

@ -32,6 +32,8 @@
#define FAILSAFE
#define MIDRC 1500 // Some radios might not have a neutral point centered on 1500. Can be changed here.
/*
* GPS configuration. For now only serial GPS that I have. Set GPS 1 if exits and GPS 0 if not exists
*/

391
src/main.cpp

@ -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_modules();
process_tasks();
}
// 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 +/-50cm in 1 second with cycle time about 3-4ms)
*/
@ -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
}
}

7
src/main.h

@ -122,7 +122,7 @@ typedef struct {
enum box {
BOXARM,
#if ACC
#if USE_ACC
BOXANGLE,
BOXHORIZON,
#endif
@ -440,12 +440,13 @@ void taskMain(void *pvParameters); // Main task
void compute_imu();
void process_rc(void);
void process_modules(void);
void process_tasks(void);
void run_tasks(void);
void acc_common(void);
void gyro_common(void);
uint8_t calculate_sum(uint8_t *cb , uint8_t siz);
void writeGlobalSet(uint8_t b);
void annexCode(void);
/***
void taskReceiver(void *pvParameters);
@ -481,6 +482,8 @@ extern int16_t BaroPID;
extern int16_t errorAltitudeI;
extern global_conf_t global_conf;
extern int16_t gyroZero[3];
extern int16_t lookupPitchRollRC[5]; // lookup table for expo & RC rate PITCH+ROLL
extern uint16_t lookupThrottleRC[11]; // lookup table for expo & mid THROTTLE
// Headers

Loading…
Cancel
Save