Checksum not working?

classic Classic list List threaded Threaded
2 messages Options
Reply | Threaded
Open this post in threaded view
|  
Report Content as Inappropriate

Checksum not working?

Paul Ozog
I have a Pelican hooked up to a power supply.  Using the USB adapter (not the X-Bee), I poll the autopilot for the LL_STATUS struct defined in the documentation.  I store the data from the serial port into a buffer ('buf').  The struct's values seem reasonable (the PSU reads 11.5V)

(gdb) p (quad_LlStatusStruct)buf[3 + 2 + 1]
{battery_voltage_1 = 11461, battery_voltage_2 = 0, status = 0, cpu_load = 1000, compass_enabled = 1 '\001', chksum_error = 0 '\000', flying = 0 '\000', motors_on = 1 '\001', flightMode = 129, up_time = 0}

The checksum according to the buffer get from the serial port says the checksum is 119:
(gdb) p buf[3 + 2 + 1 + lengthOfStructure]
119 'w'

(gdb) p lengthOfStructure
16

buf is the buffer I receive from the serial port.  3 bytes for the preamble ">*>", 2 for the length of the struct (which is 16), and 1 for the packet descriptor (0x01 as it should be for LL_STATUS).  

Using the crc16 function from the documentation, the checksum comes out to be:
45368

Which one is more reasonable?  s the documentation wrong?  Am I misunderstanding the checksum entry in the reply from the Autopilot?  All of my data is reasonable, except for the checksum.

Thanks.
Reply | Threaded
Open this post in threaded view
|  
Report Content as Inappropriate

Re: Checksum not working?

Bill Morris
Administrator
On Wed, 2011-03-23 at 15:35 -0700, Paul Ozog [via asctec-users] wrote:

> buf is the buffer I receive from the serial port.  3 bytes for the
> preamble ">*>", 2 for the length of the struct (which is 16), and 1
> for the packet descriptor (0x01 as it should be for LL_STATUS).  
>
> Using the crc16 function from the documentation, the checksum comes
> out to be:
> 45368
>
> Which one is more reasonable?  s the documentation wrong?  Am I
> misunderstanding the checksum entry in the reply from the Autopilot?
> All of my data is reasonable, except for the checksum.

The documentation could be clearer. It may or may not be helpful to take
a look at the ROS driver. http://www.ros.org/wiki/asctec_autopilot

I think the problem is that the crc is not calculated using the header.
crc_valid (packet_crc, &telemetry->LL_STATUS_, sizeof (packet_size))

  bool crc_valid (unsigned short packet_crc, void *data, unsigned short
cnt)
  {
    unsigned short checksum = crc16 (data, cnt);

    if (checksum == packet_crc)
    {
      return true;
    }
    return false;
  }

  unsigned short crc_update (unsigned short crc, unsigned char data)
  {
    data ^= (crc & 0xff);
    data ^= data << 4;

    return ((((unsigned short) data << 8) | ((crc >> 8) & 0xff)) ^
(unsigned char) (data >> 4)
            ^ ((unsigned short) data << 3));
  }

  unsigned short crc16 (void *data, unsigned short cnt)
  {
    unsigned short crc = 0xff;
    unsigned char *ptr = (unsigned char *) data;
    int i;

    for (i = 0; i < cnt; i++)
    {
      crc = crc_update (crc, *ptr);
      ptr++;
    }
    return crc;
  }


Loading...