static Eterm do_chksum(ChksumFun sumfun, Process *p, Eterm ioterm, int left,
void *sum, int *res, int *err)
{
Eterm *objp;
Eterm obj;
int c;
DECLARE_ESTACK(stack);
unsigned char *bytes = NULL;
int numbytes = 0;
*err = 0;
if (left <= 0 || is_nil(ioterm)) {
DESTROY_ESTACK(stack);
*res = 0;
return ioterm;
}
if(is_binary(ioterm)) {
Uint bitoffs;
Uint bitsize;
Uint size;
Eterm res_term = NIL;
unsigned char *bytes;
byte *temp_alloc = NULL;
ERTS_GET_BINARY_BYTES(ioterm, bytes, bitoffs, bitsize);
if (bitsize != 0) {
*res = 0;
*err = 1;
DESTROY_ESTACK(stack);
return NIL;
}
if (bitoffs != 0) {
bytes = erts_get_aligned_binary_bytes(ioterm, &temp_alloc);
/* The call to erts_get_aligned_binary_bytes cannot fail as
we'we already checked bitsize and that this is a binary */
}
size = binary_size(ioterm);
if (size > left) {
Eterm *hp;
ErlSubBin *sb;
Eterm orig;
Uint offset;
/* Split the binary in two parts, of which we
only process the first */
hp = HAlloc(p, ERL_SUB_BIN_SIZE);
sb = (ErlSubBin *) hp;
ERTS_GET_REAL_BIN(ioterm, orig, offset, bitoffs, bitsize);
sb->thing_word = HEADER_SUB_BIN;
sb->size = size - left;
sb->offs = offset + left;
sb->orig = orig;
sb->bitoffs = bitoffs;
sb->bitsize = bitsize;
sb->is_writable = 0;
res_term = make_binary(sb);
size = left;
}
(*sumfun)(sum, bytes, size);
*res = size;
DESTROY_ESTACK(stack);
erts_free_aligned_binary_bytes(temp_alloc);
return res_term;
}
if (!is_list(ioterm)) {
*res = 0;
*err = 1;
DESTROY_ESTACK(stack);
return NIL;
}
/* OK a list, needs to be processed in order, handling each flat list-level
as they occur, just like io_list_to_binary would */
*res = 0;
ESTACK_PUSH(stack,ioterm);
while (!ESTACK_ISEMPTY(stack) && left) {
ioterm = ESTACK_POP(stack);
if (is_nil(ioterm)) {
/* ignore empty lists */
continue;
}
if(is_list(ioterm)) {
L_Again: /* Restart with sublist, old listend was pushed on stack */
objp = list_val(ioterm);
obj = CAR(objp);
for(;;) { /* loop over one flat list of bytes and binaries
until sublist or list end is encountered */
if (is_byte(obj)) {
int bsize = 0;
for(;;) {
if (bsize >= numbytes) {
if (!bytes) {
bytes = erts_alloc(ERTS_ALC_T_TMP,
numbytes = 500);
} else {
if (numbytes > left) {
numbytes += left;
} else {
numbytes *= 2;
}
bytes = erts_realloc(ERTS_ALC_T_TMP, bytes,
numbytes);
}
}
bytes[bsize++] = (unsigned char) unsigned_val(obj);
--left;
ioterm = CDR(objp);
if (!is_list(ioterm)) {
break;
}
objp = list_val(ioterm);
obj = CAR(objp);
if (!is_byte(obj))
break;
if (!left) {
break;
}
}
(*sumfun)(sum, bytes, bsize);
*res += bsize;
} else if (is_nil(obj)) {
ioterm = CDR(objp);
if (!is_list(ioterm)) {
break;
}
objp = list_val(ioterm);
obj = CAR(objp);
} else if (is_list(obj)) {
/* push rest of list for later processing, start
again with sublist */
ESTACK_PUSH(stack,CDR(objp));
ioterm = obj;
goto L_Again;
} else if (is_binary(obj)) {
int sres, serr;
Eterm rest_term;
rest_term = do_chksum(sumfun, p, obj, left, sum, &sres,
&serr);
*res += sres;
if (serr != 0) {
*err = 1;
DESTROY_ESTACK(stack);
if (bytes != NULL)
erts_free(ERTS_ALC_T_TMP, bytes);
return NIL;
}
left -= sres;
if (rest_term != NIL) {
Eterm *hp;
hp = HAlloc(p, 2);
obj = CDR(objp);
ioterm = CONS(hp, rest_term, obj);
left = 0;
break;
}
ioterm = CDR(objp);
if (is_list(ioterm)) {
/* objp and obj need to be updated if
loop is to continue */
objp = list_val(ioterm);
obj = CAR(objp);
}
} else {
*err = 1;
DESTROY_ESTACK(stack);
if (bytes != NULL)
erts_free(ERTS_ALC_T_TMP, bytes);
return NIL;
}
if (!left || is_nil(ioterm) || !is_list(ioterm)) {
break;
}
} /* for(;;) */
} /* is_list(ioterm) */
if (!left) {
#ifdef ALLOW_BYTE_TAIL
if (is_byte(ioterm)) {
/* inproper list with byte tail*/
Eterm *hp;
hp = HAlloc(p, 2);
ioterm = CONS(hp, ioterm, NIL);
}
#else
;
#endif
} else if (!is_list(ioterm) && !is_nil(ioterm)) {
/* inproper list end */
#ifdef ALLOW_BYTE_TAIL
if (is_byte(ioterm)) {
unsigned char b[1];
b[0] = (unsigned char) unsigned_val(ioterm);
(*sumfun)(sum, b, 1);
++(*res);
--left;
ioterm = NIL;
} else
#endif
if is_binary(ioterm) {
int sres, serr;
ioterm = do_chksum(sumfun, p, ioterm, left, sum, &sres, &serr);
*res +=sres;
if (serr != 0) {
*err = 1;
DESTROY_ESTACK(stack);
if (bytes != NULL)
erts_free(ERTS_ALC_T_TMP, bytes);
return NIL;
}
left -= sres;
} else {
*err = 1;
DESTROY_ESTACK(stack);
if (bytes != NULL)
erts_free(ERTS_ALC_T_TMP, bytes);
return NIL;
}
}
} /* while left and not estack empty */
void send_telemetry(struct sensordata *sensorData_ptr, struct nav *navData_ptr, struct control *controlData_ptr, struct mission *missionData_ptr, uint16_t cpuLoad)
{
int bytes=0;
unsigned short flags=0;
unsigned long tmp;
uint16_t tele_data[23], output_CKSUM=0;
static byte sendpacket[TELE_PACKET_SIZE]={'U','U','T',};
// Build send_telemetry data packet
tmp = (unsigned long)( sensorData_ptr->imuData_ptr->time*1e04 ); // time buffer will now overflow after 59.6 hrs (thats what 4 bytes' worth!)
memcpy(&tele_data[0],&tmp,4);
tele_data[2] = (uint16_t)(0);
tele_data[3] = (uint16_t)(0);
tele_data[4] = (uint16_t)( sensorData_ptr->imuData_ptr->p*R2D / 200.0 * 0x7FFF ); //rate = 200 deg/s max saturation
tele_data[5] = (uint16_t)( sensorData_ptr->imuData_ptr->q*R2D / 200.0 * 0x7FFF );
tele_data[6] = (uint16_t)( sensorData_ptr->imuData_ptr->r*R2D / 200.0 * 0x7FFF );
tele_data[7] = (uint16_t)( (sensorData_ptr->adData_ptr->h) / 10000.0 * 0x7FFF ); // max AGL Alt.(m) = 10000 m
tele_data[8] = (uint16_t)( (sensorData_ptr->adData_ptr->ias) / 80.0 * 0x7FFF ); // max Indicated Airspeed(IAS) = 80 m/s
tele_data[9] = (uint16_t)( navData_ptr->psi*R2D / 180.0 * 0x7FFF ); // Euler angles [psi,theta,phi] (deg)
tele_data[10]= (uint16_t)( navData_ptr->the*R2D / 90.0 * 0x7FFF );
tele_data[11]= (uint16_t)( navData_ptr->phi*R2D / 180.0 * 0x7FFF );
tele_data[12]= (uint16_t)( (controlData_ptr->da_r-controlData_ptr->da_l)/2 / R_AILERON_MAX * 0x7FFF ); // control surface commands (normalized 0-1)
tele_data[13]= (uint16_t)( controlData_ptr->de / ELEVATOR_MAX * 0x7FFF );
tele_data[14]= (uint16_t)( controlData_ptr->dthr * 0x7FFF );
tele_data[15]= (uint16_t)( controlData_ptr->dr / RUDDER_MAX * 0x7FFF );
tele_data[16] = cpuLoad; /* cpuload */
tmp = (unsigned long)( sensorData_ptr->gpsData_ptr->lon *1e07 );
memcpy(&tele_data[17],&tmp,4);
tmp = (unsigned long)( sensorData_ptr->gpsData_ptr->lat *1e07 );
memcpy(&tele_data[19],&tmp,4);
//if (ofpMode == standby) flags = flags | 0x01;
if (missionData_ptr->mode == 2) flags = flags | 0x01<<1; // Autopilot mode
if (missionData_ptr->mode == 1) flags = flags | 0x01<<4; // Manual mode
if ( (sensorData_ptr->imuData_ptr->err_type != checksum_err) && (sensorData_ptr->imuData_ptr->err_type != got_invalid) ) flags = flags | 0x01<<6;
if (sensorData_ptr->gpsData_ptr->err_type == data_valid || sensorData_ptr->gpsData_ptr->err_type == incompletePacket) flags = flags | 0x01<<7;
if (sensorData_ptr->gpsData_ptr->navValid == 0) flags = flags | 0x01<<8;
tele_data[21] = flags;
tele_data[22] = (uint16_t)sensorData_ptr->gpsData_ptr->satVisible;
memcpy(&sendpacket[3],&tele_data,46);
// compute 16-bit checksum of output data (excluding the header)
output_CKSUM = do_chksum (sendpacket, 2,TELE_PACKET_SIZE-2);
*(uint16_t *)&sendpacket[49] = output_CKSUM;
// send send_telemetry data packet to serial port
while(bytes != TELE_PACKET_SIZE) bytes += write(port, &sendpacket[bytes], TELE_PACKET_SIZE-bytes); bytes=0;
// Send status message if present
if (statusMsg[0] != 0){
while(bytes != 103) bytes += write(port, &statusMsg[bytes], 103-bytes); bytes=0;
statusMsg[0] =0;
}
}
int read_gps(struct gps *gpsData_ptr)
{
// serial port buffer handle
cyg_io_handle_t port_handle;
cyg_serial_buf_info_t buff_info;
unsigned int len = sizeof (buff_info);
// get serial port handle
cyg_io_lookup( gpsData_ptr->portName, &port_handle );
cyg_io_get_config (port_handle, CYG_IO_GET_CONFIG_SERIAL_BUFFER_INFO,\
&buff_info, &len);
unsigned int bytesInBuffer = buff_info.rx_count;
unsigned int bytesReadThisCall =0;;
unsigned short msgPayloadSize = 0, bytesToRead = 0, bytesRead = 0;
int status = 0;
// Initialization of persistent local buffer
if (gpsData_ptr->localBuffer == NULL)
{
gpsData_ptr->localBuffer = (unsigned char*) malloc (1024 * sizeof (unsigned char));
}
// First check if there are any bytes in the serial buffer, return if none
if( bytesInBuffer == 0 )
return -1;
// Get localBuffer stored in gps packet. This is to keep the following code readable
localBuffer = gpsData_ptr->localBuffer;
bytesInLocalBuffer= gpsData_ptr->bytesInLocalBuffer;
readState = gpsData_ptr->readState;
// Keep reading until we've processed all of the bytes in the buffer
while (bytesReadThisCall < bytesInBuffer){
switch (readState){
case 0: //Look for packet header bytes
// Read in up to 2 bytes to the first open location in the local buffer
bytesRead = read(gpsData_ptr->port,&localBuffer[bytesInLocalBuffer],2-bytesInLocalBuffer);
bytesReadThisCall += bytesRead; // keep track of bytes read during this call
if (localBuffer[0] == 0xA0){ // Check for first header byte
bytesInLocalBuffer = 1;
if (localBuffer[1] == 0xA2){ // Check for second header byte
bytesInLocalBuffer = 2;
readState++; // header complete, move to next stage
}
}
else {
gpsData_ptr->err_type = noPacketHeader;
}
break; // end case 0
case 1: //Look for message size bytes
// Find how many bytes need to be read for data length (2) - bytes already read (includes 2 byte header)
bytesToRead = 4 - bytesInLocalBuffer;
// Read in up to 2 bytes to the first open location in the local buffer
bytesRead = read(gpsData_ptr->port,&localBuffer[bytesInLocalBuffer],bytesToRead);
bytesReadThisCall += bytesRead; // keep track of bytes read during this call
bytesInLocalBuffer += bytesRead; // keep track of bytes in local buffer
if (bytesRead == bytesToRead){
readState++; // size bytes complete, move to next stage
}
else{
//printf ("\n<GPS>: Failed to get size bytes");
gpsData_ptr->err_type = incompletePacket;
}
break; // end case 1
case 2: //Read payload, checksum, and stop bytes
// Find message payload size
msgPayloadSize = getbeuw(localBuffer,2)&0x7FFF; // packet size is 15 bits. bitwise AND ensures only 15 bits are used.
//printf("<GPS>: msgPayloadSize: %d\n",msgPayloadSize);
// Error checking on payload size. If size is bigger than expected, dump packet
if(msgPayloadSize > GPS_MAX_MSG_SIZE){
gpsData_ptr->err_type = incompletePacket;
reset_localBuffer();
}
// Find how many bytes need to be read for the total message (Header (2) + Size (2) + Payload + checksum (2) + stop (2) - bytes already read )
bytesToRead = msgPayloadSize + 8 - bytesInLocalBuffer;
// Read in the remainder of the message to the local buffer, starting at the first empty location
bytesRead = read (gpsData_ptr->port, &localBuffer[bytesInLocalBuffer], bytesToRead);
bytesInLocalBuffer += bytesRead; // keep track of bytes in local buffer
bytesReadThisCall += bytesRead; // keep track of bytes read during this call
if (bytesRead == bytesToRead){
//printf ("\n<GPS>: Got complete message! Tried for %d, got %d",bytesToRead,bytesRead);
//printf("<GPS>: Packet ID %d \n", localBuffer[4]);
//printf("<GPS>: msgPayloadSize: %d\n",msgPayloadSize);
//printf("<GPS>: My checksum: %d Recv: %d\n",do_chksum(localBuffer, 4, msgPayloadSize+4),getbeuw(localBuffer,4+msgPayloadSize));
// Checksum verification (15-bit, big endian)
if ( (do_chksum(localBuffer, 4, msgPayloadSize+4)&0x7FFF) == (getbeuw(localBuffer,4+msgPayloadSize)&0x7FFF) ){
// If it's OK, extract data
parse_gps( gpsData_ptr );
gpsData_ptr->err_type = data_valid;
//printf ("\n<GPS>: Success!");
status = 1;
}
else{
//printf ("\n<GPS>: Checksum mismatch!");
gpsData_ptr->err_type = checksum_err;
}
reset_localBuffer(); // Clear the local buffer regardless
}
else{
//printf ("<GPS>: Didn't get complete message. Tried for %d, got %d. %d\n",bytesToRead,bytesRead,msgPayloadSize);
gpsData_ptr->err_type= incompletePacket;
status = 0;
}
break; // end case 2
default:
reset_localBuffer();
printf ("\n<GPS>: Why are you here?");
status = 0;
break; // end default
} // end switch (readState)
} // end while (bytesReadThisCall < bytesInBuffer)
// Store local buffer in gps packet
gpsData_ptr->localBuffer = localBuffer;
gpsData_ptr->bytesInLocalBuffer = bytesInLocalBuffer;
gpsData_ptr->readState = readState;
return status;
}
// How to handle static variables with multiple sensors? objects? add to gpspacket?
int read_gps(struct gps *gpsData_ptr)
{
cyg_io_handle_t port_handle;
cyg_serial_buf_info_t buff_info;
unsigned int len = sizeof (buff_info);
// get serial port handle
cyg_io_lookup( gpsData_ptr->portName, &port_handle );
cyg_io_get_config (port_handle, CYG_IO_GET_CONFIG_SERIAL_BUFFER_INFO,\
&buff_info, &len);
unsigned int bytesInBuffer = buff_info.rx_count;
unsigned int bytesReadThisCall =0;;
unsigned short msgPayloadSize = 0, bytesToRead = 0, bytesRead = 0;
int status =0;
// Initialization of persistent local buffer
if (gpsData_ptr->localBuffer == NULL)
{
gpsData_ptr->localBuffer = (unsigned char*) malloc (1024 * sizeof (unsigned char));
}
// First check if there are any bytes in the serial buffer, return if none
if( bytesInBuffer == 0 )
return -1;
// Get localBuffer stored in gps packet. This is to keep the following code readable
localBuffer = gpsData_ptr->localBuffer;
bytesInLocalBuffer= gpsData_ptr->bytesInLocalBuffer;
readState = gpsData_ptr->readState;
//fprintf(stderr, "read state is %d (before while)\n", readState);
// Keep reading until we've processed all of the bytes in the buffer
while (bytesReadThisCall < bytesInBuffer){
//fprintf(stderr, "read state is %d (after while)\n", readState);
switch (readState){
case 0: //Look for packet header bytes
// Read in up to 4 bytes to the first open location in the local buffer
bytesRead = read(gpsData_ptr->port,&localBuffer[bytesInLocalBuffer],4-bytesInLocalBuffer);
bytesReadThisCall += bytesRead; // keep track of bytes read during this call
if (localBuffer[0] == '$'){ // Check for first header byte
bytesInLocalBuffer = 1;
//fprintf(stderr, "case 0, $ header type \n");
if (localBuffer[1] == 'B'){ // Check for second header byte
bytesInLocalBuffer = 2;
if (localBuffer[2] == 'I'){ // Check for third header byte
bytesInLocalBuffer = 3;
if (localBuffer[3] == 'N'){ // Check for fourth header byte
bytesInLocalBuffer = 4;
readState++; // header complete, move to next stage
}
}
}
}
else {
gpsData_ptr->err_type = noPacketHeader;
}
break; // end case 0
case 1: // Look for block ID and data length
// Find how many bytes need to be read for block ID (2) + data length (2) - bytes already read (includes 4 byte header)
bytesToRead = 8 - bytesInLocalBuffer;
// Read in bytes to the last location in the local buffer
bytesRead = read(gpsData_ptr->port,&localBuffer[bytesInLocalBuffer],bytesToRead);
bytesInLocalBuffer += bytesRead; // keep track of bytes in local buffer
bytesReadThisCall += bytesRead; // keep track of bytes read during this call
if (bytesRead == bytesToRead){
readState++;
//printf ("\n<GPS>: Got msgID: %d and Data Length: %d", localBuffer[5]*256 + localBuffer[4], localBuffer[7]*256 + localBuffer[6]);
}
else{
gpsData_ptr->err_type = incompletePacket;
}
break; // end case 1
case 2: //Read payload, checksum, and stop bytes
// Find message payload size
msgPayloadSize = localBuffer[7]*256 + localBuffer[6]; // data is in little endian format
// Error checking on payload size. If size is bigger than expected, dump packet
if(msgPayloadSize > GPS_MAX_MSG_SIZE){
gpsData_ptr->err_type = incompletePacket;
reset_localBuffer();
}
// Find how many bytes need to be read for the total message (Header (4) + ID (2) + Size (2) + Payload + checksum (2) + stop (2) - bytes already read )
bytesToRead = msgPayloadSize + 12 - bytesInLocalBuffer;
// Read in the remainder of the message to the local buffer, starting at the first empty location
bytesRead = read (gpsData_ptr->port, &localBuffer[bytesInLocalBuffer], bytesToRead);
bytesInLocalBuffer += bytesRead; // keep track of bytes in local buffer
bytesReadThisCall += bytesRead; // keep track of bytes read during this call
if (bytesRead == bytesToRead){
//printf ("\n<GPS>: Got complete message! Tried for %d, got %d",bytesToRead,bytesRead);
//printf ("\n<GPS>: My checksum: %d Recv: %d",do_chksum(localBuffer, 8, msgPayloadSize+8),(localBuffer[8+msgPayloadSize+1]*256 + localBuffer[8+msgPayloadSize]));
// Checksum verification
if ( do_chksum(localBuffer, 8, msgPayloadSize+8) == (localBuffer[8+msgPayloadSize+1]*256 + localBuffer[8+msgPayloadSize]) ){
// If it's OK, extract data
parse_gps( gpsData_ptr );
gpsData_ptr->err_type = data_valid;
//printf ("\n<GPS>: Success!");
status = 1;
}
else{
//printf ("\n<GPS>: Checksum mismatch!");
gpsData_ptr->err_type = checksum_err;
}
reset_localBuffer();
}
else{
//printf ("\n<GPS>: Didn't get complete message. Tried for %d, got %d",bytesToRead,bytesRead);
gpsData_ptr->err_type= incompletePacket;
status = 0;
}
break; // end case 2
default:
reset_localBuffer();
printf ("\n<GPS>: Why are you here?");
status = 0;
break; // end default
} // end switch (readState)
} // end while (bytesReadThisCall < bytesInBuffer)
// Store local buffer in gps packet
gpsData_ptr->localBuffer = localBuffer;
gpsData_ptr->bytesInLocalBuffer = bytesInLocalBuffer;
gpsData_ptr->readState = readState;
return status;
}
