/* AUTO-GENERATED FILE. DO NOT MODIFY.
*
* This class was automatically generated by the
* java mavlink generator tool. It should not be modified by hand.
*/
// MESSAGE CONTROL_SYSTEM_STATE PACKING
package com.MAVLink.common;
import com.MAVLink.MAVLinkPacket;
import com.MAVLink.Parser;
import com.MAVLink.ardupilotmega.CRC;
import java.nio.ByteBuffer;
import org.junit.Test;
import static org.junit.Assert.assertArrayEquals;
/**
* The smoothed, monotonic system state used to feed the control loops of the system.
*/
public class msg_control_system_state_test{
public static final int MAVLINK_MSG_ID_CONTROL_SYSTEM_STATE = 146;
public static final int MAVLINK_MSG_LENGTH = 100;
private static final long serialVersionUID = MAVLINK_MSG_ID_CONTROL_SYSTEM_STATE;
private Parser parser = new Parser();
public CRC generateCRC(byte[] packet){
CRC crc = new CRC();
for (int i = 1; i < packet.length - 2; i++) {
crc.update_checksum(packet[i] & 0xFF);
}
crc.finish_checksum(MAVLINK_MSG_ID_CONTROL_SYSTEM_STATE);
return crc;
}
public byte[] generateTestPacket(){
ByteBuffer payload = ByteBuffer.allocate(6 + MAVLINK_MSG_LENGTH + 2);
payload.put((byte)MAVLinkPacket.MAVLINK_STX); //stx
payload.put((byte)MAVLINK_MSG_LENGTH); //len
payload.put((byte)0); //seq
payload.put((byte)255); //sysid
payload.put((byte)190); //comp id
payload.put((byte)MAVLINK_MSG_ID_CONTROL_SYSTEM_STATE); //msg id
payload.putLong((long)93372036854775807L); //time_usec
payload.putFloat((float)73.0); //x_acc
payload.putFloat((float)101.0); //y_acc
payload.putFloat((float)129.0); //z_acc
payload.putFloat((float)157.0); //x_vel
payload.putFloat((float)185.0); //y_vel
payload.putFloat((float)213.0); //z_vel
payload.putFloat((float)241.0); //x_pos
payload.putFloat((float)269.0); //y_pos
payload.putFloat((float)297.0); //z_pos
payload.putFloat((float)325.0); //airspeed
//vel_variance
payload.putFloat((float)353.0);
payload.putFloat((float)354.0);
payload.putFloat((float)355.0);
//pos_variance
payload.putFloat((float)437.0);
payload.putFloat((float)438.0);
payload.putFloat((float)439.0);
//q
payload.putFloat((float)521.0);
payload.putFloat((float)522.0);
payload.putFloat((float)523.0);
payload.putFloat((float)524.0);
payload.putFloat((float)633.0); //roll_rate
payload.putFloat((float)661.0); //pitch_rate
payload.putFloat((float)689.0); //yaw_rate
CRC crc = generateCRC(payload.array());
payload.put((byte)crc.getLSB());
payload.put((byte)crc.getMSB());
return payload.array();
}
@Test
public void test(){
byte[] packet = generateTestPacket();
for(int i = 0; i < packet.length - 1; i++){
parser.mavlink_parse_char(packet[i] & 0xFF);
}
MAVLinkPacket m = parser.mavlink_parse_char(packet[packet.length - 1] & 0xFF);
byte[] processedPacket = m.encodePacket();
assertArrayEquals("msg_control_system_state", processedPacket, packet);
}
}