Problem vTaskSuspend and xTaskResumeFromISR

wellingj wrote on Monday, May 26, 2008:

Hi.
I’m using gcc version 4.3.0 and FreeRTOS V5.0.0 for reference.

I’m trying to use the vTaskSuspend and xTaskResumeFromISR to debug some servo code that I had modified.
Theory of operation is that I have a task that does nothing, until all my ticks of my servo code has gone through and I have all the data, then it gets output onto a serial port using some avrlib stuff. The code is below. What boggles me is that I’ve used the vTaskSuspend and xTaskResumeFromISR in this manner for a I2C command processing with FreeRTOS V 4.3 and it worked wonderfully and so I did a direct port. When it didn’t work, meaning the DebugServo task never runs after it’s first suspension, I started throwing in toggles on a led to see where the code was going and where it wasn’t and I’ve marked which calls to LED_TOGGLE( YELLOW ) don’t work (no flashing light). The only other task running is just changes the servo position ever 50 milliseconds. I’m running this at a really high rate of 1ms ticks, but I don’t think that should be the problem…

Any Ideas?

/*! \file servo_class.c \brief Interrupt-driven RC Servo function library. */
//*****************************************************************************
//
// File Name    : ‘servo.c’
// Title        : Interrupt-driven RC Servo function library
// Author       : Pascal Stang - Copyright (C) 2002
// Modified     : Jesse Welling - Copyright (C) 2008
// Created      : 7/31/2002
// Revised      : 4/02/2008
// Version      : 1.1
// Target MCU   : Atmel AVR Series
// Editor Tabs  : 3 spaces = 1 tab
//
// This code is distributed under the GNU Public License
//      which can be found at http://www.gnu.org/licenses/gpl.txt
//
//*****************************************************************************

#include <avr/io.h>
#include <avr/interrupt.h>

#include "FreeRTOS.h"
#include "task.h"
#include "semphr.h"

#include "…/avrlib/global.h"
#include "…/avrlib/uart.h"    // include uart function library
#include "…/avrlib/rprintf.h" // include printf function library

#include "servo.h"
#include "…/Robostix.h"

/*roll over number*/
#define TOP   ((uint16_t)40000u)

// handy to think with values
#define CENTER_POSITION_MSX100   1500u
#define MIN_POSITION_MSX100      1000u
#define MAX_POSITION_MSX100      2000u

// actual raw values
#define CENTER_POSITION_RAW      (CENTER_POSITION_MSX100 * 2)  // 3000
#define MIN_POSITION_RAW         (MIN_POSITION_MSX100 * 2) // 2000
#define MAX_POSITION_RAW         (MAX_POSITION_MSX100 * 2) // 4000

/* Note: Since only 500 millisecondsX100 is between center and min, and likewise center to max, when multiplied
*       by 2 to get the raw value that gives 1000 which then can be used as a signed percentX10 from center.
*       Nifty! But only for 16Mhz clocks
*/

// Global variables
// servo channel registers
static volatile uint16_t ServoPosTics;
static volatile uint8_t  ServoChannel;
static const uint16_t ServoPeriodTics = TOP;

xTaskHandle ServoDebugTaskHandle;

typedef struct

   uint8_t  chan;
   uint16_t inc;
   uint16_t nxt;
   uint16_t ocval;
   uint16_t srvpos;
}TimerDebug_T;

TimerDebug_T ServoTimerDebugBuffer[3];

// functions

static void DebugServo( void * param);

