void copyBufferToDMA (unsigned char size){
unsigned char i;
for( i = 0; i < size; i += 1 )
{
BufferA[i] = (unsigned int) readFront(logBuffer);
}
}
/****************************************************************************
Function
GetChar
Parameters
None.
Returns
ch - char from the serial port
Description
reads first character from buffer or returns 0 if no chars available
Notes
Author
Max Dunne, 2011.11.10
****************************************************************************/
char GetChar(void) {
char ch;
if (getLength(receiveBuffer) == 0) {
ch = 0;
} else {
ch = readFront(receiveBuffer);
}
return ch;
}
// function to read bytes from uart
//mostly defunct as now bytes are read by DMA
char UART2GetChar()
{
//return getLength(UART_receiveBuffer);
if(getLength(UART_receiveBuffer)==0)
{
return 0;
}
return readFront(UART_receiveBuffer);
}
uint16_t UART_getChar(uint8_t id)
{
uint16_t ch;
if(id == UART1_ID){
if (getLength(receiveBufferUart1) == 0) {
ch = 0xFF00;
} else {
ch = (readFront(receiveBufferUart1) & 0x00FF);
}
return ch;
}else if(id == UART2_ID){
if (getLength(receiveBufferUart2) == 0) {
ch = 0xFF00;
} else {
ch = (readFront(receiveBufferUart2) & 0x00FF);
}
return ch;
}
}
/****************************************************************************
Function
IntUart1Handler
Parameters
None.
Returns
None.
Description
Interrupt Handle for the uart. with the PIC32 architecture both send and receive are handled within the same interrupt
Notes
Author
Max Dunne, 2011.11.10
****************************************************************************/
void __ISR(_UART2_VECTOR, ipl4) IntUart2Handler(void)
{
if (mU2RXGetIntFlag()) {
mU2RXClearIntFlag();
writeBack(receiveBufferUart2, (unsigned char) U2RXREG);
}
if (mU2TXGetIntFlag()) {
mU2TXClearIntFlag();
if (!(getLength(transmitBufferUart2) == 0)) {
U2TXREG = readFront(transmitBufferUart2);
}
}
}
//interupt for handling transmitting of bytes
void __attribute__((__interrupt__, no_auto_psv)) _U2TXInterrupt(void)
{
IFS1bits.U2TXIF=0;
if(getLength(UART_transmitBuffer)==0)
{
IEC1bits.U2TXIE=0;
}
else
{
U2TXREG=readFront(UART_transmitBuffer);
//IEC1bits.U2TXIE=1;
}
}
/*
* testRangeFinders()
* This function is used to test and classify the idealized function for the range finders, as well as
* implementing the basic functionality. It will turn LEDs on and off depending on if an object comes
* within a certain range (see spreadsheet for distance and readouts). Setting a breakpoint at the top
* of the while loop while the code is running will give a good value for what the ADC reads at a particular
* distance.
*/
void testRangeFinders()
{
BCSCTL1 = CALBC1_8MHZ;
DCOCTL = CALDCO_8MHZ;
initRangeFinders();
P1DIR |= LEFT_LED|RIGHT_LED;
int fBuffer[BUFFER_LN];
int lBuffer[BUFFER_LN];
int rBuffer[BUFFER_LN];
int mf;
int ml;
int mr;
fillBuffers(fBuffer, lBuffer, rBuffer);
while(1)
{
readFront(fBuffer); // Place Breakpoint here
readLeft(lBuffer);
readRight(rBuffer);
mf = median(fBuffer); // Medians were chosen as they are less influenced by outliers
ml = median(lBuffer); // The user is free to change these functions to test and classify the
mr = median(rBuffer); // means, however they should be close to the median values.
if(mf > 0x01F0)
{
P1OUT |= (RIGHT_LED|LEFT_LED);
}
else if(ml > 0x0220)
{
P1OUT &= ~RIGHT_LED;
P1OUT |= LEFT_LED;
}
else if(mr > 0x0220)
{
P1OUT &= ~LEFT_LED;
P1OUT |= RIGHT_LED;
}
else
{ // Important reset condition, do not forget in robot mevement code
P1OUT &= ~(LEFT_LED|RIGHT_LED);
}
}
}
void hilRead(unsigned char* hilChunk){
// fix the data length so if the interrupt adds data
// during execution of this block, it will be read
// until the next hilRead
unsigned char tmpLen = getLength(uartBufferIn), i=0;
// if the buffer has more data than the max size, set it to max,
// otherwise set it to the length
hilChunk[0] = (tmpLen > MAXSEND -1)? MAXSEND -1: tmpLen;
// read the data
for(i = 1; i <= hilChunk[0]; i += 1 )
{
hilChunk[i] = readFront(uartBufferIn);
}
}
/**
* @Function GetChar(void)
* @param None.
* @return ch - char from the serial port
* @brief reads first character from buffer or returns 0 if no chars available
* @author Max Dunne, 2011.11.10 */
char GetChar(void)
{
char ch;
if (getLength(receiveBuffer) == 0) {
ch = 0;
} else {
GettingFromReceive = TRUE;
ch = readFront(receiveBuffer);
GettingFromReceive = FALSE;
}
//re-enter the interrupt if we added a character while transmitting another one
if (ReceiveCollisionOccured) {
INTSetFlag(INT_U1RX);
ReceiveCollisionOccured = FALSE;
}
return ch;
}
void init(void) {
int i;
for(i=0;i<numSpheres;i++){
spheres[i][0] = (rand()%600)-300;//XPOS
spheres[i][1] = (rand()%600)-300;//YPOS
spheres[i][2] = rand()%400;//ZPOS
spheres[i][3] = rand()%20;//SIZE
spheres[i][4] = (rand()%20);//SPEED
}
MAXROT = 1.0/6.0*PI;
glClearColor (0.0, 0.0, 0.0, 0.0);
glEnable(GL_DEPTH_TEST);
glShadeModel(GL_SMOOTH);
pmodel = glmReadOBJ("city.obj");
if (!pmodel) fprintf(stderr,"Cannot parse vase.obj");
//glmUnitize(pmodel); // make model to fit in a unit cube
glmFacetNormals(pmodel); // generate normals - is this needed?
glmVertexNormals(pmodel, 90.0); // average joining normals - allow for hard edges.
tmodel = glmReadOBJ("tower.obj");
glmFacetNormals(tmodel); // generate normals - is this needed?
glmVertexNormals(tmodel, 90.0); // average joining normals - allow for hard edges.
readTop();
readFront();
readBack();
readRight();
readLeft();
readFire();
glPixelStorei(GL_UNPACK_ALIGNMENT, 1);
glGenTextures(1, &front);
glGenTextures(1, &top);
glGenTextures(1, &left);
glGenTextures(1, &right);
glGenTextures(1, &back);
glGenTextures(1, &fire);
qsphere = gluNewQuadric();
}
/****************************************************************************
Function
IntUart1Handler
Parameters
None.
Returns
None.
Description
Interrupt Handle for the uart. with the PIC32 architecture both send and receive are handled within the same interrupt
Notes
Author
Max Dunne, 2011.11.10
****************************************************************************/
void __ISR(_UART1_VECTOR, ipl4) IntUart1Handler(void)
{
if (INTGetFlag(INT_U1RX)) {
INTClearFlag(INT_U1RX);
if (!GettingFromReceive) {
writeBack(receiveBuffer, (unsigned char) U1RXREG);
} else {
//acknowledge we have a collision and return
ReceiveCollisionOccured = TRUE;
}
}
if (INTGetFlag(INT_U1TX)) {
INTClearFlag(INT_U1TX);
if (!(getLength(transmitBuffer) == 0)) {
if (!AddingToTransmit) {
U1TXREG = readFront(transmitBuffer);
} else {
//acknowledge we have a collision and return
TransmitCollisionOccured = TRUE;
}
}
}
}
/****************************************************************************
Function
IntUart1Handler
Parameters
None.
Returns
None.
Description
Interrupt Handle for the uart. with the PIC32 architecture both send and receive are handled within the same interrupt
Notes
Author
Max Dunne, 2011.11.10
****************************************************************************/
void __ISR(_UART1_VECTOR, IPL4AUTO) IntUart1Handler(void) {
static int transmitcount = 0;
static int clearcount = 0;
static int stallcount = 0;
if (INTGetFlag(INT_U1RX)) {
LATBbits.LATB8 ^= 1;
writeBack(receiveBuffer, (unsigned char) U1RXREG);
INTClearFlag(INT_U1RX);
}
if (INTGetFlag(INT_U1TX)) {
//INTClearFlag(INT_U1TX);
//IFS1bits.U1TXIF=0;
//INTSTATbits.
//Nop();
//LATBbits.LATB7=0;
//LATBbits.LATB8=PORTCbits.RC3;
//LATCbits.LATC3^=1;
if (!(getLength(transmitBuffer) == 0)) {
transmitcount++;
U1TXREG = readFront(transmitBuffer);
INTClearFlag(INT_U1TX);
stallcount = 0;
} else {
clearcount++;
//INTEnable(INT_U1TX, 0);
stallcount++;
Nop();
}
if (stallcount > 1) {
INTEnable(INT_U1TX, INT_DISABLED);
INTClearFlag(INT_U1TX);
}
}
}
// TODO: Include a messaging Mechanism for immediate or once in time messages
// TODO: Include a File option for Archiving checksum fails for debuging
float protParseDecode (unsigned char* fromSPI, FILE* outFile, unsigned char prevException){
#else
void protParseDecode (unsigned char* fromSPI){
#endif
// Static variables CAREFUL
static unsigned char prevBuffer[2*MAXLOGLEN];
static unsigned char previousComplete =1;
static unsigned char indexLast = 0;
#ifdef _IN_PC_
static long long checkSumFail = 0;
static long long totalPackets = 0;
static float test = 0.0;
float alpha = 0.3;
#endif
// local variables
unsigned char i;
unsigned char tmpChksum = 0, headerFound=0, noMoreBytes = 1;
unsigned char trailerFound = 0;
//unsigned char logSize = 0;
// Add the received bytes to the protocol parsing circular buffer
for(i = 1; i <= fromSPI[0]; i += 1 )
//for(i = 0; i <= 95; i += 1 )
{
writeBack(ppBuffer, fromSPI[i]);
}
// update the noMoreBytes flag accordingly
noMoreBytes = (fromSPI[0]>0)?0:1;
// noMoreBytes = 0;
while (!noMoreBytes){
// if the previous message was complete then read from the circular buffer
// and make sure that you are into the begining of a message
if(previousComplete){
while (!headerFound && !noMoreBytes) {
// move along until you find a dollar sign or run out of bytes
while (getLength(ppBuffer)>1 && peak(ppBuffer)!=DOLLAR){
readFront(ppBuffer);
}
// if you found a dollar then make sure the next one is an AT
if(getLength(ppBuffer)>1 && peak(ppBuffer) == DOLLAR){
// read it
prevBuffer[indexLast++] = readFront(ppBuffer);
// if next is a at sign
if (peak(ppBuffer) == AT){
// read it
prevBuffer[indexLast++] = readFront(ppBuffer);
// and signal you found a header
headerFound = 1;
// and set as incomplete the sentece
previousComplete = 0;
}
} else {
noMoreBytes = 1;
} // else no dollar
} // while we found header && no more bytes
}// if previous complete
// At this point either you found a header from a previous complete
// or you are reading from a message that was incomplete the last time
// in any of those two cases, keep reading until you run out of bytes
// or find a STAR and an AT
while (!trailerFound && !noMoreBytes){
while (getLength(ppBuffer)>2 && peak(ppBuffer)!=STAR){
prevBuffer[indexLast++] = readFront(ppBuffer);
}
// if you found a STAR (42) and stil have bytes
if (getLength(ppBuffer)>2 && peak(ppBuffer)==STAR){
// read it
prevBuffer[indexLast++] = readFront(ppBuffer);
// if you still have 2 bytes
if (getLength(ppBuffer)>1){
// and the next byte is an AT sign
if (peak(ppBuffer)==AT){
// then you found a trailer
trailerFound =1;
}
} else {
noMoreBytes =1;
}
} else {
// no more bytes
noMoreBytes =1;
}
}
// if you found a trailer, then the message is done
if(trailerFound){
// read the AT and the checksum
prevBuffer[indexLast++] = readFront(ppBuffer);
prevBuffer[indexLast] = readFront(ppBuffer);
// Compute the checksum
tmpChksum= getChecksum(prevBuffer, indexLast-1);
#ifdef _IN_PC_
totalPackets++;
#endif
// if the checksum is valid
if (tmpChksum ==prevBuffer[indexLast]){
// update the states depending on the message
updateStates(&prevBuffer[0]);
// increment the log size
//logSize += (indexLast+1);
#ifdef _IN_PC_
// if in PC and loggin is enabled
if ((outFile != NULL)){
printState(outFile, prevException);
}
//test = alpha*test;
#endif
}
else{
#ifdef _IN_PC_
checkSumFail++;
//test = (1.0-alpha) + alpha*test;
#endif
}
// get everything ready to start all-over
previousComplete =1;
indexLast = 0;
headerFound = 0;
trailerFound = 0;
memset(prevBuffer, 0, sizeof(prevBuffer));
}else { // you ran out of bytes
// No More Bytes
noMoreBytes = 1;
}// else no star
} // big outer while (no more bytes)
#ifdef _IN_PC_
if (totalPackets>0){
//test = ((float)checkSumFail/(float)totalPackets);
test = (float)checkSumFail;
} else {
test = 0.0;
}
return test;
#endif
}
unsigned char getFilterOnOff (void){
return filterControlData;
}
// ================================
// hardware in the loop methods
// ================================
void hil_getRawRead(short * rawData){
rawData[0] = rawControlData.gyroX.shData;
rawData[1] = rawControlData.gyroY.shData;
rawData[2] = rawControlData.gyroZ.shData;
rawData[3] = rawControlData.accelX.shData;
rawData[4] = rawControlData.accelY.shData;
rawData[5] = rawControlData.accelZ.shData;
rawData[6] = rawControlData.magX.shData;
rawData[7] = rawControlData.magY.shData;
rawData[8] = rawControlData.magZ.shData;
rawData[9] = rawControlData.baro.shData;
rawData[10] = rawControlData.pito.shData;
rawData[11] = rawControlData.powr.shData;
rawData[12] = rawControlData.ther.shData;
}
void readIpc (unsigned char* bufferedData) {
// fix the data length so if the interrupt adds data
// during execution of this block, it will be read
// until the next readIpc
unsigned int tmpLen = getLength(protBuffer);
unsigned int i=0;
unsigned int availBytes = 0, sendMore = 0;
unsigned char failureTrue = 0;
//static unsigned long long timeStamp = 0;
// Set the output size accordingly
bufferedData[0] = (tmpLen > MAXLOGLEN)? MAXLOGLEN: tmpLen;
// TODO: Remove debugging info from readIPC
//if ((timeStamp % 1000)== 0){
// printToUart2("T: %6.0f\n\r\0",(float) timeStamp*0.01);
//}
//timeStamp++;
// write the data
for(i = 1; i <= bufferedData[0]; i += 1 )
{
bufferedData[i] = readFront(protBuffer);
}
if (getOverflow(protBuffer)>0) {
// disable the SPI module
SPI1STATbits.SPIEN = 0;
// Disable the interrupts
IEC0bits.SPI1IE = 0;
IFS0bits.SPI1IF = 0;
printToUart2("\n=== %s =====\n\r\n\r\n\r", "BEGIN DUMP ");
//printToUart2("Ts: %f\n\r\0",(float) timeStamp*0.01);
printToUart2("Ovrflw: %d\n\r", getOverflow(protBuffer));
printToUart2("Head: %d\n\r", readHead(protBuffer));
printToUart2("Tail: %d\n\r", readTail(protBuffer));
printToUart2("Len: %d\n\r", getLength(protBuffer));
printToUart2("Siz: %d\n\r", protBuffer->size);
for(i = 0; i <BSIZE; i ++ )
{
printToUart2("%d ", protBuffer->buffer[i]);
}
printToUart2("\n=== %s =====\n\r\n\r\n\r", "END ");
// Empty the buffer
makeEmpty(protBuffer);
// Enable the interrupts
IFS0bits.SPI1IF = 0;
IEC0bits.SPI1IE = 1;
// Enable the SPI module
SPI1STATbits.SPIEN = 1;
}
}
void midgSplit (unsigned char * data) {
// Static Variables: CAREFUL
static unsigned char prevBuffer [2*MAXLOGLEN];
static unsigned char previousComplete =1;
static unsigned char indexLast = 0;
static unsigned char incompleteLen = 0;
// local variables
unsigned char i;
unsigned char headerFound=0, noMoreBytes = 1;
// FIXME: this requires MIDG_CHUNKSIZE < 257.
// if data[0] is 255, then i overflows from 255 to 0 and this is infinite
// Add the received bytes to the protocol parsing circular buffer
for(i = 1; i <= data[0]; i += 1 )
{
writeBack(midgBuffer, data[i]);
}
// update the noMoreBytes flag accordingly
noMoreBytes = (data[0]>0)?0:1;
while (!noMoreBytes){
// if the previous message was complete then read from the circular buffer
// and make sure that you are into the begining of a message
if(previousComplete){
while (!headerFound && !noMoreBytes) {
// move along until you find a 0x81 or run out of bytes
while (getLength(midgBuffer)>3 && peak(midgBuffer)!=0x81){
readFront(midgBuffer);
}
// if you found a dollar then make sure the next one is an A1
// You always make sure to have 4 bytes remaining (including
// BOF so you can read the ID and the length)
if(getLength(midgBuffer)>3 && peak(midgBuffer) == 0x81){
// read it
prevBuffer[indexLast++] = readFront(midgBuffer);
// if next is a 0xA1
if (peak(midgBuffer) == 0xA1){
// read the sync
prevBuffer[indexLast++] = readFront(midgBuffer);
// read the ID
prevBuffer[indexLast++] = readFront(midgBuffer);
// read the count
prevBuffer[indexLast++] = readFront(midgBuffer);
// start monitoring the pending count
incompleteLen = prevBuffer[indexLast -1];
// and signal you found a header
headerFound = 1;
// and set as incomplete the sentece
previousComplete = 0;
}
} else {
noMoreBytes = 1;
} // else no dollar
} // while we found header && no more bytes
} // if previous complete
// At this point either you found a header from a previous complete
// or you are reading from a message that was incomplete the last time
// in any of those two cases, keep reading until you run out of bytes
// or incompleteLen == 0
while ((incompleteLen > 0) && !noMoreBytes){
while (incompleteLen >0 && getLength(midgBuffer)>2){
prevBuffer[indexLast++] = readFront(midgBuffer);
incompleteLen --;
}
// if you completed the Length of the payload and you have
// at least two bytes to read to get the checksum
if (incompleteLen == 0 && getLength(midgBuffer)>=2){
// read the checksum 0
prevBuffer[indexLast++] = readFront(midgBuffer);
// read the checksum 1
prevBuffer[indexLast++] = readFront(midgBuffer);
// Parse the message.
// Note that parse does the checksumming
midgParse(prevBuffer);
// Reset the variables
previousComplete =1;
indexLast = 0;
headerFound = 0;
memset(prevBuffer, 0, sizeof(prevBuffer));
} else {
noMoreBytes =1;
}
}
} // big outer loop
}
void protDecodeMavlink(void) {
uint8_t indx, writeSuccess, commChannel = 1;
mavlink_param_set_t set;
mavlink_message_t msg;
mavlink_status_t status;
// uint8_t* dataIn;
// fix the data length so if the interrupt adds data
// during execution of this block, it will be read
// until the next gsRead
unsigned int tmpLen = getLength(uartMavlinkInBuffer), i = 0;
// if the buffer has more data than the max size, set it to max,
// otherwise set it to the length
//DatafromGSmavlink[0] = (tmpLen > MAXINLEN) ? MAXINLEN : tmpLen;
// read the data
//for (i = 1; i <= DatafromGSmavlink[0]; i += 1) {
//mavlink_parse_char(commChannel, readFront(uartBufferInMavlink), &msg, &status);
//DatafromGSmavlink[i] = readFront(uartMavlinkInBuffer);
//}
//dataIn = DatafromGSmavlink;
// increment the age of heartbeat
mlPending.heartbeatAge++;
for (i = 0; i <= tmpLen; i++) {
// Try to get a new message
if (mavlink_parse_char(commChannel, readFront(uartMavlinkInBuffer), &msg, &status)) {
// Handle message
switch (msg.msgid) {
case MAVLINK_MSG_ID_HEARTBEAT:
mavlink_msg_heartbeat_decode(&msg, &mlHeartbeat);
// Reset the heartbeat
mlPending.heartbeatAge = 0;
break;
//AM DBG
case MAVLINK_MSG_ID_MISSION_COUNT:
if (!mlPending.wpTransaction && (mlPending.wpProtState == WP_PROT_IDLE)) {
mavlink_msg_mission_count_decode(&msg, &mlWpCount);
// Start the transaction
mlPending.wpTransaction = 1;
// change the state
mlPending.wpProtState = WP_PROT_GETTING_WP_IDLE;
// reset the rest of the state machine
mlPending.wpTotalWps = mlWpCount.count;
mlPending.wpCurrentWpInTransaction = 0;
mlPending.wpTimeOut = 0;
}
break;
case MAVLINK_MSG_ID_MISSION_REQUEST_LIST:
// if there is no transaction going on
if (!mlPending.wpTransaction && (mlPending.wpProtState == WP_PROT_IDLE)) {
// Start the transaction
mlPending.wpTransaction = 1;
// change the state
mlPending.wpProtState = WP_PROT_LIST_REQUESTED;
// reset the rest of the state machine
mlPending.wpCurrentWpInTransaction = 0;
mlPending.wpTimeOut = 0;
}
break;
case MAVLINK_MSG_ID_MISSION_REQUEST:
mavlink_msg_mission_request_decode(&msg, &mlWpRequest);
if (mlPending.wpTransaction && (mlWpRequest.seq < mlWpValues.wpCount)) {
// change the state
mlPending.wpProtState = WP_PROT_TX_WP;
// reset the rest of the state machine
mlPending.wpCurrentWpInTransaction = mlWpRequest.seq;
mlPending.wpTimeOut = 0;
} else {
// TODO: put here a report for a single WP, i.e. not inside a transaction
}
break;
case MAVLINK_MSG_ID_MISSION_ACK:
mavlink_msg_mission_ack_decode(&msg, &mlWpAck);
if (mlPending.wpTransaction) {
// End the transaction
mlPending.wpTransaction = 0;
// change the state
mlPending.wpProtState = WP_PROT_IDLE;
// reset the rest of the state machine
mlPending.wpCurrentWpInTransaction = 0;
mlPending.wpTimeOut = 0;
// send current waypoint index
mlPending.wpSendCurrent = TRUE;
}
break;
case MAVLINK_MSG_ID_MISSION_ITEM:
writeSuccess = SUCCESS;
mavlink_msg_mission_item_decode(&msg, &mlSingleWp);
if (mlPending.wpTransaction && (mlPending.wpProtState == WP_PROT_RX_WP)) {
mlPending.wpProtState = WP_PROT_GETTING_WP_IDLE;
}
indx = (uint8_t) mlSingleWp.seq;
mlWpValues.lat[indx] = mlSingleWp.x;
mlWpValues.lon[indx] = mlSingleWp.y;
mlWpValues.alt[indx] = mlSingleWp.z;
mlWpValues.type[indx] = mlSingleWp.command;
mlWpValues.orbit[indx] = (uint16_t) mlSingleWp.param3;
/*
// Record the data in EEPROM
writeSuccess = storeWaypointInEeprom(&mlSingleWp);
// Set the flag of Aknowledge for the AKN Message
// if the write was not successful
if (writeSuccess != SUCCESS) {
mlPending.wpAck++;
mlWpAck.target_component = MAV_COMP_ID_MISSIONPLANNER;
mlWpAck.type = MAV_MISSION_ERROR;
}
*/
break;
case MAVLINK_MSG_ID_MISSION_CLEAR_ALL:
writeSuccess = SUCCESS;
// clear the WP values in memory;
memset(&mlWpValues, 0, sizeof (mavlink_mission_item_values_t));
/*
writeSuccess = clearWaypointsFrom(0);
// Set the flag of Aknowledge fail
// if the write was unsuccessful
if (writeSuccess != SUCCESS) {
mlPending.statustext++;
mlStatustext.severity = MAV_SEVERITY_ERROR;
strncpy(mlStatustext.text, "Failed to clear waypoints from EEPROM.", 49);
}
*/
// Update the waypoint count
mlWpValues.wpCount = 0;
// Set the state machine ready to send the WP akn
mlPending.wpCurrentWpInTransaction = 0;
mlPending.wpTotalWps = 0;
mlPending.wpTransaction = 1;
mlPending.wpProtState = WP_PROT_GETTING_WP_IDLE;
break;
case MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN:
writeSuccess = SUCCESS;
memset(&mlSingleWp, 0, sizeof (mavlink_mission_item_t));
mavlink_msg_set_gps_global_origin_decode(&msg, &mlGSLocation);
mlSingleWp.x = (float) (mlGSLocation.latitude);
mlSingleWp.y = (float) (mlGSLocation.longitude);
mlSingleWp.z = (float) (mlGSLocation.altitude);
indx = (uint8_t) MAX_NUM_WPS - 1;
mlWpValues.lat[indx] = mlSingleWp.x;
mlWpValues.lon[indx] = mlSingleWp.y;
mlWpValues.alt[indx] = mlSingleWp.z;
mlWpValues.type[indx] = MAV_CMD_NAV_LAND;
mlWpValues.orbit[indx] = 0;
// Record the data in EEPROM
/*
writeSuccess = storeWaypointInEeprom(&mlSingleWp);
if (writeSuccess != SUCCESS) {
mlPending.statustext++;
mlStatustext.severity = MAV_SEVERITY_ERROR;
strncpy(mlStatustext.text, "Failed to write origin to EEPROM.", 49);
}
else {
mlPending.statustext++;
mlStatustext.severity = MAV_SEVERITY_INFO;
strncpy(mlStatustext.text, "Control DSC GPS origin set.", 49);
}
*/
break;
//AM DBG
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
mlPending.piTransaction = 1;
mlPending.piProtState = PI_SEND_ALL_PARAM;
mlPending.piCurrentParamInTransaction = 0;
break;
case MAVLINK_MSG_ID_PARAM_REQUEST_READ:
// If it was in the middle of a list transmission or there is already a param enqueued
mlPending.piTransaction = 1;
switch (mlPending.piProtState) {
case PI_IDLE:
mlPending.piBackToList = 0; // no need to go back
mlPending.piQIdx = -1; // no Index
mlPending.piCurrentParamInTransaction = mavlink_msg_param_request_read_get_param_index(&msg); // assign directly
mlPending.piProtState = PI_SEND_ONE_PARAM;
break;
case PI_SEND_ALL_PARAM:
mlPending.piBackToList = 1; // mark to go back
mlPending.piQIdx++; // done like this because when empty index = -1
mlPending.piQueue[mlPending.piQIdx] = mavlink_msg_param_request_read_get_param_index(&msg); // put in in queue
mlPending.piProtState = PI_SEND_ONE_PARAM;
break;
case PI_SEND_ONE_PARAM:
if (mlPending.piBackToList) {
mlPending.piQIdx++; // done like this because when empty index = -1
mlPending.piQueue[mlPending.piQIdx] = mavlink_msg_param_request_read_get_param_index(&msg); // put in in queue
}
mlPending.piProtState = PI_SEND_ONE_PARAM;
break;
}
break;
case MAVLINK_MSG_ID_PARAM_SET:
mavlink_msg_param_set_decode(&msg, &set);
if ((uint8_t) set.target_system == (uint8_t) SYSTEMID &&
(uint8_t) set.target_component == (uint8_t) COMPID) {
char* key = (char*) set.param_id;
uint8_t i, j;
uint8_t match;
for (i = 0; i < PAR_PARAM_COUNT; i++) {
match = 1;
for (j = 0; j < PARAM_NAME_LENGTH; j++) {
// Compare
if (((char) (mlParamInterface.param_name[i][j]))
!= (char) (key[j])) {
match = 0;
} // if
// End matching if null termination is reached
if (((char) mlParamInterface.param_name[i][j]) == '\0') {
break;
} // if
}// for j
// Check if matched
if (match) {
//sw_debug = 1;
// Only write and emit changes if there is actually a difference
// AND only write if new value is NOT "not-a-number"
// AND is NOT infinity
if (isFinite(set.param_value)) {
mlParamInterface.param[i] = set.param_value;
// Report back new value
mlPending.piBackToList = 0; // no need to go back
mlPending.piQIdx = -1; // no Index
mlPending.piCurrentParamInTransaction = i; // assign directly
mlPending.piProtState = PI_SEND_ONE_PARAM;
mlPending.piTransaction = 1;
} // if different and not nan and not inf
} // if match
}// for i
} // if addressed to this
break;
default:
break;
}
}
}
}
TEST_CASE("Peak muestra el proximo elemento a retirar", "Peak debe de mostrar el proximo elemento que readFront nos daria"){
struct CircBuffer buffer;
int x;
CBRef bp = &buffer;
newCircBuffer(bp);
writeBack(bp, 5);
REQUIRE(peak(bp) == 5);
writeBack(bp, 2);
REQUIRE(peak(bp) == 5);
x = readFront(bp);
REQUIRE_FALSE(peak(bp) == 5);
REQUIRE(peak(bp) == 2);
writeBack(bp, 6);
REQUIRE(peak(bp) == 2);
x = readFront(bp);
REQUIRE(peak(bp) == 6);
x = readFront(bp);
REQUIRE_FALSE(peak(bp) == 6);
REQUIRE(peak(bp) == 0);
}
TEST_CASE( "Anadir y quitar elementos", "La cola debe de anadir y remover elementos de manera adecuada" )
{
/*
* main.c
* Author: Ian R Goodbody
* Function: Implements maze navigation
*/
void main(void)
{
WDTCTL = WDTPW | WDTHOLD; // Stop watchdog timer
initRangeFinders();
initMotors();
stop();
P1DIR |= LEFT_LED|RIGHT_LED;
P1OUT &= ~(LEFT_LED|RIGHT_LED);
int fBuffer[BUFFER_LN]; // Code probably unnecessary but it cannot hurt
int lBuffer[BUFFER_LN];
int rBuffer[BUFFER_LN];
int fMedian;
int lMedian;
int rMedian;
unsigned int lWheelSpeed = 4040;
unsigned int rWheelSpeed = 4000;
unsigned int runTime = 350;
int i;
fillBuffers(fBuffer, lBuffer, rBuffer);
while(1)
{
P1OUT &= ~(LEFT_LED|RIGHT_LED); // turn off LEDs
stop();
for (i = BUFFER_LN; i > 0; i--)
{
readFront(fBuffer);
waitMiliseconds(1);
readLeft(lBuffer);
waitMiliseconds(1);
readRight(rBuffer);
waitMiliseconds(1);
}
fMedian = median(fBuffer);
lMedian = median(lBuffer);
rMedian = median(rBuffer);
if(fMedian < 0x01F0) // Crash into wall test; ~2.5 inch threshold
{
if(lMedian < 0x01FF) // There is no wall remotely close to the left side,
{ // Initiate sweeping turn
lWheelSpeed = 2600;
rWheelSpeed = 5000;
}
else if(lMedian < 0x0266) // Getting too far from the wall start turning in
{
P1OUT |= LEFT_LED; // Red LED on, green off
P1OUT &= ~RIGHT_LED;
rWheelSpeed = 4270;
lWheelSpeed = 4040;
}
else if(lMedian > 0x02E4) // Getting too close to the wall start turning out
{
P1OUT |= RIGHT_LED; // Green LED on, red off
P1OUT &= ~LEFT_LED;
rWheelSpeed = 3900;
lWheelSpeed = 4040;
}
else // Acceptable distance from wall, cruise forward normally
{
//P1OUT |= RIGHT_LED;
P1OUT &= ~(RIGHT_LED|LEFT_LED);
rWheelSpeed = 4000;
lWheelSpeed = 4040;
}
setLeftWheel(lWheelSpeed, FORWARD);
setRightWheel(rWheelSpeed, FORWARD);
runTime = 350;
}
else
{
// About to run into a wall, initiate hard right turn
P1OUT |= RIGHT_LED|LEFT_LED;
setLeftWheel(5080,FORWARD);
setRightWheel(5260, BACKWARD);
runTime = 450;
}
go();
waitMiliseconds(runTime);
}
}
