NT Bus Protocol
Jump to navigation
Jump to search
The information on this page refers to firmware v2.56e.
This page describes the protocol of the NT bus communication.
Hardware Details
The NT bus is nothing else than a standard TTL-UART, with parameters:
2.000.000 bps, 8 bits, 1 stop, no parity
The voltage levels on the Tx and Rx lines are 3.3 V.
The Tx pin of any NT module connected to the bus must be in a high-impedance state in normal conditions. This line should be pulled to high on the main board side of the bus, ideally with a pull up resistor in the kilo-Ohm range.
Concept
The NT bus is designed as a master-slave network, with one master, the main STorM32 board, and up to 15 slaves, the NT modules. Any slave connected to the bus needs a unique ID. The ID has 4 bits. ID = 0 is however used to address all NT slaves on the bus at once.
The slaves are either Talker&Listener or only Listener. At any point in time only one slave is permitted to be talking, i.e., send data to the master. Any slave may talk only in response to a command received by the master, a slave may not send any message on its own.
Currently these IDs are assigned:
#define NTBUS_ID_ALLMODULES 0 #define NTBUS_ID_IMU1 1 #define NTBUS_ID_IMU2 2 #define NTBUS_ID_MOTORALL 3 #define NTBUS_ID_MOTORPITCH 4 #define NTBUS_ID_MOTORROLL 5 #define NTBUS_ID_MOTORYAW 6 #define NTBUS_ID_CAMERA 7 #define NTBUS_ID_LOGGER 11 #define NTBUS_ID_IMU3 12
Communication Details
Communication from Master to Slaves
Every message emitted by the master begins with a start byte, which has three fields:
bit 7: always 1, indicates that it is a start byte bits 6,5,4: short command bits 3,2,1,0: ID of the NT module the message is for
#define NTBUS_STX 0x80 // 0b 1000 0000 #define NTBUS_SFMASK 0x70 // 0b 0111 0000 #define NTBUS_IDMASK 0x0F // 0b 0000 1111
The following short commands are defined:
#define NTBUS_FLASH 0x70 // 0b 0111 0000 #define NTBUS_RESET 0x50 // 0b 0101 0000 #define NTBUS_SET 0x40 // 0b 0100 0000 #define NTBUS_GET 0x30 // 0b 0011 0000 #define NTBUS_TRIGGER 0x10 // 0b 0001 0000 #define NTBUS_CMD 0x00 // 0b 0000 0000
The start byte may be followed by further bytes, but these bytes must have their 7th bit set to low!
Transmitted data must be protected by a crc byte. It is calculated by a XOR over all data bytes, with the 7th bit set to low.
Examples:
- The first message emitted by the master at the beginning of every new cycle is a group trigger, i.e.
0x90: triggers all NT modules
- When the data of the camera imu (IMU1) is requested by emitting
0xB1: get IMU1 data
- Eventually the new motor angles will be send to all motor modules, which is done by addressing them all, i.e.
0xC3 + 10x motor data bytes + crc: set all motor to the new positions
Communication from Slave to Master
Whenever a NT module sends data to the STorM32, it first must enable it's Tx line, send the data, and then disable the Tx line.
In contrast to the messages from the master to slaves, full bytes can be send, i.e., the 7th bit doesn't have to be 0, since by design a slave never sends a command but only data.
Error Handling
In the data stream from the master to the slaves only the start byte may have the 7th bit set high. The data parser must reset on this occasion, which establishes the mechanism to return to proper operation if some error should have occurred.
In addition the receiving UARTs should check for error conditions offered by the hardware. For a STM32F103 this would be e.g. the overrun, noise, and frame flags.
Flashing of NT Modules and Handling of CLI
The NT modules can be flashed via the STorM32 controller's USB port, and also the CLI of the NT modules can be accessed this way.
Technically this is achieved by the STorM32 controller's ability to establish a transparent/opaque tunnel to and from NT modules. The general procedure is as follows:
- First, a specific command needs to be send to the main controller's USB, to put the main controller into the respective tunnel mode, and to set the NT module into the respective mode too.
- Then one can communicate with the NT module via the main controller's USB as if a USB-TLL adpater would be directly connected to the NT module.
Comment: Some STorM32 boards do not have a USB port, in these cases it is usually the UART1 port which takes up the function.
Flashing of NT Modules
- Send 'xQTcNTFLASHxx' (without the quotation marks) to the main controller. The last 'xx' stands for the ID of the NT module, e.g., 'xQTcNTFLASH01' for the 1st IMU.
- This opens the tunnel and sends a command to the NT module (see next subsection), which puts it into system bootloader mode.
- The USB port is set to 115200 8E1, suitable e.g. for using STM32CubeProgrammer as usual.
- The tunnel will be closed and the system be reset after 30 seconds if communication has not started, or after 2 seconds after communication has stopped.
- This is currently implemented only for the assigned module IDs (see list above) except of NTBUS_ID_CAMERA.
Required Implementation in the NT module
The NT module will receive from the main controller a flash start byte addressed to its ID (NTBUS_STX & NTBUS_FLASH & NTBUS_ID), followed by the chars 'FLASH' (without quotation marks). Upon receiption of this finger print, the NT module needs to put itself into system bootloader mode.
Accessing NT Module CLI
- Send 'xQTcNTQMODExx' (without the quotation marks) to the main controller. The last 'xx' stands for the ID of the NT module, e.g., 'xQTcNTQMODE01' for the 1st IMU.
- This opens the tunnel and sends a command to the NT module (see next subsection), which puts it into CLI mode.
- The USB port is maintained at 115200 8N1, suitable e.g. for using with standard serial terminals.
Required Implementation in the NT module
The NT module will receive from the main controller a flash start byte addressed to its ID (NTBUS_STX & NTBUS_FLASH & NTBUS_ID), followed by the chars 'QMODE' (without quotation marks). Upon receiption of this finger print, the NT module needs to put itself into CLI mode. The serial communication must be kept at 2000000 8N1. The CLI mode must be stopped and the NT module be restarted when a '@Q' (without quotation marks) is received by the NT module.
Implementation Details
//STorM32 firmware v2.56e
//31. Aug. 2020
//is used by NT Logger to mark log files with a header, from which also the NT communication version can be retrieved
#define STORM32NTBUS_VERSION 003
#define STORM32NTBUS_VERSIONSTR "v0.03"
// NT MODULE ID LIST
#define NTBUS_ID_ALLMODULES 0
#define NTBUS_ID_IMU1 1
#define NTBUS_ID_IMU2 2
#define NTBUS_ID_MOTORALL 3
#define NTBUS_ID_MOTORPITCH 4
#define NTBUS_ID_MOTORROLL 5
#define NTBUS_ID_MOTORYAW 6
#define NTBUS_ID_CAMERA 7
#define NTBUS_ID_LOGGER 11
#define NTBUS_ID_IMU3 12
// STARTBYTE flags and masks
#define NTBUS_STX 0x80 // 0b 1000 0000
#define NTBUS_SFMASK 0x70 // 0b 0111 0000
#define NTBUS_IDMASK 0x0F // 0b 0000 1111
#define NTBUS_FLASH 0x70 // 0b 0111 0000
#define NTBUS_RESET 0x50 // 0b 0101 0000 -> 0xD0
#define NTBUS_SET 0x40 // 0b 0100 0000
#define NTBUS_GET 0x30 // 0b 0011 0000 -> 0xB0
#define NTBUS_TRIGGER 0x10 // 0b 0001 0000 -> 0x90
#define NTBUS_CMD 0x00 // 0b 0000 0000
// COMMANDS , this is the command byte after a NTBUS_STX|NTBUS_CMD|NTBUS_ID startbyte
typedef enum {
// general cmds
NTBUS_CMD_GETSTATUS = 1,
NTBUS_CMD_GETVERSIONSTR, //must be supported by any NT module
NTBUS_CMD_GETBOARDSTR, //must be supported by any NT module
NTBUS_CMD_GETCONFIGURATION, //must be supported by any NT module
__NTBUS_CMD_ACCGYRO1RAWDATA_V1 = 32, //DEPRECTAED
__NTBUS_CMD_ACCGYRO2RAWDATA_V1, //DEPRECTAED
__NTBUS_CMD_ACCGYRODATA_V1, //DEPRECTAED
NTBUS_CMD_PIDDATA, //35
NTBUS_CMD_PARAMETERDATA, //36
NTBUS_CMD_AHRS1DATA, //37
__NTBUS_CMD_AHRS2DATA, //NOT USED
__NTBUS_CMD_ACCGYRO3RAWDATA_V1, //DEPRECATED
NTBUS_CMD_ACCGYRO1RAWDATA_V2, //40
NTBUS_CMD_ACCGYRO2RAWDATA_V2, //41
__NTBUS_CMD_ACCGYRO3RAWDATA_V2, //DEPRECATED
NTBUS_CMD_ACCGYRO1DATA_V2, //43
NTBUS_CMD_ACCGYRO2DATA_V2, //44
NTBUS_CMD_ENCODERDATA, //45
NTBUS_CMD_STORM32LINKDATA, //46
NTBUS_CMD_TUNNELTX, //47
NTBUS_CMD_TUNNELRXGET, //48
NTBUS_CMD_AUTOPILOTSYSTEMTIME, //49
NTBUS_CMD_PIDINDATA, //50
NTBUS_CMD_FUNCTIONINPUTVALUES, //51
NTBUS_CMD_READLOGGERDATETIME = 114, //logger date & time
NTBUS_CMD_WRITELOGGERDATETIME = 115,
NTBUS_CMD_STOREMOTORCALIBRATION = 116, //motor encoder poles & ofs data
NTBUS_CMD_READMOTORCALIBRATION = 117,
NTBUS_CMD_STOREIMUCALIBRATION = 118, //gyro & acc calibration data
NTBUS_CMD_READIMUCALIBRATION = 119,
NTBUS_CMD_READSETUP = 120, //NOT USED
NTBUS_CMD_WRITESETUP, //NOT USED
NTBUS_CMD_STORESETUP, //NOT USED
NTBUS_CMD_DEBUGDATA = 127,
//the commands MUST be smaller than 128, since 7th bit must be zero
NTBUS_CMD_NOTUSED = 0xFF
} NTBUSCMDTYPE;
// NTBUS_CMD_GETSTATUS
// from STorM32: stx, cmdbyte -> to STorM32: 2 x u8 cmddata, crc
PACKED(
typedef struct {
char Status;
uint8_t State;
}) tNTBusCmdGetStatusData;
#define NTBUS_CMDGETSTATUS_DATALEN (sizeof(tNTBusCmdGetStatusData))
// NTBUS_CMD_GETVERSIONSTR
// from STorM32: stx, cmdbyte -> to STorM32: 16 x u8 cmddata, crc
PACKED(
typedef struct {
char VersionStr[16];
}) tNTBusCmdGetVersionStrData;
#define NTBUS_CMDGETVERSIONSTR_DATALEN (sizeof(tNTBusCmdGetVersionStrData))
// NTBUS_CMD_GETBOARDSTR
// from STorM32: stx, cmdbyte -> to STorM32: 16 x u8 cmddata, crc
PACKED(
typedef struct{
char BoardStr[16];
}) tNTBusCmdGetBoardStrData;
#define NTBUS_CMDGETBOARDSTR_DATALEN (sizeof(tNTBusCmdGetBoardStrData))
// NTBUS_CMD_GETCONFIGURATION
// from STorM32: stx, cmdbyte -> to STorM32: 1 x u16 cmddata, crc
PACKED(
typedef struct {
uint16_t Configuration;
}) tNTBusCmdGetConfigurationData;
#define NTBUS_CMDGETCONFIGURATION_DATALEN (sizeof(tNTBusCmdGetConfigurationData))
// structure to hold NT module configuration informations
typedef struct {
char VersionStr[16];
char BoardStr[16];
uint16_t Configuration;
} tNTModuleConfigurationInfo;
//-------------------------------------------------------
// IMU MODULE
// SET: -
// GET: 3xi16 acc + 3xi16 gyro + 1xi16 temp + 1xu8 imu status
// configuration word
// response to NTBUS_CMD_GETCONFIGURATION
#define NTBUS_IMU_CONFIG_MPU6050 0x0001
#define NTBUS_IMU_CONFIG_MPU6000 0x0002
#define NTBUS_IMU_CONFIG_MPU6500 0x0003
#define NTBUS_IMU_CONFIG_MPU9250 0x0004
#define NTBUS_IMU_CONFIG_ICM42605 0x0005
#define NTBUS_IMU_CONFIG_ICM42688 0x0006
#define NTBUS_IMU_CONFIG_MODELUNKNOWN 0x0000
#define NTBUS_IMU_CONFIG_MODELMASK 0x0007
#define NTBUS_IMU_CONFIG_OVERSAMPLED 0x0010
#define NTBUS_IMU_CONFIG_FASTGYRO 0x0020
#define NTBUS_IMU_CONFIG_FASTACC 0x0040
#define NTBUS_IMU_CONFIG_GYROFILTER 0x0080
#define NTBUS_IMU_CONFIG_ACCFILTER 0x0100
#define NTBUS_IMU_CONFIG_CALIBRATIONCMDSUPPORTED 0x8000
// status byte , is used for both internal bookkeeping and CMD command
// response to NTBUS_CMD_GETSTATUS
#define NTBUS_IMU_STATUS_IMU_PRESENT 0x80
// GET IMU imu status byte
// response to NTBUS_GET IMU
// also used in NTBUS_CMD_ACCGYRODATA
#define NTBUS_IMU_IMUSTATUS_BASE 0x01 //must always be set, so that status is always >0
#define NTBUS_IMU_IMUSTATE_OK 0x02 //this one is injected, comes from the STorM32 controller, not used by NT imu
#define NTBUS_IMU_IMUSTATUS_GYRODATA_OK 0x04
#define NTBUS_IMU_IMUSTATUS_ACCDATA_OK 0x08
// NTBUS_GET
// to STorM32: 6xi16 + 1xi16 + 1xu8 + crc = 1+14+1 bytes
PACKED(
typedef struct {
int16_t AccX; //+-4g
int16_t AccY; //+-4g
int16_t AccZ; //+-4g
int16_t GyroX; //+-1000°/s
int16_t GyroY; //+-1000°/s
int16_t GyroZ; //+-1000°/s
int16_t Temp; //xxx.xx C°
uint8_t ImuStatus; //status of the NT imu module
}) tNTBusGetImuData;
#define NTBUS_GETIMU_DATALEN (sizeof(tNTBusGetImuData))
// NTBUS_CMD_READIMUCALIBRATION & NTBUS_CMD_STOREIMUCALIBRATION
PACKED(
typedef struct {
int16_t AccScaleX;
int16_t AccScaleY;
int16_t AccScaleZ;
int16_t AccZeroX;
int16_t AccZeroY;
int16_t AccZeroZ;
int16_t GyroZeroX;
int16_t GyroZeroY;
int16_t GyroZeroZ;
}) tNTBusCmdReadStoreImuCalibration; //used for both read and store
// NTBUS_CMD_READIMUCALIBRATION
// from STorM32: stx, cmdbyte -> to STorM32: 9xi16 + crc = 18+1 bytes
#define NTBUS_CMDREADIMUCALIBRATION_DATALEN (sizeof(tNTBusCmdReadStoreImuCalibration))
// NTBUS_CMD_STOREIMUCALIBRATION
// from STorM32: 9xi16 = 18 bytes => 2 + 18 + 4 + 1 = 25 bytes
#define NTBUS_CMDSTOREIMUCALIBRATION_DATALEN (sizeof(tNTBusCmdReadStoreImuCalibration))
#define NTBUS_CMDSTOREIMUCALIBRATION_HIGHBITSLEN 4 //18 bytes => 3 bytes required => 2xu16 = 4 bytes used
// NTBUS_CMD_TUNNEL
// from STorM32: 1xu8 + 12xu8 = 13 bytes => 2 + 13 + 2 + 1 = 18 bytes = 90us
PACKED(
typedef struct {
uint8_t len;
uint8_t data[12];
}) tNTBusCmdTunnelTx;
#define NTBUS_CMDTUNNELTX_DATALEN (sizeof(tNTBusCmdTunnelTx))
#define NTBUS_CMDTUNNELTX_HIGHBITSLEN 2 //13 bytes => 2 bytes required => 1xu16 = 2 bytes used
// to STorM32: 1xu8 + 16xu8 = 17 byte => 17 + 1 = 18 bytes
PACKED(
typedef struct {
uint8_t len;
uint8_t data[16];
}) tNTBusCmdTunnelRxGet;
#define NTBUS_CMDTUNNELRXGET_DATALEN (sizeof(tNTBusCmdTunnelRxGet))
//-------------------------------------------------------
// MOTOR MODULE:
// SET: 1xu8 flags + 1xu8 vmax pitch + 1xi16 angle pitch + 1xu8 vmax roll + 1xi16 angle roll + 1xu8 vmax yaw + 1xi16 angle yaw
// GET: 1xi16 + 1xu8 flags
// configuration word
// response to NTBUS_CMD_GETCONFIGURATION
#define NTBUS_MOTOR_CONFIG_TLE5012B 0x0001
#define NTBUS_MOTOR_CONFIG_AS5048A 0x0002
#define NTBUS_MOTOR_CONFIG_POT 0x0003
#define NTBUS_MOTOR_CONFIG_HALL 0x0004
#define NTBUS_MOTOR_CONFIG_ENCODERUNKNOWN 0x0000
#define NTBUS_MOTOR_CONFIG_ENCODERMODELMASK 0x0007
#define NTBUS_MOTOR_CONFIG_FOC 0x1000 //this is still set, but is obsolete, as it is always set
// status byte , is used for both internal bookkeeping and CMD command
// response to NTBUS_CMD_GETSTATUS
#define NTBUS_MOTOR_STATUS_ENCODER_PRESENT 0x80
// SET MOTOR flag byte
#define NTBUS_MOTOR_ENABLEFLAG_BEEP 0x40
#define NTBUS_MOTOR_ENABLEFLAG_GLOBAL 0x10 //this MUST agree with Motor flag
#define NTBUS_MOTOR_ENABLEFLAG_PITCH 0x01 //this MUST agree with Motor flag
#define NTBUS_MOTOR_ENABLEFLAG_ROLL 0x02 //this MUST agree with Motor flag
#define NTBUS_MOTOR_ENABLEFLAG_YAW 0x04 //this MUST agree with Motor flag
#define NTBUS_MOTOR_FLAG_FOC 0x20 //this is to indicate that this is a FOC command
// GET MOTOR status byte
// response to NTBUS_GET MOTOR
#define NTBUS_MOTOR_MOTORSTATUS_BASE 0x01 //must always be set, so that status is always >0
#define NTBUS_MOTOR_MOTORSTATUS_ENCDATA_OK 0x04
// NTBUS_SET
// from STorM32: stx + data + crc = 1 + 10 + 1 bytes
// it has it's "own" way to ensure that the 7th bits are zero
PACKED(
typedef struct {
uint8_t Flags;
uint8_t VmaxPitch;
int16_t AnglePitch;
uint8_t VmaxRoll;
int16_t AngleRoll;
uint8_t VmaxYaw;
int16_t AngleYaw;
}) tNTBusSetMotorAllData; //that's the data structure when sinusoidal-drive motor data is send
PACKED(
typedef struct {
uint8_t Flags;
uint8_t SpeedPitchHigh;
int16_t SpeedPitch;
uint8_t SpeedRollHigh;
int16_t SpeedRoll;
uint8_t SpeedYawHigh;
int16_t SpeedYaw;
}) tNTBusSetMotorAllDataVFoc; //that's the data structure when FOC-drive motor data is send
#define NTBUS_SETMOTORALL_DATALEN (sizeof(tNTBusSetMotorAllData))
// NTBUS_GET
PACKED(
typedef struct {
int16_t EncoderAngle;
uint8_t MotorStatus; //status of the NT motor module
}) tNTBusGetMotorData;
#define NTBUS_GETMOTOR_DATALEN (sizeof(tNTBusGetMotorData))
// NTBUS_CMD_READMOTORCALIBRATION & NTBUS_CMD_STOREMOTORCALIBRATION
PACKED(
typedef struct {
int16_t MotorPolePairs;
int32_t MotorOfs;
}) tNTBusCmdReadStoreMotorCalibration; //used for both read and store
// NTBUS_CMD_READMOTORCALIBRATION
// from STorM32: stx, cmdbyte -> to STorM32: 1xi16 + 1xi32 + crc = 6+1 bytes
#define NTBUS_CMDREADMOTORCALIBRATION_DATALEN (sizeof(tNTBusCmdReadStoreMotorCalibration))
// NTBUS_CMD_STOREMOTORCALIBRATION
// from STorM32: 1xi16 + 1xi32 = 6 bytes => 2 + 6 + 2 + 1 = 11 bytes
#define NTBUS_CMDSTOREMOTORCALIBRATION_DATALEN (sizeof(tNTBusCmdReadStoreMotorCalibration))
#define NTBUS_CMDSTOREMOTORCALIBRATION_HIGHBITSLEN 2 //6 data bytes => 1 bytes required => 1xu16 = 2 bytes used
//-------------------------------------------------------
// CAMERA MODULE:
// SET: 1xu8 flags + 1xu8 camera model + 1xu8 function + 1xu8 unused + 1xu8 pwm
// GET: -
// configuration word
// response to NTBUS_CMD_GETCONFIGURATION
#define NTBUS_CAMERA_CONFIG_DEFAULT 0x0000
// flags byte //not yet used
#define NTBUS_CAMERA_FLAG_CNTRL1ENABLED 0x01
#define NTBUS_CAMERA_FLAG_CNTRL2ENABLED 0x02
#define NTBUS_CAMERA_FLAG_OUTENABLED 0x10
#define NTBUS_CAMERA_FLAG_SET2 0x40 //this is to map two data into SET
// NTBUS_SET
// from STorM32: stx + data + crc = 1 + 5 + 1 bytes
// it has it's "own" way to ensure that the 7th bits are zero
PACKED(
typedef struct {
uint8_t Flags;
uint8_t CameraModel;
uint8_t CameraCmd; //0 = ignore, shifted by 1 as in RemoteCamera
uint8_t CameraCmdValue; //0 = ignore, 1000 ...2000us -> 1...101 => pwm with 10us resolution
uint8_t Pwm; //0 = ignore, 1000 ...2000us -> 1...101 => pwm with 10us resolution
}) tNTBusSetCameraData;
// NTBUS_SET 2
// from STorM32: stx + data + crc = 1 + 5 + 1 bytes
// it has it's "own" way to ensure that the 7th bits are zero
PACKED(
typedef struct {
uint8_t Flags;
uint8_t CameraCmd2; //0 = ignore, shifted by 1 as in RemoteCamera
uint8_t CameraCmd2Value; //0 = ignore, 1000 ...2000us -> 1...101 => pwm with 10us resolution
uint8_t noutuse1;
uint8_t noutuse2;
}) tNTBusSetCameraData2;
#define NTBUS_SETCAMERA_DATALEN (sizeof(tNTBusSetCameraData))
//-------------------------------------------------------
// LOGGER MODULE:
// SET: 1xu32 + 7xu8 + 5xu16 + 6xi16 + 3xu8
// GET: -
// configuration word
// response to NTBUS_CMD_GETCONFIGURATION
#define NTBUS_LOGGER_CONFIG_DEFAULT 0x0000
// NTBUS_SET VERSION V3
// from STorM32: 1xu32 + 7xu8 + 5xu16 + 6xi16 + 3xu8 = 36 bytes => 1 + 36 + 6 + 1 = 44 bytes
PACKED(
typedef struct {
uint32_t TimeStamp32; // in us
uint8_t Imu1received; // in 10us units
uint8_t Imu1done; // in 10us units
uint8_t PIDdone; // in 10us units
uint8_t Motorsdone; // in 10us units
uint8_t Imu2done; // in 10us units
uint8_t Loggingdone; // in 10us units
uint8_t Loopdone; // in 10us units
uint16_t State;
uint16_t Status;
uint16_t Status2;
uint16_t ErrorCnt;
uint16_t Voltage;
int16_t Imu1AnglePitch;
int16_t Imu1AngleRoll;
int16_t Imu1AngleYaw;
int16_t Imu2AnglePitch;
int16_t Imu2AngleRoll;
int16_t Imu2AngleYaw;
uint8_t highres1;
uint8_t highres2;
uint8_t highres3;
}) tNTBusSetLoggerDataV3;
#define NTBUS_SETLOGGERV3_DATALEN (sizeof(tNTBusSetLoggerDataV3))
#define NTBUS_SETLOGGERV3_HIGHBITSLEN 6 //36 bytes => 6 bytes required => 3xu16 = 6 bytes used
// NTBUS_CMD_ACCGYRODATA VERSION2
// from STorM32: 6xi16 + 1xu8 = 13 bytes => 2 + 13 + 2 + 1 = 18 bytes
PACKED(
typedef struct {
int16_t ax;
int16_t ay;
int16_t az;
int16_t gx;
int16_t gy;
int16_t gz;
uint8_t ImuState; //status of the imu as seen by STorM32
}) tNTBusCmdAccGyroDataV2;
#define NTBUS_CMDACCGYRODATAV2_DATALEN (sizeof(tNTBusCmdAccGyroDataV2))
#define NTBUS_CMDACCGYRODATAV2_HIGHBITSLEN 2 //13 bytes => 2 bytes required => 1xu16 = 2 bytes used
// NTBUS_CMD_AHRSDATA
// from STorM32: 5xi16 = 10 bytes => 2 + 10 + 2 + 1 = 15 bytes
PACKED(
typedef struct {
int16_t rx;
int16_t ry;
int16_t rz;
int16_t accconfidence;
int16_t yawtarget;
}) tNTBusCmdAhrsData;
#define NTBUS_CMDAHRSDATA_DATALEN (sizeof(tNTBusCmdAhrsData))
#define NTBUS_CMDAHRSDATA_HIGHBITSLEN 2 //10 bytes => 2 bytes required => 1xu16 = 2 bytes used
// NTBUS_CMD_PIDDATA
// from STorM32: 6xi16 = 12 bytes => 2 + 12 + 2 + 1 = 17 bytes
PACKED(
typedef struct {
int16_t PIDCntrlPitch;
int16_t PIDCntrlRoll;
int16_t PIDCntrlYaw;
int16_t PIDMotorCntrlPitch;
int16_t PIDMotorCntrlRoll;
int16_t PIDMotorCntrlYaw;
}) tNTBusCmdPidData;
#define NTBUS_CMDPIDDATA_DATALEN (sizeof(tNTBusCmdPidData))
#define NTBUS_CMDPIDDATA_HIGHBITSLEN 2 //12 bytes => 2 bytes required => 1xu16 = 2 bytes used
// NTBUS_CMD_PIDINDATA
// from STorM32: 3xi24 + 3xi16 = 15 bytes => 2 + 15 + 4 + 1 = 22 bytes
PACKED(
typedef struct {
int16_t PIDErrorAnglePitch;
int16_t PIDErrorAngleRoll;
int16_t PIDErrorAngleYaw;
uint8_t highres1;
uint8_t highres2;
uint8_t highres3;
int16_t PIDSetPointPitch;
int16_t PIDSetPointRoll;
int16_t PIDSetPointYaw;
}) tNTBusCmdPidInData;
#define NTBUS_CMDPIDINDATA_DATALEN (sizeof(tNTBusCmdPidInData))
#define NTBUS_CMDPIDINDATA_HIGHBITSLEN 4 //15 bytes => 3 bytes required => 2xu16 = 4 bytes used
// NTBUS_CMD_ACCGYRO1RAWDATA, NTBUS_CMD_ACCGYRO2RAWDATA V2
// from STorM32: 7xi16 = 14 bytes => 2 + 14 + 2 + 1 = 19 bytes
PACKED(
typedef struct {
int16_t axraw;
int16_t ayraw;
int16_t azraw;
int16_t gxraw;
int16_t gyraw;
int16_t gzraw;
int16_t temp;
}) tNTBusCmdAccGyroRawDataV2;
#define NTBUS_CMDACCGYRORAWDATAV2_DATALEN (sizeof(tNTBusCmdAccGyroRawDataV2))
#define NTBUS_CMDACCGYRORAWDATAV2_HIGHBITSLEN 2 //14 bytes => 2 bytes required => 1xu16 = 2 bytes used
// NTBUS_CMD_ENCODERDATA
// from STorM32: 3xi16 + 1xu8 = 7 bytes => 2 + 7 + 2 + 1 = 12 bytes
PACKED(
typedef struct {
int16_t EncoderPitch;
int16_t EncoderRoll;
int16_t EncoderYaw;
uint8_t MotState; //status of all motors as seen by STorM32
}) tNTBusCmdEncoderData;
#define NTBUS_CMDENCODERDATA_DATALEN (sizeof(tNTBusCmdEncoderData))
#define NTBUS_CMDENCODERDATA_HIGHBITSLEN 2 //7 bytes => 1 bytes required => 1xu16 = 2 bytes used
// NTBUS_CMD_PARAMETERDATA
// from STorM32: 3xi16 + 16xu8 = 22 bytes => 2 + 22 + 4 + 1 = 29 bytes
PACKED(
typedef struct{
uint16_t Adr;
uint16_t Value;
uint16_t Format;
char NameStr[16];
}) tNTBusCmdParameterData;
#define NTBUS_CMDPARAMETERDATA_DATALEN (sizeof(tNTBusCmdParameterData))
#define NTBUS_CMDPARAMETERDATA_HIGHBITSLEN 4 //22 bytes => 4 bytes required => 2xu16 = 4 bytes used
// NTBUS_CMD_STORM32LINKDATA
// from STorM32: 8xi16 + 2xu8 = 18 bytes => 2 + 18 + 4 + 1 = 25 bytes
#define NTBUS_STORM32LINK_STATUSFLAG_PRESENT 0x01
#define NTBUS_STORM32LINK_STATUSFLAG_OK 0x02 //INUSE is not useful, since it's set on first occasion of OK
#define NTBUS_STORM32LINK_STATUSFLAG_QFIX 0x04
#define NTBUS_STORM32LINK_STATUSFLAG_3DFIX 0x08
#define NTBUS_STORM32LINK_STATUSFLAG_AHRSFIX 0x10
#define NTBUS_STORM32LINK_STATUSFLAG_HDCINUSE 0x20
#define NTBUS_STORM32LINK_STATUSFLAG_UPDATED 0x80
PACKED(
typedef struct {
int16_t q0; //1 = 10000
int16_t q1;
int16_t q2;
int16_t q3;
int16_t vx; //cm/s => +-327m/s = +-1177km/h
int16_t vy; //cm/s
int16_t vz; //cm/s
int16_t YawRateCmd;
uint8_t FCStatus;
uint8_t LinkStatus;
}) tNTBusCmdStorm32LinkData;
#define NTBUS_CMDSTORM32LINKDATA_DATALEN (sizeof(tNTBusCmdStorm32LinkData))
#define NTBUS_CMDSTORM32LINKDATA_HIGHBITSLEN 4 //18 bytes => 3 bytes required => 2xu16 = 4 bytes used
// NTBUS_CMD_DEBUGDATA
// from STorM32: 7xi16 = 14 bytes => 2 + 14 + 2 + 1 = 19 bytes
PACKED(
typedef struct {
int16_t debug1;
int16_t debug2;
int16_t debug3;
int16_t debug4;
int16_t debug5;
int16_t debug6;
int16_t debug7;
}) tNTBusCmdDebugData;
#define NTBUS_CMDDEBUGDATA_DATALEN (sizeof(tNTBusCmdDebugData))
#define NTBUS_CMDDEBUGDATA_HIGHBITSLEN 2 //14 bytes => 2 bytes required => 1xu16 = 2 bytes used
// NTBUS_CMD_READWRITELOGGERADTETIME
// from STorM32: 1xi16 + 5xi8 = 7 bytes => 2 + 7 + 2 + 1 = 12 bytes
PACKED(
typedef struct {
uint16_t year;
uint8_t month;
uint8_t day;
uint8_t hour;
uint8_t minute;
uint8_t second;
}) tNTBusCmdReadWriteLoggerDateTime; //used for both read and write
// NTBUS_CMD_READLOGGERTIME
// from STorM32: stx, cmdbyte -> to STorM32: 1xu16 + 5xu8 + crc = 7+1 bytes
#define NTBUS_CMDREADLOGGERDATETIME_DATALEN (sizeof(tNTBusCmdReadWriteLoggerDateTime))
// NTBUS_CMD_WRITELOGGERTIME
// from STorM32: 1xu16 + 5xu8 = 7 bytes => 2 + 7 + 2 + 1 = 12 bytes
#define NTBUS_CMDWRITELOGGERDATETIME_DATALEN (sizeof(tNTBusCmdReadWriteLoggerDateTime))
#define NTBUS_CMDWRITELOGGERDATETIME_HIGHBITSLEN 2 //7 bytes => 2 bytes required => 1xu16 = 2 bytes used
// NTBUS_CMD_AUTOPILOTSYSTEMTIME
// from STorM32: 1xu64 = 8 bytes => 2 + 8 + 2 + 1 = 13 bytes
PACKED(
typedef struct {
uint64_t unix_time;
}) tNTBusCmdAutopilotSystemTime;
#define NTBUS_CMDAUTOPILOTSYSTEMTIME_DATALEN (sizeof(tNTBusCmdAutopilotSystemTime))
#define NTBUS_CMDAUTOPILOTSYSTEMTIME_HIGHBITSLEN 2 //8 bytes => 2 bytes required => 1xu16 = 2 bytes used
// NTBUS_CMD_FUNCTIONINPUTVALUES
// from STorM32: 14xu8 = 14 bytes => 2 + 14 + 2 + 1 = 19 bytes
PACKED(
typedef struct {
int8_t Pitch;
int8_t Roll;
int8_t Yaw;
int8_t PanMode;
int8_t StandBy;
int8_t Camera;
int8_t ReCenterCamera;
int8_t Script1;
int8_t Script2;
int8_t Script3;
int8_t Script4;
int8_t PwmOut;
int8_t Camera2;
int8_t reserve;
}) tNTBusCmdFunctionInputValues;
#define NTBUS_CMDFUNCTIONINPUTVALUES_DATALEN (sizeof(tNTBusCmdFunctionInputValues))
#define NTBUS_CMDFUNCTIONINPUTVALUES_HIGHBITSLEN 2 //14 bytes => 2 bytes required => 1xu16 = 2 bytes used
//-------------------------------------------------------
// IMU MODULE SPECIFIC routines
// send from imu module to STorM32
void ntbus_putimudata(void *imu, uint8_t imustatus)
{
uint8_t i; uint8_t crc; char c;
crc = 0;
for( i=0; i<NTBUS_GETIMU_DATALEN-1; i++ ){ c = *((u8*)(imu++)); ntbus_putc( c ); crc ^= c; }
c = imustatus;
ntbus_putc( c ); crc ^= c;
ntbus_putc( crc );
}
void ntbus_putcmddata_wcrc(void *data, uint16_t len)
{
uint8_t i; uint8_t crc; char c;
crc = 0;
for( i=0; i<len; i++ ){ c = *((u8*)(data++)); ntbus_putc( c ); crc ^= c; }
ntbus_putc( crc );
}
// send from STorM32 to imu module
#define NTBUS_IMUDATA_ISOK (NTBUS_IMU_IMUSTATUS_GYRODATA_OK|NTBUS_IMU_IMUSTATUS_ACCDATA_OK)
uint16_t ntbus_getimudata(uint32_t tmo)
{
if( ntbus_getdatawcrc(NTBUS_GETIMU_DATALEN+1,tmo) ){
if( (ntbus_buf[NTBUS_GETIMU_DATALEN-1] & NTBUS_IMUDATA_ISOK) == NTBUS_IMUDATA_ISOK ) return 1;
}
_ntbus.errorcnt++;
return 0;
}
void ntbus_storecalibration_imumodule(uint8_t id, tNTBusCmdReadStoreImuCalibration* calibration)
{
ntbus_putcmd( id, NTBUS_CMD_STOREIMUCALIBRATION );
_ntbus_senddata( calibration, NTBUS_CMDSTOREIMUCALIBRATION_DATALEN ); //2 + 18 + 4 + 1 = 25 = 125us
}
// read calibration data from imu module
uint16_t ntbus_readcalibration_imumodule(uint8_t id, tNTBusCmdReadStoreImuCalibration* calibration)
{
uint16_t config = 0;
ntbus_putcmd( id, NTBUS_CMD_GETCONFIGURATION );
if( !ntbus_getdatawcrc(NTBUS_CMDGETCONFIGURATION_DATALEN+1,50) ) return 0;
memcpy( &config, ntbus_buf, sizeof(config) );
if( config & NTBUS_IMU_CONFIG_CALIBRATIONCMDSUPPORTED ){
ntbus_putcmd( id, NTBUS_CMD_READIMUCALIBRATION );
if( ntbus_getdatawcrc(NTBUS_CMDREADIMUCALIBRATION_DATALEN+1,50) ){
memcpy( calibration, ntbus_buf, sizeof(tNTBusCmdReadStoreImuCalibration) );
return 1;
}
}
return 0;
}
// initialize imu module when it had been found
// returns 1 or 0x03
uint16_t ntbus_ready_imumodule(uint8_t id)
{
uint16_t i, ret;
for(i=0; i<100; i++){
ntbus_putcmd( id, NTBUS_CMD_GETSTATUS );
ret = ntbus_getdatawcrc( NTBUS_CMDGETSTATUS_DATALEN+1, 50 ); //wait only for 50 us
if( ret && (ntbus_buf[0] == NTBUS_IMU_STATUS_IMU_PRESENT) && (ntbus_buf[1] == NTBUS_STATE_READY) ){
//read it once to startup
ntbus_putsf_wflushall( id, NTBUS_TRIGGER );
ntbus_putsf( id, NTBUS_GET );
ntbus_getdatawcrc( NTBUS_GETIMU_DATALEN+1, 500 );
return 3;
}
delay_ms(50);
}
return 1;
}
// find and initialize imu module
// returns 0, 0x01 or 0x03
uint16_t ntbus_find_and_ready_imumodule(uint8_t id)
{
uint16_t n;
n = ntbus_find_module( id, 10 );
if( n == 0 ){ ntbus_reseterrorcnt(); return 0; }
n = ntbus_ready_imumodule( id );
ntbus_reseterrorcnt();
return n;
}
//-------------------------------------------------------
// MOTOR MODULE SPECIFIC routines
// send from motor module to STorM32
void ntbus_putmotordata(void *m, uint8_t motorstatus) //must be void, otherwise m++ goes wrong
{
uint8_t i; uint8_t crc; char c;
crc = 0;
for( i=0; i<NTBUS_GETMOTOR_DATALEN-1; i++ ){ c = *((u8*)(m++)); ntbus_putc( c ); crc ^= c; }
c = motorstatus;
ntbus_putc( c ); crc ^= c;
ntbus_putc( crc );
}
// send from STorM32 to motor module
#define NTBUS_MOTORDATA_ISOK (NTBUS_MOTOR_MOTORSTATUS_ENCDATA_OK)
uint16_t ntbus_getmotordata(uint32_t tmo)
{
if( ntbus_getdatawcrc(NTBUS_GETMOTOR_DATALEN+1,tmo) ){
if( (ntbus_buf[NTBUS_GETMOTOR_DATALEN-1] & NTBUS_MOTORDATA_ISOK) == NTBUS_MOTORDATA_ISOK ) return 1;
}
_ntbus.errorcnt++;
return 0;
}
// send from STorM32 to all motor modules
void ntbus_setmotoralldata(uint16_t enabledflag, uint16_t beep, tu16Angles vmax, tu16Angles angle)
{
uint8_t b, crc;
b = 0;
if( enabledflag & MOTORGLOBALENABLED ) b |= NTBUS_MOTOR_ENABLEFLAG_GLOBAL;
if( enabledflag & MOTORPITCHENABLED ) b |= NTBUS_MOTOR_ENABLEFLAG_PITCH;
if( enabledflag & MOTORROLLENABLED ) b |= NTBUS_MOTOR_ENABLEFLAG_ROLL;
if( enabledflag & MOTORYAWENABLED ) b |= NTBUS_MOTOR_ENABLEFLAG_YAW;
if( beep ) b |= NTBUS_MOTOR_ENABLEFLAG_BEEP;
ntbus_putsf( NTBUS_ID_MOTORALL, NTBUS_SET ); //DO NOT FLUSH THE RX CHANNEL!
crc = 0;
ntbus_putc( b ); crc ^= b;
ANGLESENUM axis;
for( axis=PITCH; axis<=YAW; axis++ ){
b = (u8)(vmax.a[axis]>>1) & 0x7F;
ntbus_putc( b ); crc ^= b;
uint16_t a = angle.a[axis];
b = (u8)(a) & 0x7F; //low byte
ntbus_putc( b ); crc ^= b;
b = (u8)(a >> 7) & 0x7F; //high byte
ntbus_putc( b ); crc ^= b;
}
ntbus_putc( crc & 0x7F );
}
// send from STorM32 to all motor modules
void ntbus_setmotoralldataVFoc(uint16_t enabledflag, uint16_t beep, tq16Angles qU)
{
uint8_t b, crc;
b = NTBUS_MOTOR_FLAG_FOC;
if( enabledflag & MOTORGLOBALENABLED ) b |= NTBUS_MOTOR_ENABLEFLAG_GLOBAL;
if( enabledflag & MOTORPITCHENABLED ) b |= NTBUS_MOTOR_ENABLEFLAG_PITCH;
if( enabledflag & MOTORROLLENABLED ) b |= NTBUS_MOTOR_ENABLEFLAG_ROLL;
if( enabledflag & MOTORYAWENABLED ) b |= NTBUS_MOTOR_ENABLEFLAG_YAW;
if( beep ) b |= NTBUS_MOTOR_ENABLEFLAG_BEEP;
ntbus_putsf( NTBUS_ID_MOTORALL, NTBUS_SET ); //DO NOT FLUSH THE RX CHANNEL!
crc = 0;
ntbus_putc( b ); crc ^= b;
ANGLESENUM axis;
for( axis=PITCH; axis<=YAW; axis++ ){
q16 a = qU.a[axis];
if( a > +Q16(4.0f) ) a = +Q16(4.0f); //the q16 is packed into 21 bits, including sign, thus [-16...+16[ is maximum
if( a < -Q16(4.0f) ) a = -Q16(4.0f);
b = (u8)(a) & 0x7F; //low byte
ntbus_putc( b ); crc ^= b;
b = (u8)(a >> 7) & 0x7F; //high byte
ntbus_putc( b ); crc ^= b;
b = (u8)(a >> 14) & 0x7F; //highest byte
ntbus_putc( b ); crc ^= b;
}
ntbus_putc( crc & 0x7F );
}
// read calibration data from motor module
uint16_t ntbus_readcalibration_motormodule(uint8_t id, tNTBusCmdReadStoreMotorCalibration* calibration)
{
ntbus_putcmd( id, NTBUS_CMD_READMOTORCALIBRATION );
if( ntbus_getdatawcrc( NTBUS_CMDREADMOTORCALIBRATION_DATALEN+1, 50 ) ){
memcpy( calibration, ntbus_buf, sizeof(tNTBusCmdReadStoreMotorCalibration) );
return 1;
}
return 0;
}
// initialize motor module when it had been found
// returns 1 or 0x03
uint16_t ntbus_ready_motormodule(uint8_t id, uint8_t* status)
{
uint16_t i, ret;
for(i=0; i<100; i++){
ntbus_putcmd( id, NTBUS_CMD_GETSTATUS );
ret = ntbus_getdatawcrc( NTBUS_CMDGETSTATUS_DATALEN+1, 50 ); //wait only for 50 us
if( ret && (ntbus_buf[1] == NTBUS_STATE_READY) ){
*status = ntbus_buf[0];
//read it once to startup
ntbus_putsf_wflushall( id, NTBUS_TRIGGER );
ntbus_putsf( id, NTBUS_GET );
ntbus_getdatawcrc( NTBUS_GETMOTOR_DATALEN+1, 150 );
return 3;
}
delay_ms(50);
}
return 1;
}
// find and initialize imu module
// returns 0, 0x01 or 0x03
uint16_t ntbus_find_and_ready_motormodule(uint8_t id, uint8_t* status)
{
uint16_t n;
*status = 0;
n = ntbus_find_module( id, 40 ); //the motors modules somehow sometimes can take long to startup
if( n == 0 ){ ntbus_reseterrorcnt(); return 0; }
n = ntbus_ready_motormodule( id, status );
ntbus_reseterrorcnt();
return n;
}
//-------------------------------------------------------
// CAMERA MODULE SPECIFIC routines
void ntbus_setcameradata(uint16_t flags, uint16_t model, uint16_t cmd, uint16_t cmdvalue, uint16_t pwm)
{
uint8_t b, crc;
ntbus_putsf( NTBUS_ID_CAMERA, NTBUS_SET ); //DO NOT FLUSH THE RX CHANNEL!
crc = 0;
b = (u8)(flags & 0x007F); ntbus_putc( b ); crc ^= b;
b = (u8)(model & 0x007F); ntbus_putc( b ); crc ^= b;
b = (u8)(cmd & 0x007F); ntbus_putc( b ); crc ^= b;
b = (u8)(cmdvalue & 0x007F); ntbus_putc( b ); crc ^= b; //it needs to be converted outside, to also allow 0
b = (u8)(pwm & 0x007F); ntbus_putc( b ); crc ^= b;
ntbus_putc( crc & 0x7F );
}
void ntbus_setcameradata2(uint16_t cmd2, uint16_t cmd2value)
{
uint8_t b, crc;
ntbus_putsf( NTBUS_ID_CAMERA, NTBUS_SET ); //DO NOT FLUSH THE RX CHANNEL!
crc = 0;
b = (u8)(NTBUS_CAMERA_FLAG_SET2 & 0x007F); ntbus_putc( b ); crc ^= b; //flags is 0x40 to indicate SET2
b = (u8)(cmd2 & 0x007F); ntbus_putc( b ); crc ^= b;
b = (u8)(cmd2value & 0x007F); ntbus_putc( b ); crc ^= b; //it needs to be converted outside, to also allow 0
b = (u8)(0 & 0x007F); ntbus_putc( b ); crc ^= b; //notused1
b = (u8)(0 & 0x007F); ntbus_putc( b ); crc ^= b; //notused2
ntbus_putc( crc & 0x7F );
}