//! initializes software PWM system
void servoInit(void)
{
   uint8_t channel;
  
   //TODO REMOVE DEBUG
   uartInit();
   uartSetBaudRate(115200);
   rprintfInit(uartSendByte);

   // disble the timer3 output compare A interrupt
   cbi(ETIMSK, OCIE3A);

   /* setup timer 3
    *
    * TCCR3A Bit 7:6 – COMnA1:0: Compare Output Mode for Channel A
    * TCCR3A Bit 5:4 – COMnB1:0: Compare Output Mode for Channel B
    * TCCR3A Bit 3:2 – COMnC1:0: Compare Output Mode for Channel C
    * TCCR3A Bit 1:0 – WGMn1:0: Waveform Generation Mode
    *
    * TCCR3B Bit 4:3 – WGMn3:2: Waveform Generation Mode
    * TCCR3B Bit 2:0 – CSn2:0: Clock Select
    *
    * TCCR3C Bit 7 – FOCnA: Force Output Compare for Channel A
    * TCCR3C Bit 6 – FOCnB: Force Output Compare for Channel B
    * TCCR3C Bit 5 – FOCnC: Force Output Compare for Channel C
    *
    */

   TCCR3A |= 0x00;
   TCCR3B = 0x02;
   TCCR3C |= 0x00;

   // enable and clear channels
   for(channel=0; channel<NumberOfServoChannels; channel++)
   {
      // set minimum position as default
      ServoChannels[channel]->duty_raw = CENTER_POSITION_RAW;

      // set up DDR for each port.
      *DDR(ServoChannels[ServoChannel]->port) |= ServoChannels[ServoChannel]->pin_mask;

      // turn off current channel to break out of tristate. see page 67 of the data sheet
      *(ServoChannels[ServoChannel]->port) &= ~(ServoChannels[ServoChannel]->pin_mask);
   }
   // set PosTics
   ServoPosTics = 0;

   // Start at just past the last forcing the intrrupts to start processing the first.
   ServoChannel = NumberOfServoChannels + 1;

   // set initial interrupt time
   uint16_t OCValue = 0;

   // read in current value of output compare register OCR3A
   //OCValue = OCR3A;      // read OCR3A
   OCValue =  inb(OCR3AL);      // read low byte of OCR3A
   OCValue += inb(OCR3AH)<<8;   // read high byte of OCR3A

   // increment OCR3A value by nextTics
   OCValue += ServoPeriodTics;

   ServoTimerDebugBuffer[0].ocval = ServoPeriodTics;
   ServoTimerDebugBuffer[1].ocval = ServoPeriodTics;
   ServoTimerDebugBuffer[2].ocval = ServoPeriodTics;

   // set future output compare time to this new value
   //OCR3A = OCValue;         // write 16bit
   outb(OCR3AH, (OCValue>>8));         // write high byte
   outb(OCR3AL, (OCValue & 0x00FF));   // write low byte

   // enable the timer3 output compare A interrupt
   sbi(ETIMSK, OCIE3A);
}

//! turns off software PWM system
void servoOff(void)
{
   // disable the timer3 output compare A interrupt
   cbi(ETIMSK, OCIE3A);
}

//! set servo position on channel
void servoSetPosition(ServoChannel_T* this, int16_t percentX10)
{
   // input should be between 0 and SERVO_POSITION_MAX
   uint16_t pos_scaled;

   percentX10 = MAX(percentX10, -1000);
   percentX10 = MIN(percentX10, 1000);
  
   // calculate scaled position
   pos_scaled = (uint16_t)( percentX10 + CENTER_POSITION_RAW);

   // set position
   servoSetPositionRaw(this, pos_scaled);
}

//! get servo position on channel
int16_t servoGetPosition(ServoChannel_T* this)
{
   return ( (int16_t)servoGetPositionRaw(this) - CENTER_POSITION_RAW );
}

//! set servo position on channel (raw unscaled format)
void servoSetPositionRaw(ServoChannel_T* this, uint16_t position)
{
   // bind to limits
   position = MAX(position, MIN_POSITION_RAW);
   position = MIN(position, MAX_POSITION_RAW);

   // set position
   this->duty_raw = position;

   //TODO REMOVE DEBUG
   
   /*
   rprintfu08(this->pin_mask);
   rprintfChar(’@’);
   rprintfu08(this->port);
   rprintfChar(’:’);
   rprintfu16(this->duty_raw);
   rprintfCRLF();
   */
  
}

//! get servo position on channel (raw unscaled format)
uint16_t servoGetPositionRaw(ServoChannel_T* this)
{
   return this->duty_raw;
}

