WebSVN - FlightCtrl - Diff - Rev 2017 and 2019 - /branches/dongfang_FC



 
// The channel array is now 0-based.
volatile int16_t PPM_in[MAX_CHANNELS];
volatile int16_t PPM_diff[MAX_CHANNELS];
volatile uint8_t NewPpmData = 1;
volatile uint8_t RCQuality = 0;
int16_t RC_PRTY[4];
uint8_t lastRCCommand = COMMAND_NONE;

uint8_t commandTimer = 0;

  // Disable Output Compare A & B Match Interrupts (bit: OCIE1B=0, OICIE1A=0)
  // Enable Overflow Interrupt (bit: TOIE1=0)
  TIMSK1 &= ~((1<<OCIE1B) | (1<<OCIE1A) | (1<<TOIE1));
  TIMSK1 |= (1<<ICIE1);

 

  RCQuality = 0;
 

  SREG = sreg;

      if ((signal > 250) && (signal < 687)) {
        // shift signal to zero symmetric range  -154 to 159
        signal -= 470; // offset of 1.4912 ms ??? (469 * 3.2us = 1.5008 ms)
        // check for stable signal
        if (abs(signal - PPM_in[index]) < 6) {
          if (RCQuality < 200)
            RCQuality += 10;
          else
            RCQuality = 200;
        }
        // If signal is the same as before +/- 1, just keep it there.
        if (signal >= PPM_in[index] - 1 && signal <= PPM_in[index] + 1) {
          // In addition, if the signal is very close to 0, just set it to 0.
          if (signal >= -1 && signal <= 1) {

            tmp = PPM_in[index];
          }
        } else
          tmp = signal;
        // calculate signal difference on good signal level
        if (RCQuality >= 195)
          PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3; // cut off lower 3 bit for nois reduction
        else
          PPM_diff[index] = 0;
        PPM_in[index] = tmp; // update channel value
      }

/*
 * This must be called (as the only thing) for each control loop cycle (488 Hz).
 */
void RC_update() {
  int16_t tmp1, tmp2;
  if (RCQuality) {
    RCQuality--;
    if (NewPpmData-- == 0) {
      RC_PRTY[CONTROL_PITCH] = RCChannel(CH_PITCH) * staticParams.stickP
          + RCDiff(CH_PITCH) * staticParams.stickD;
      RC_PRTY[CONTROL_ROLL] = RCChannel(CH_ROLL) * staticParams.stickP
          + RCDiff(CH_ROLL) * staticParams.stickD;

    }
  } else { // Bad signal
    RC_PRTY[CONTROL_PITCH] = RC_PRTY[CONTROL_ROLL] = RC_PRTY[CONTROL_THROTTLE]
        = RC_PRTY[CONTROL_YAW] = 0;
  }
 
  debugOut.analog[21] = RCQuality;
}

 
/*
 * Get Pitch, Roll, Throttle, Yaw values

   */
  return PPM_in[varNum] + POT_OFFSET;
}

 
uint8_t RC_getSignalQuality(void) {
  if (RCQuality >= 160)
    return SIGNAL_GOOD;
  if (RCQuality >= 140)
    return SIGNAL_OK;
  if (RCQuality >= 120)
    return SIGNAL_BAD;
  return SIGNAL_LOST;

}

