void SDK_mainloop(void)
{
sdkCycleStartTime = T1TC;
WO_SDK.ctrl_mode = 0x02; // attitude and throttle control
WO_SDK.disable_motor_onoff_by_stick = 0;
sdkLoops++;
parseRxFifo();
// check for new LL command packet
if (packetCmdLL->updated)
{
cmdLLNew = 1;
packetCmdLL->updated = 0;
}
// check if LL commands arrive at max every CMD_MAX_PERIOD ms
if ((sdkLoops % CMD_MAX_PERIOD) == 0)
{
if (cmdLLNew == 1)
{
cmdLLNew = 0;
cmdLLValid++;
}
else
{
cmdLLValid--;
}
if (cmdLLValid < -2)
cmdLLValid = -2; // min 3 packets required
else if (cmdLLValid > 3)
cmdLLValid = 3; // fall back after 3 missed packets
}
// check for motor start/stop packet
if (packetMotors->updated)
{
motor_state = motors.motors;
motor_state_count = 0;
packetMotors->updated = 0;
}
// check for camera control commands
if (packetCamera->updated)
{
int cam_pitch = camera.desired_cam_pitch * 1000;
int cam_roll = camera.desired_cam_roll * 1000;
PTU_set_desired_pitch(cam_pitch);
PTU_set_desired_roll(cam_roll);
packetCamera->updated = 0;
}
// check for new HL command packet
if (packetCmdHL->updated)
{
packetCmdHL->updated = 0;
// SSDK operates in NED, need to convert from ENU
extPositionCmd.heading = -extPositionCmd.heading + 360000;
extPositionCmd.y = -extPositionCmd.y;
extPositionCmd.z = -extPositionCmd.z;
extPositionCmd.vY = -extPositionCmd.vY;
extPositionCmd.vZ = -extPositionCmd.vZ;
extPositionCmd.vYaw = -extPositionCmd.vYaw;
if (extPositionCmd.bitfield & EXT_POSITION_CMD_BODYFIXED)
{
float s_yaw, c_yaw;
c_yaw = approxCos((float)extPosition.heading / 1000 / 180 * M_PI);
s_yaw = approxCos(M_halfPI - (float)extPosition.heading / 1000 / 180 * M_PI);
if (extPositionCmd.bitfield & EXT_POSITION_CMD_VELOCITY)
{
float vx = extPositionCmd.vX;
float vy = extPositionCmd.vY;
extPositionCmd.vX = c_yaw * vx - s_yaw * vy;
extPositionCmd.vY = s_yaw * vx + c_yaw * vy;
}
else
{
float x = extPositionCmd.x;
float y = extPositionCmd.y;
extPositionCmd.x = c_yaw * x - s_yaw * y + extPosition.x;
extPositionCmd.y = s_yaw * x + c_yaw * y + extPosition.y;
extPositionCmd.z += extPosition.z;
extPositionCmd.heading += extPosition.heading;
if(extPositionCmd.heading > 360000)
extPositionCmd.heading -= 360000;
// should not happen ...
else if(extPositionCmd.heading < 0)
extPositionCmd.heading += 360000;
}
}
}
// handle parameter packet
if (packetSSDKParams->updated == 1)
{
packetSSDKParams->updated = 0;
statusData.have_SSDK_parameters = 1;
ssdk_status.have_parameters = 1;
}
// decide which position/state input we take for position control
// SSDK operates in NED --> convert from ENU
switch(hli_config.mode_state_estimation){
case HLI_MODE_STATE_ESTIMATION_HL_SSDK:
extPositionValid = 1;
extPosition.bitfield = 0;
extPosition.count = ext_position_update.count;
extPosition.heading = -ext_position_update.heading + 360000;
extPosition.x = ext_position_update.x;
extPosition.y = -ext_position_update.y;
extPosition.z = -ext_position_update.z;
extPosition.vX = ext_position_update.vX;
extPosition.vY = -ext_position_update.vY;
extPosition.vZ = -ext_position_update.vZ;
extPosition.qualX = ext_position_update.qualX;
extPosition.qualY = ext_position_update.qualY;
extPosition.qualZ = ext_position_update.qualZ;
extPosition.qualVx = ext_position_update.qualVx;
extPosition.qualVy = ext_position_update.qualVy;
extPosition.qualVz = ext_position_update.qualVz;
break;
case HLI_MODE_STATE_ESTIMATION_EXT:
extPositionValid = 1;
extPosition.bitfield = EXT_POSITION_BYPASS_FILTER;
extPosition.count = ext_position_update.count;
extPosition.heading = -ext_position_update.heading + 360000;
extPosition.x = ext_position_update.x;
extPosition.y = -ext_position_update.y;
extPosition.z = -ext_position_update.z;
extPosition.vX = ext_position_update.vX;
extPosition.vY = -ext_position_update.vY;
extPosition.vZ = -ext_position_update.vZ;
extPosition.qualX = ext_position_update.qualX;
extPosition.qualY = ext_position_update.qualY;
extPosition.qualZ = ext_position_update.qualZ;
extPosition.qualVx = ext_position_update.qualVx;
extPosition.qualVy = ext_position_update.qualVy;
extPosition.qualVz = ext_position_update.qualVz;
break;
case HLI_MODE_STATE_ESTIMATION_HL_EKF:
DEKF_step(&dekf, timestamp);
if(DEKF_getInitializeEvent(&dekf) == 1)
ssdk_reset_state = 1;
extPositionValid = 1;
break;
default:
extPositionValid = 0;
}
// dekf initialize state machine
// sets the acc/height/gps switch to 0 for 10 loops so that refmodel gets reset to the new state
if (ssdk_reset_state >= 1 && ssdk_reset_state < 10)
{
RO_RC_Data.channel[0] = 2048;
RO_RC_Data.channel[1] = 2048;
RO_RC_Data.channel[2] = 2048;
RO_RC_Data.channel[3] = 2048;
RO_RC_Data.channel[5] = 0;
ssdk_reset_state++;
}
else
{
ssdk_reset_state = 0;
}
// execute ssdk - only executed if ssdk parameters are available
// reads position reference from extPosition
// reads position/velocity command from extPositionCmd
// finally writes to WO_CTRL_Input. therefore, make sure to overwrite it after this call if you don't want to have its output
rt_OneStep();
// --- write commands to LL ------------------------------------------------
short motorsRunning = LL_1khz_attitude_data.status2 & 0x1;
if (motor_state == -1 || motor_state == 2)
{ // motors are either stopped or running --> normal operation
// commands are always written to LL by the Matlab controller, decide if we need to overwrite them
if (extPositionValid > 0 && statusData.have_SSDK_parameters == 1 && hli_config.mode_position_control == HLI_MODE_POSCTRL_HL)
{
WO_CTRL_Input.ctrl = hli_config.position_control_axis_enable;
WO_SDK.ctrl_enabled = 1;
// limit yaw rate:
if(WO_CTRL_Input.yaw > 1000)
WO_CTRL_Input.yaw = 1000;
else if(WO_CTRL_Input.yaw < -1000)
WO_CTRL_Input.yaw = -1000;
}
else if (cmdLLValid > 0 && (hli_config.mode_position_control == HLI_MODE_POSCTRL_LL || hli_config.mode_position_control == HLI_MODE_POSCTRL_OFF))
{
writeCommand(cmdLL.x, -cmdLL.y, -cmdLL.yaw, cmdLL.z, hli_config.position_control_axis_enable, 1);
}
else
{
writeCommand(0, 0, 0, 0, 0, 0);
}
}
// start / stop motors, allow commands max for 1.5 s
else if (motor_state == 1)
{
if (motor_state_count < 1500)
{
if (!motorsRunning)
writeCommand(0, 0, 2047, 0, HLI_YAW_BIT | HLI_THRUST_BIT, 1); // start/stop sequence, set ctrl to acc so that motors will be safely started
else if (motorsRunning)
{
writeCommand(0, 0, 0, 0, HLI_YAW_BIT | HLI_THRUST_BIT, 1);
motor_state = 2;
}
}
else
{
writeCommand(0, 0, 0, 0, HLI_YAW_BIT | HLI_THRUST_BIT, 1);
motor_state = -1;
}
motor_state_count++;
}
else if (motor_state == 0)
{
if (motor_state_count < 1500)
{
if (motorsRunning)
writeCommand(0, 0, 2047, 0, HLI_YAW_BIT | HLI_THRUST_BIT, 1); // start/stop sequence, set ctrl to acc so that motors will be safely shut down
else if (!motorsRunning)
{
writeCommand(0, 0, 0, 0, HLI_YAW_BIT | HLI_THRUST_BIT, 1);
motor_state = -1;
}
}
else
{
writeCommand(0, 0, 0, 0, HLI_YAW_BIT | HLI_THRUST_BIT, 1);
motor_state = -1;
}
motor_state_count++;
}
else
{
// undefined state, disable everything
writeCommand(0, 0, 0, 0, 0, 0);
}
// TODO: thrust limit in case something really goes wrong, may be removed
if (WO_CTRL_Input.thrust > 4095)
WO_CTRL_Input.thrust = 4095;
// ------------------------------------------------------------------------
// --- send data to UART 0 ------------------------------------------------
if (checkTxPeriod(subscription.imu))
{
sendImuData();
}
if (checkTxPeriod(subscription.rcdata))
{
sendRcData();
}
if (checkTxPeriod(subscription.gps))
{
sendGpsData();
}
if ((sdkLoops + 20) % 500 == 0)
{
sendStatus();
// writePacket2Ringbuffer(HLI_PACKET_ID_SSDK_STATUS, (unsigned char*)&ssdk_status, sizeof(ssdk_status));
}
if (checkTxPeriod(subscription.ssdk_debug))
{
ssdk_debug.timestamp = timestamp;
writePacket2Ringbuffer(HLI_PACKET_ID_SSDK_DEBUG, (unsigned char*)&ssdk_debug, sizeof(ssdk_debug));
}
if (checkTxPeriod(subscription.ekf_state))
{
// sendEkfState();
DEKF_sendState(&dekf, timestamp);
}
if (checkTxPeriod(subscription.mag))
{
sendMagData();
}
//
UART_send_ringbuffer();
synchronizeTime();
if (packetBaudrate->updated)
{
packetBaudrate->updated = 0;
while (!UART0_txEmpty())
;
}
// ------------------------------------------------------------------------
unsigned int dt;
if (T1TC < sdkCycleStartTime)
dt = (processorClockFrequency() - sdkCycleStartTime) + T1TC;
else
dt = T1TC - sdkCycleStartTime;
cpuLoad = ControllerCyclesPerSecond * ((dt * 1e2) / processorClockFrequency()); // cpu load in %
watchdog();
}
void SDK_mainloop(void)
{
sdkCycleStartTime = T1TC;
WO_SDK.ctrl_mode = 0x00; //0x00: absolute angle and throttle control
sdkLoops++;
parseRxFifo();
// check for new LL command packet
if (packetCmdLL->updated)
{
cmdLLNew = 1;
packetCmdLL->updated = 0;
}
// check if LL commands arrive at max every CMD_MAX_PERIOD ms
if ((sdkLoops % CMD_MAX_PERIOD) == 0)
{
if (cmdLLNew == 1)
{
cmdLLNew = 0;
cmdLLValid++;
}
else
{
cmdLLValid--;
}
if (cmdLLValid < -2)
cmdLLValid = -2; // min 3 packets required
else if (cmdLLValid > 3)
cmdLLValid = 3; // fall back after 3 missed packets
}
// check for motor start/stop packet
if (packetMotors->updated)
{
motor_state = motors.motors;
motor_state_count = 0;
packetMotors->updated = 0;
}
// check for new HL command packet
if (packetCmdHL->updated)
{
packetCmdHL->updated = 0;
// SSDK operates in NED, need to convert from ENU
extPositionCmd.heading = -extPositionCmd.heading + 360000;
extPositionCmd.y = -extPositionCmd.y;
extPositionCmd.z = -extPositionCmd.z;
extPositionCmd.vY = -extPositionCmd.vY;
extPositionCmd.vZ = -extPositionCmd.vZ;
extPositionCmd.vYaw = -extPositionCmd.vYaw;
}
// handle parameter packet
if (packetSSDKParams->updated == 1)
{
packetSSDKParams->updated = 0;
statusData.have_SSDK_parameters = 1;
ssdk_status.have_parameters = 1;
}
// decide which position/state input we take for position control
// SSDK operates in NED --> convert from ENU
switch(config.mode_state_estimation){
case HLI_MODE_STATE_ESTIMATION_HL_SSDK:
extPositionValid = 1;
extPosition.bitfield = 0;
extPosition.count = ext_position_update.count;
extPosition.heading = -ext_position_update.heading + 360000;
extPosition.x = ext_position_update.x;
extPosition.y = -ext_position_update.y;
extPosition.z = -ext_position_update.z;
extPosition.vX = ext_position_update.vX;
extPosition.vY = -ext_position_update.vY;
extPosition.vZ = -ext_position_update.vZ;
extPosition.qualX = ext_position_update.qualX;
extPosition.qualY = ext_position_update.qualY;
extPosition.qualZ = ext_position_update.qualZ;
extPosition.qualVx = ext_position_update.qualVx;
extPosition.qualVy = ext_position_update.qualVy;
extPosition.qualVz = ext_position_update.qualVz;
break;
case HLI_MODE_STATE_ESTIMATION_EXT:
extPositionValid = 1;
extPosition.bitfield = EXT_POSITION_BYPASS_FILTER;
extPosition.count = ext_position_update.count;
extPosition.heading = -ext_position_update.heading + 360000;
extPosition.x = ext_position_update.x;
extPosition.y = -ext_position_update.y;
extPosition.z = -ext_position_update.z;
extPosition.vX = ext_position_update.vX;
extPosition.vY = -ext_position_update.vY;
extPosition.vZ = -ext_position_update.vZ;
extPosition.qualX = ext_position_update.qualX;
extPosition.qualY = ext_position_update.qualY;
extPosition.qualZ = ext_position_update.qualZ;
extPosition.qualVx = ext_position_update.qualVx;
extPosition.qualVy = ext_position_update.qualVy;
extPosition.qualVz = ext_position_update.qualVz;
break;
default:
extPositionValid = 0;
}
// execute ssdk - only executed if ssdk parameters are available
// reads position reference from extPosition
// reads position/velocity command from extPositionCmd
// finally writes to WO_CTRL_Input. therefore, make sure to overwrite it after this call if you don't want to have its output
rt_OneStep();
// --- write commands to LL ------------------------------------------------
short motorsRunning = LL_1khz_attitude_data.status2 & 0x1;
if (motor_state == -1 || motor_state == 2)
{ // motors are either stopped or running --> normal operation
// commands are always written to LL by the Matlab controller, decide if we need to overwrite them
if (extPositionValid > 0 && statusData.have_SSDK_parameters == 1 && config.mode_position_control == HLI_MODE_POSCTRL_HL)
{
WO_CTRL_Input.ctrl = config.position_control_axis_enable;
WO_SDK.ctrl_enabled = 1;
}
else if (cmdLLValid > 0 && (config.mode_position_control == HLI_MODE_POSCTRL_LL || config.mode_position_control == HLI_MODE_POSCTRL_OFF))
{
writeCommand(cmdLL.x, cmdLL.y, cmdLL.yaw, cmdLL.z, config.position_control_axis_enable, 1);
}
else
{
writeCommand(0, 0, 0, 0, 0, 0);
}
}
// start / stop motors, allow commands max for 1.5 s
else if (motor_state == 1)
{
if (motor_state_count < 1500)
{
if (!motorsRunning)
writeCommand(0, 0, 2047, 0, HLI_YAW_BIT | HLI_THRUST_BIT, 1); // start/stop sequence, set ctrl to acc so that motors will be safely started
else if (motorsRunning)
{
writeCommand(0, 0, 0, 0, HLI_YAW_BIT | HLI_THRUST_BIT, 1);
motor_state = 2;
}
}
else
{
writeCommand(0, 0, 0, 0, HLI_YAW_BIT | HLI_THRUST_BIT, 1);
motor_state = -1;
}
motor_state_count ++;
}
else if (motor_state == 0)
{
if (motor_state_count < 1500)
{
if (motorsRunning)
writeCommand(0, 0, 2047, 0, HLI_YAW_BIT | HLI_THRUST_BIT, 1); // start/stop sequence, set ctrl to acc so that motors will be safely shut down
else if (!motorsRunning)
{
writeCommand(0, 0, 0, 0, HLI_YAW_BIT | HLI_THRUST_BIT, 1);
motor_state = -1;
}
}
else
{
writeCommand(0, 0, 0, 0, HLI_YAW_BIT | HLI_THRUST_BIT, 1);
motor_state = -1;
}
motor_state_count++;
}
else
{
// undefined state, disable everything
writeCommand(0, 0, 0, 0, 0, 0);
}
// TODO: thrust limit in case something really goes wrong, may be removed
if (WO_CTRL_Input.thrust > 4095)
WO_CTRL_Input.thrust = 4095;
// ------------------------------------------------------------------------
// --- send data to UART 0 ------------------------------------------------
if (checkTxPeriod(subscription.imu))
{
sendImuData();
}
if (checkTxPeriod(subscription.rcdata))
{
sendRcData();
}
if (checkTxPeriod(subscription.gps))
{
sendGpsData();
}
if ((sdkLoops + 20) % 500 == 0)
{
sendStatus();
writePacket2Ringbuffer(HLI_PACKET_ID_SSDK_STATUS, (unsigned char*)&ssdk_status, sizeof(ssdk_status));
}
if (checkTxPeriod(subscription.ssdk_debug))
{
ssdk_debug.timestamp = timestamp;
writePacket2Ringbuffer(HLI_PACKET_ID_SSDK_DEBUG, (unsigned char*)&ssdk_debug, sizeof(ssdk_debug));
}
//
UART_send_ringbuffer();
synchronizeTime();
if (packetBaudrate->updated)
{
packetBaudrate->updated = 0;
while (!UART0_txEmpty())
;
}
// ------------------------------------------------------------------------
unsigned int dt;
if (T1TC < sdkCycleStartTime)
dt = (processorClockFrequency() - sdkCycleStartTime) + T1TC;
else
dt = T1TC - sdkCycleStartTime;
cpuLoad = ControllerCyclesPerSecond * ((dt * 1e2) / processorClockFrequency()); // cpu load in %
watchdog();
}