void PrintServoDebug(void)
{
   {
      rprintfu08(ServoTimerDebugBuffer[0].chan);
      rprintfChar(’ ‘);
      rprintfu16(ServoTimerDebugBuffer[0].inc);
      rprintfChar(’ ‘);
      rprintfu16(ServoTimerDebugBuffer[0].ocval);
      rprintfChar(’ ‘);
      rprintfu16(ServoTimerDebugBuffer[0].nxt);
      rprintfChar(’ ');
      rprintfu16(ServoTimerDebugBuffer[0].srvpos);
      rprintfCRLF();
   }

   {
      rprintfu08(ServoTimerDebugBuffer[1].chan);
      rprintfChar(’ ‘);
      rprintfu16(ServoTimerDebugBuffer[1].inc);
      rprintfChar(’ ‘);
      rprintfu16(ServoTimerDebugBuffer[1].ocval);
      rprintfChar(’ ‘);
      rprintfu16(ServoTimerDebugBuffer[1].nxt);
      rprintfChar(’ ');
      rprintfu16(ServoTimerDebugBuffer[1].srvpos);
      rprintfCRLF();
   }

   {
      rprintfu08(ServoTimerDebugBuffer[2].chan);
      rprintfChar(’ ‘);
      rprintfu16(ServoTimerDebugBuffer[2].inc);
      rprintfChar(’ ‘);
      rprintfu16(ServoTimerDebugBuffer[2].ocval);
      rprintfChar(’ ‘);
      rprintfu16(ServoTimerDebugBuffer[2].nxt);
      rprintfChar(’ ');
      rprintfu16(ServoTimerDebugBuffer[2].srvpos);
      rprintfCRLF();
   }
}

void StartDebugServoTask(void)
{
   xTaskCreate (DebugServo, "DbSrv", configMINIMAL_STACK_SIZE, NULL, ( tskIDLE_PRIORITY + 2), &ServoDebugTaskHandle);
}

static void DebugServo( void * param)
{
   for(;:wink:
   {
      PrintServoDebug();
      vTaskSuspend( NULL );
      //The toggle does not work here
      //LED_TOGGLE( YELLOW );
   }
}

ISR(TIMER3_COMPA_vect)
{
   uint16_t nextTics;

   // schedule next interrupt
   uint16_t OCValue;

   if(ServoChannel < NumberOfServoChannels)
   {
      // turn off current channel
      *(ServoChannels[ServoChannel]->port) &= ~(ServoChannels[ServoChannel]->pin_mask);
   }
  
   // next channel
   ServoChannel++;

   if(ServoChannel != NumberOfServoChannels)
   {
      // loop to channel 0 if needed
      if(ServoChannel > NumberOfServoChannels)   ServoChannel = 0;
      // turn on new channel
      *(ServoChannels[ServoChannel]->port) |= ServoChannels[ServoChannel]->pin_mask;
      // schedule turn off time
      nextTics = ServoChannels[ServoChannel]->duty_raw;
   }
   else //(Channel == NumberOfServoChannels)
   {
      // ***we could save time by precalculating this
      // schedule end-of-period
      nextTics = ServoPeriodTics - ServoPosTics;
   }

   // read in current value of output compare register OCR3A,
   // but within an interrupt so this direct access is safe
   OCValue = OCR3A;   // read all 16 bits of OCR3A
   //OCValue =  inb(OCR3AL);      // read low byte of OCR3A
   //OCValue += inb(OCR3AH)<<8;   // read high byte of OCR3A

   //TODO REMOVE DEBUG
   ServoTimerDebugBuffer[ServoChannel].chan = ServoChannel;
   ServoTimerDebugBuffer[ServoChannel].ocval = OCValue;
   ServoTimerDebugBuffer[ServoChannel].inc++;

   // increment OCR3A value by nextTics
   OCValue += nextTics;

   //   OCR3A+=nextTics; // but must protect the register so byte wise shift it in
   // set future output compare time to this new value
   // but within an interrupt so this direct access is safe
   OCR3A = OCValue;
   //outb(OCR3AH, (OCValue>>8));         // write high byte
   //outb(OCR3AL, (OCValue & 0x00FF));   // write low byte

   // set our new tic position
   ServoPosTics += nextTics;

   //TODO REMOVE DEBUG
   ServoTimerDebugBuffer[ServoChannel].nxt = nextTics;
   ServoTimerDebugBuffer[ServoChannel].srvpos = ServoPosTics;

   if(ServoPosTics >= ServoPeriodTics)
   {
      ServoPosTics = 0;

      //The toggle works here
      LED_TOGGLE( YELLOW );

      if (xTaskResumeFromISR(ServoDebugTaskHandle) == pdTRUE)
      {
         //The toggle does not work here
         //LED_TOGGLE( YELLOW );
         taskYIELD();
      }
   }
}

woops_ wrote on Monday, May 26, 2008:

The ResumeFromISR function has the following statement

vTaskResumeFromISR() should not be used to synchronise a task with an interrupt if there is a chance that the interrupt could arrive prior to the task being suspended - as this can lead to interrupts being missed. Use of a semaphore as a synchronisation mechanism would avoid this eventuality.

could that be the problem?

wellingj wrote on Tuesday, May 27, 2008:

That was exactly it…

Coincidentally, The example for vTaskResumeFromISR() has a "xSemaphoreCreateBinary()" when it should be a "vSemaphoreCreateBinary()". Had to look at the .h file to figure that one out :wink:

Thanks a bunch!

wellingj wrote on Tuesday, May 27, 2008:

Correction: The example for xSemaphoreGiveFromISR() had the error…

rtel wrote on Tuesday, May 27, 2008:

>Correction: The example for xSemaphoreGiveFromISR() had the error

Fixed - thanks for pointing this out.

Regards.