Rev 2017	Rev 2019
Line 60...	Line 60...
60		60
61	// The channel array is now 0-based.	61	// The channel array is now 0-based.
62	volatile int16_t PPM_in[MAX_CHANNELS];	62	volatile int16_t PPM_in[MAX_CHANNELS];
63	volatile int16_t PPM_diff[MAX_CHANNELS];	63	volatile int16_t PPM_diff[MAX_CHANNELS];
64	volatile uint8_t NewPpmData = 1;	64	volatile uint8_t NewPpmData = 1;
65	volatile uint8_t RC_Quality = 0;	65	volatile uint8_t RCQuality = 0;
66	int16_t RC_PRTY[4];	66	int16_t RC_PRTY[4];
67	uint8_t lastRCCommand = COMMAND_NONE;	67	uint8_t lastRCCommand = COMMAND_NONE;
Line 68...	Line 68...
68	uint8_t commandTimer = 0;	68	uint8_t commandTimer = 0;
Line 112...	Line 112...
112	// Disable Output Compare A & B Match Interrupts (bit: OCIE1B=0, OICIE1A=0)	112	// Disable Output Compare A & B Match Interrupts (bit: OCIE1B=0, OICIE1A=0)
113	// Enable Overflow Interrupt (bit: TOIE1=0)	113	// Enable Overflow Interrupt (bit: TOIE1=0)
114	TIMSK1 &= ~((1<<OCIE1B) \| (1<<OCIE1A) \| (1<<TOIE1));	114	TIMSK1 &= ~((1<<OCIE1B) \| (1<<OCIE1A) \| (1<<TOIE1));
115	TIMSK1 \|= (1<<ICIE1);	115	TIMSK1 \|= (1<<ICIE1);
Line 116...	Line 116...
116		116
Line 117...	Line 117...
117	RC_Quality = 0;	117	RCQuality = 0;
118		118
Line 119...	Line 119...
119	SREG = sreg;	119	SREG = sreg;
Line 171...	Line 171...
171	if ((signal > 250) && (signal < 687)) {	171	if ((signal > 250) && (signal < 687)) {
172	// shift signal to zero symmetric range -154 to 159	172	// shift signal to zero symmetric range -154 to 159
173	signal -= 470; // offset of 1.4912 ms ??? (469 * 3.2us = 1.5008 ms)	173	signal -= 470; // offset of 1.4912 ms ??? (469 * 3.2us = 1.5008 ms)
174	// check for stable signal	174	// check for stable signal
175	if (abs(signal - PPM_in[index]) < 6) {	175	if (abs(signal - PPM_in[index]) < 6) {
176	if (RC_Quality < 200)	176	if (RCQuality < 200)
177	RC_Quality += 10;	177	RCQuality += 10;
178	else	178	else
179	RC_Quality = 200;	179	RCQuality = 200;
180	}	180	}
181	// If signal is the same as before +/- 1, just keep it there.	181	// If signal is the same as before +/- 1, just keep it there.
182	if (signal >= PPM_in[index] - 1 && signal <= PPM_in[index] + 1) {	182	if (signal >= PPM_in[index] - 1 && signal <= PPM_in[index] + 1) {
183	// In addition, if the signal is very close to 0, just set it to 0.	183	// In addition, if the signal is very close to 0, just set it to 0.
184	if (signal >= -1 && signal <= 1) {	184	if (signal >= -1 && signal <= 1) {
Line 187...	Line 187...
187	tmp = PPM_in[index];	187	tmp = PPM_in[index];
188	}	188	}
189	} else	189	} else
190	tmp = signal;	190	tmp = signal;
191	// calculate signal difference on good signal level	191	// calculate signal difference on good signal level
192	if (RC_Quality >= 195)	192	if (RCQuality >= 195)
193	PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3; // cut off lower 3 bit for nois reduction	193	PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3; // cut off lower 3 bit for nois reduction
194	else	194	else
195	PPM_diff[index] = 0;	195	PPM_diff[index] = 0;
196	PPM_in[index] = tmp; // update channel value	196	PPM_in[index] = tmp; // update channel value
197	}	197	}
Line 239...	Line 239...
239	/*	239	/*
240	* This must be called (as the only thing) for each control loop cycle (488 Hz).	240	* This must be called (as the only thing) for each control loop cycle (488 Hz).
241	*/	241	*/
242	void RC_update() {	242	void RC_update() {
243	int16_t tmp1, tmp2;	243	int16_t tmp1, tmp2;
244	if (RC_Quality) {	244	if (RCQuality) {
245	RC_Quality--;	245	RCQuality--;
246	if (NewPpmData-- == 0) {	246	if (NewPpmData-- == 0) {
247	RC_PRTY[CONTROL_PITCH] = RCChannel(CH_PITCH) * staticParams.stickP	247	RC_PRTY[CONTROL_PITCH] = RCChannel(CH_PITCH) * staticParams.stickP
248	+ RCDiff(CH_PITCH) * staticParams.stickD;	248	+ RCDiff(CH_PITCH) * staticParams.stickD;
249	RC_PRTY[CONTROL_ROLL] = RCChannel(CH_ROLL) * staticParams.stickP	249	RC_PRTY[CONTROL_ROLL] = RCChannel(CH_ROLL) * staticParams.stickP
250	+ RCDiff(CH_ROLL) * staticParams.stickD;	250	+ RCDiff(CH_ROLL) * staticParams.stickD;
Line 272...	Line 272...
272	}	272	}
273	} else { // Bad signal	273	} else { // Bad signal
274	RC_PRTY[CONTROL_PITCH] = RC_PRTY[CONTROL_ROLL] = RC_PRTY[CONTROL_THROTTLE]	274	RC_PRTY[CONTROL_PITCH] = RC_PRTY[CONTROL_ROLL] = RC_PRTY[CONTROL_THROTTLE]
275	= RC_PRTY[CONTROL_YAW] = 0;	275	= RC_PRTY[CONTROL_YAW] = 0;
276	}	276	}
-		277
-		278	debugOut.analog[21] = RCQuality;
277	}	279	}
Line 278...	Line 280...
278		280
279	/*	281	/*
280	* Get Pitch, Roll, Throttle, Yaw values	282	* Get Pitch, Roll, Throttle, Yaw values
Line 296...	Line 298...
296	*/	298	*/
297	return PPM_in[varNum] + POT_OFFSET;	299	return PPM_in[varNum] + POT_OFFSET;
298	}	300	}
Line 299...	Line 301...
299		301
300	uint8_t RC_getSignalQuality(void) {	302	uint8_t RC_getSignalQuality(void) {
301	if (RC_Quality >= 160)	303	if (RCQuality >= 160)
302	return SIGNAL_GOOD;	304	return SIGNAL_GOOD;
303	if (RC_Quality >= 140)	305	if (RCQuality >= 140)
304	return SIGNAL_OK;	306	return SIGNAL_OK;
305	if (RC_Quality >= 120)	307	if (RCQuality >= 120)
306	return SIGNAL_BAD;	308	return SIGNAL_BAD;
307	return SIGNAL_LOST;	309	return SIGNAL_LOST;
Line 308...	Line 310...
308	}	310	}

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/rc.c @ 2189 - Rev 2017 → 2019