//receive a byte from the UART
__attribute__((__noinline__)) unsigned char sget_char(void)
{
#if USE_TIMER1 == 1
/*
This flag bit is set when there are unread data in the receive buffer and cleared when the receive
buffer is empty (i.e., does not contain any unread data).
*/
TOGGLE_LED();
TCNT1 = 0;
TIFR_REG |= (1 << OCF1A);
while( (_UCSRA_ & (1 << _RXC_)) == 0 )
{
if( (TIFR_REG & (1 << OCF1A)) ) { return(0); }
}
#else
unsigned long timeout = uart_rx_timeout;
while( (_UCSRA_ & (1 << _RXC_)) == 0 )
{
timeout--;
if(timeout == 0){ return(0); }
}
#endif
return(_UDR_);
}
/*******************************************************************
| led_drive - takes three parameters and performs 1 of
| four commands on an led sensor:
| 1.Read LED current state
| 2&3.SET LED ON or OFF
| 4.TOGGLE the LED state
| Parameters:
| lednum: a value between 0 and 2 PORTD<2-0>
| cmd: a value between 0 and 2 indicating which command
| LED_ON: Default is off (0) for LED_ON value, will be used with
| SET command (1).
| Return: return_value indicates if the command request was
| successful and if it wasn't, why not.
*******************************************************************/
short led_drive(int lednum, int cmd, int LED_ON)
{
short return_value;
return_value = -1;
// switch on the current command
switch(cmd)
{
case 0: // READ the value for return
{
return_value = READ_LED_STATE(lednum);
break;
}
case 1: // set the led on or off
{
if(LED_ON)
{
// SET LED_ON
TURN_ON_LED(lednum);
}
else
{
// SET LED_OFF
TURN_OFF_LED(lednum);
}
return_value = 2; // return success
break;
}
case 2: // TOGGLE the bits
{
// Toggle LED_ON/LED_OFF
TOGGLE_LED(lednum);
return_value = 2; // return success
break;
}
// return invalid cmd
default : {
return_value = INVALID_LED_COMMAND;
break;
}
}
return return_value; // EXIT drive_led with return status
}
void DriveControl(void)
{
float destination_floor_in_feet = 0;
while(1)
{
//tx Beginning trip from , going to ->
while(ELEVATOR.TRAVELING)
{
CLEAR_AND_START_TIMER5();
// Sleep until TIMER 5 is ready
if(xSemaphoreTake(TIMER5_RDY,portMAX_DELAY))
{
// fires every 500 ms not every second so CurrentVelocity
// is divided by 2 and breakingdecel is a division of 4
if(!ELEVATOR.BRAKING)
{
if( ELEVATOR.CurrentVelocity + (ELEVATOR.CurrentAccelPerSec + ELEVATOR.CurrentVelocity) < MAX_SPEED)
ELEVATOR.CurrentVelocity += (ELEVATOR.CurrentAccelPerSec + ELEVATOR.CurrentVelocity);
else
ELEVATOR.CurrentVelocity = MAX_SPEED;
if(ELEVATOR.UP)
{
ELEVATOR.CurrentLocation += ELEVATOR.CurrentVelocity;
if((ELEVATOR.CurrentLocation +
(ELEVATOR.CurrentAccelPerSec + ELEVATOR.CurrentVelocity/2))
>= ELEVATOR.BrakingStartLocation )
{
ELEVATOR.BRAKING = true;
destination_floor_in_feet = ELEVATOR.DestinationFloor *10;
}
}
else
{
ELEVATOR.CurrentLocation -= ELEVATOR.CurrentVelocity;
if((ELEVATOR.CurrentLocation +
(ELEVATOR.CurrentAccelPerSec - ELEVATOR.CurrentVelocity/2))
<= ELEVATOR.BrakingStartLocation )
{
ELEVATOR.BRAKING = true;
destination_floor_in_feet = ELEVATOR.DestinationFloor *10;
}
}
}
else // ELSE BRAKING!
{
if(ELEVATOR.UP)
{
if(destination_floor_in_feet - ELEVATOR.CurrentLocation > 10)
{
ELEVATOR.CurrentLocation += ELEVATOR.CurrentVelocity;
static float percent_complete;
percent_complete = (float)(ELEVATOR.CurrentLocation - ELEVATOR.BrakingStartLocation)/(float)((destination_floor_in_feet - (ELEVATOR.LastFloorStopped *10)));
ELEVATOR.BrakingDecelPerSec = (ELEVATOR.CurrentVelocity * percent_complete);
if(ELEVATOR.CurrentVelocity - ELEVATOR.BrakingDecelPerSec > MINIMUM_SPEED)
ELEVATOR.CurrentVelocity -= ELEVATOR.BrakingDecelPerSec;
}
else
{
if(destination_floor_in_feet - ELEVATOR.CurrentLocation < ELEVATOR.CurrentVelocity)
{
ELEVATOR.CurrentLocation = destination_floor_in_feet;
ELEVATOR.CurrentVelocity = 0;
ELEVATOR.TRAVELING = false;
ELEVATOR.BRAKING = false;
ELEVATOR.BrakingDecelPerSec = 0;
}
else
{
if(ELEVATOR.CurrentVelocity > 1.0f)
ELEVATOR.CurrentVelocity /= 2;
ELEVATOR.CurrentLocation += ELEVATOR.CurrentVelocity;
}
}
}
else
{
if(destination_floor_in_feet - ELEVATOR.CurrentLocation > 10)
{
ELEVATOR.CurrentLocation -= ELEVATOR.CurrentVelocity;
ELEVATOR.BrakingDecelPerSec = ELEVATOR.CurrentVelocity/4;
if(ELEVATOR.CurrentVelocity > MINIMUM_SPEED)
ELEVATOR.CurrentVelocity -= ELEVATOR.BrakingDecelPerSec;
}
else
{
if(destination_floor_in_feet - ELEVATOR.CurrentLocation < 1.5)
{
ELEVATOR.CurrentLocation = destination_floor_in_feet;
ELEVATOR.CurrentVelocity = 0;
ELEVATOR.TRAVELING = false;
ELEVATOR.BRAKING = false;
ELEVATOR.BrakingDecelPerSec = 0;
}
else
{
ELEVATOR.CurrentLocation -= ELEVATOR.CurrentVelocity;
}
}
}
}
}
// TX Current Location Data
}
TOGGLE_LED(2);
vTaskSuspend(NULL);
}
}
/**
* BootLoader_State_Machine
*
* The state machine for the boot loader which performs certain tasks
* based on the current state of the boot loader. The current state must
* be changed appropriately before calling this fuction.
*
*/
void BootLoader_State_Machine ()
{
switch (CurrentState)
{
case NORMAL:
/*FIXME Move the code from main and post the state machine*/
break;
case CPY_TO_BOOT:
{
uint8_t buffer [61];
Read_Attribute_Value(BL_ATTR_TYP_PRIMARY_IMG_LOCATION, &GImgAddr);
Read_Attribute_Value(BL_ATTR_TYP_PRIMARY_IMG_CRC, &GImgCrc);
Read_Attribute_Value(BL_ATTR_TYP_PRIMARY_IMG_SIZE, &GImgSize);
__nesc_atomic_t atomic = __nesc_atomic_start();
if (Move_Image_To_Boot_Loc (CurrImageAddr, CurrImageSize) == FAIL)
Reboot_Device ();
__nesc_atomic_end (atomic);
TOGGLE_LED (YELLOW);
if(Change_BootLoader_State_Attribute (SET_LOADED_TO_GOLDEN) == SUCCESS)
{
sprintf (buffer, "Successfully copied image to boot location.\n");
Send_USB_Command_Packet (CMD_BOOTLOADER_MSG, 61, buffer);
TOS_post (&BootLoader_State_Machine);
}
else
{
/**
* If we cant switch state then its hard to determine the problem
* So let reboot and try to recover.
*/
sprintf (buffer, "Could not switch to SET_LOADED_TO_GOLDEN. Rebooting\n");
Send_USB_Error_Packet (ERR_FATAL_ERROR_ABORT, 60, buffer);
Reboot_Device ();
}
}
break;
case SET_LOADED_TO_GOLDEN:
{
uint8_t buffer [61];
result_t ret = SUCCESS;
/*It may not be required, but still we dont want an interrupt to stop us*/
TOGGLE_LED (GREEN);
__nesc_atomic_t atomic = __nesc_atomic_start();
ret = Map_Loaded_To_Golden ();
__nesc_atomic_end (atomic);
TOGGLE_LED (GREEN);
if (ret == SUCCESS)
{
sprintf (buffer, "Mapped Boot Image to golden. Booting New Image.\n");
Send_USB_Command_Packet (CMD_CREATED_GOLDEN_IMG, 61, buffer);
Reboot_Device ();
}
else
{
/* Very Bad, We dont know exactly where we failed. I Guess the
* best way is reset the whole primary to secondary switch and
* reboot.
*/
sprintf (buffer, "Mark loaded to golden failed. Trying to recover\n");
Send_USB_Error_Packet (ERR_FATAL_ERROR_ABORT, 60, buffer);
Reboot_Device ();
}
}
break;
case SET_BOOT_TO_GOLDEN:
{
Change_BootLoader_State_Attribute (NORMAL);
Reboot_Device ();
}
break;
case BOOT_IMAGE:
break;
case CODE_LOAD:
switch (CLoaderState)
{
case REQUEST_IMAGE_DETAIL:
{
USBCommand cmd;
if (!MMU_ENABLED)
{
MMU_ENABLED = TRUE;
Enable_MMU ();
}
cmd.type = CMD_GET_IMAGE_DETAILS;
Send_Command_Packet (&cmd, sizeof (USBCommand));
}
break;
case REQUEST_USB_PACKETS:
#ifdef MMU_ENABLE
Request_Next_Binary_Chunk ();
#else
Request_Next_Binary_Packet ();
#endif
break;
case VERIFY_CURRENT_BUFFER:
break;
case VERIFY_CURRENT_IMAGE:
Send_USB_Command_Packet (CMD_IMG_UPLOAD_COMPLETE, 0, NULL);
break;
default:
break;
}
break;
case VERIFY_IMAGE:
break;
case VERIFY_SELF_TEST:
/*Implemented in Recovery State Machine*/
break;
default:
/* This is fatal, It is very essential to investigate why
* this might have happened but at the same time we dont
* want to panic. The best is to go back and try normal
* operation.
*/
CurrentState = NORMAL;
break;
}
}
/**
* Move_Image_To_Boot_Loc
*
* Move the image at imgAddr to the boot location. This
* function holds a copy of the vector table. If incase there is
* a failure then it will try to replace the vector table to
* save the boot loader.
*
* @param imgAddr Starting address of the image to be moved.
* @param imgSiz Size of the image at imgAddr.
*
* @return SUCCESS | FAIL
*/
result_t Move_Image_To_Boot_Loc (uint32_t imgAddr, uint32_t imgSiz)
{
result_t ret;
uint32_t ImgFlashAddr = imgAddr + 32; /*Drop the vec table*/
uint32_t CSize= 0x8000;
uint32_t RSize = 0;
uint32_t CurSize = 0x20;
uint32_t BootAddr = 0x0;
uint32_t AddVec = 0x0;
Flash_Read (0x0, 32, vecbuffer); /* Keep this for recovery purpose*/
if (Flash_Param_Partition_Erase (BootAddr) == SUCCESS)
{
if (Flash_Write (BootAddr, vecbuffer, 0x20) == FAIL)
{
memcpy (cpybuffer, vecbuffer, 0x20);
AddVec = 0x20;
}
else
BootAddr = 0x20;
}
ret = __Binary_Erase (CurrImageAddr, CurrImageSize, vecbuffer);
if (Flash_Read (ImgFlashAddr, (CSize - CurSize), (cpybuffer + AddVec)) == SUCCESS)
{
if (Flash_Write (BootAddr, cpybuffer, (0x7FE0 + AddVec)) == SUCCESS)
{
CurSize += (0x8000 - 0x20);
ImgFlashAddr += (0x8000 - 0x20);
BootAddr += (0x8000 - 0x20);
}
else
{
if (Flash_Write (BootAddr, vecbuffer, 0x20) == FAIL)
{
TOGGLE_LED (RED);
while (1);
}
else
return FAIL;
}
}
while (CurSize < imgSiz)
{
RSize = ((imgSiz - CurSize) > CSize)? CSize : (imgSiz - CurSize);
if (Flash_Read (ImgFlashAddr, RSize, cpybuffer) == SUCCESS)
{
if (Flash_Write (BootAddr, cpybuffer, 0x8000) == SUCCESS)
{
CurSize += RSize;
ImgFlashAddr += RSize;
BootAddr += RSize;
}
else
return FAIL;
}
}
return SUCCESS;
}
/**
* BootLoader_Recovery_State_Machine
*
* State machine that is invoked while we are recovering from
* a crash. The actions that has to be taken after a crash differs
* from the normal one, just to prevent an infinite loop.
* Also there are only few states in which the boot loader
* crash could crash.
* NOTE:
* if the function doesnt recognize the crash state or if there is a
* possibility of an infinite loop then stall.
*/
void BootLoader_Recovery_State_Machine ()
{
result_t ret = FAIL;
switch (CurrentState)
{
case CPY_TO_BOOT:
ret = Read_Attribute_Value(BL_ATTR_TYP_PRIMARY_IMG_LOCATION, &GImgAddr);
ret = Read_Attribute_Value(BL_ATTR_TYP_PRIMARY_IMG_CRC, &GImgCrc);
ret = Read_Attribute_Value(BL_ATTR_TYP_PRIMARY_IMG_SIZE, &GImgSize);
ret = Read_Attribute_Value(BL_ATTR_TYP_SECONDARY_IMG_LOCATION, &CurrImageAddr);
ret = Read_Attribute_Value(BL_ATTR_TYP_SECONDARY_IMG_CRC, &CurrImageCrc);
ret = Read_Attribute_Value(BL_ATTR_TYP_SECONDARY_IMG_SIZE, &CurrImageSize);
if (ret == SUCCESS)
{
if (Move_Image_To_Boot_Loc (CurrImageAddr, CurrImageSize) == SUCCESS)
{
if (Map_Loaded_To_Golden () == FAIL)
{
/*Prepare for the second pass*/
if (Change_BootLoader_State_Attribute (SET_LOADED_TO_GOLDEN) == FAIL)
{
TOGGLE_LED (RED);
while (1); /* Thats a stall case, dont go back*/
}
}
}
}
if (Change_BootLoader_State_Attribute (NORMAL) == FAIL)
{
TOGGLE_LED (RED);
while (1); /* Thats a stall case, dont go back*/
}
Reboot_Device ();
break;
case SET_LOADED_TO_GOLDEN:
{
ret = Read_Attribute_Value(BL_ATTR_TYP_PRIMARY_IMG_LOCATION, &GImgAddr);
ret = Read_Attribute_Value(BL_ATTR_TYP_PRIMARY_IMG_CRC, &GImgCrc);
ret = Read_Attribute_Value(BL_ATTR_TYP_PRIMARY_IMG_SIZE, &GImgSize);
if (ret == SUCCESS)
{
ret = Read_Attribute_Value(BL_ATTR_TYP_SECONDARY_IMG_LOCATION, &CurrImageAddr);
ret = Read_Attribute_Value(BL_ATTR_TYP_SECONDARY_IMG_CRC, &CurrImageCrc);
ret = Read_Attribute_Value(BL_ATTR_TYP_SECONDARY_IMG_SIZE, &CurrImageSize);
if (ret == SUCCESS)
{
Map_Loaded_To_Golden ();
}
else
{
/* We know that we have a good PIMG. so lets replace it and boot the old app
*/
Move_Image_To_Boot_Loc (GImgAddr, GImgSize);
if ((CurrImageAddr != BL_PRIMARY_IMAGE_LOCATION) &&
(CurrImageAddr != BL_SECONDARY_IMAGE_LOCATION))
{
/* Find where the current primary location is to determine
* the secondary location.
*/
if ((GImgAddr - BL_PRIMARY_IMAGE_LOCATION) > 0)
ret = Write_Attribute_Value(BL_ATTR_TYP_SECONDARY_IMG_LOCATION,
BL_PRIMARY_IMAGE_LOCATION);
else
ret = Write_Attribute_Value(BL_ATTR_TYP_SECONDARY_IMG_LOCATION,
BL_SECONDARY_IMAGE_LOCATION);
ret = Write_Attribute_Value(BL_ATTR_TYP_SECONDARY_IMG_LOCATION, 0);
ret = Write_Attribute_Value(BL_ATTR_TYP_SECONDARY_IMG_LOCATION, 0);
}
}
}
else
{
/* We might have busted the whole table, just replace the default*/
Recover_Default_Table (BL_ATTR_TYP_BOOTLOADER);
}
if (Change_BootLoader_State_Attribute (NORMAL) == FAIL)
{
TOGGLE_LED (RED);
while (1); /* Thats a stall case, dont go back, Will lead to a loop*/
}
Reboot_Device ();
}
break;
case SET_BOOT_TO_GOLDEN:
if (Change_BootLoader_State_Attribute (NORMAL) == FAIL)
{
TOGGLE_LED (RED);
while (1); /* Thats a stall case, dont go back*/
}
Reboot_Device ();
break;
case VERIFY_SELF_TEST:
if (Validate_Self_Test_Image () == SUCCESS)
{
if (Self_Test_Succeeded () == FAIL)
; /* Serious, we need to some how save the vector table*/
}
else
{
Self_Test_Failed ();
Reboot_Device ();
}
break;
default:
Recover_Default_Table (BL_ATTR_TYP_BOOTLOADER);
Reboot_Device ();
break;
}
}
