示例#1
0
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();
}
示例#2
0
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();
}