void sdkInit(void)
{
g_sdk_loops = 0;
g_motors_running = 0;
// **** these should be sent by the CPU upon successful connection
g_tx_freq_cfg_pkt.imu_period = 0;
g_tx_freq_cfg_pkt.rcdata_period = 0;
g_tx_freq_cfg_pkt.flight_state_period = 0;
g_tx_freq_cfg_pkt.pose_period = 0;
g_tx_freq_cfg_pkt.status_period = 0;
g_tx_freq_cfg_pkt.ctrl_debug_period = 0;
g_tx_freq_cfg_pkt.imu_phase = 0;
g_tx_freq_cfg_pkt.rcdata_phase = 0;
g_tx_freq_cfg_pkt.flight_state_phase = 0;
g_tx_freq_cfg_pkt.pose_phase = 0;
g_tx_freq_cfg_pkt.status_phase = 0;
g_tx_freq_cfg_pkt.ctrl_debug_phase = 0;
// **** register packets to receive
g_dummy_pkt_info = registerPacket(MAV_DUMMY_PKT_ID, &g_dummy_pkt);
g_mav_pose2D_pkt_info = registerPacket(MAV_POSE2D_PKT_ID, &g_mav_pose2D_pkt);
g_mav_height_pkt_info = registerPacket(MAV_HEIGHT_PKT_ID, &g_mav_height_pkt);
g_mav_kf_cfg_pkt_info = registerPacket(MAV_KF_CFG_PKT_ID, &g_mav_kf_cfg_pkt);
g_timesync_pkt_info = registerPacket(MAV_TIMESYNC_PKT_ID, &g_timesync_pkt);
g_ctrl_cfg_pkt_info = registerPacket(MAV_CTRL_CFG_PKT_ID, &g_ctrl_cfg_pkt);
g_pid_cfg_pkt_info = registerPacket(MAV_PID_CFG_PKT_ID, &g_pid_cfg_pkt);
g_flight_action_pkt_info = registerPacket(MAV_FLIGHT_ACTION_PKT_ID, &g_flight_action_pkt);
g_des_pose_pkt_info = registerPacket(MAV_DES_POSE_PKT_ID, &g_des_pose_pkt);
g_ctrl_input_pkt_info = registerPacket(MAV_CTRL_INPUT_PKT_ID, &g_ctrl_input_pkt);
g_tx_freq_cfg_pkt_info = registerPacket(MAV_TX_FREQ_CFG_PKT_ID, &g_tx_freq_cfg_pkt);
g_des_vel_pkt_info = registerPacket(MAV_DES_VEL_PKT_ID, &g_des_vel_pkt);
UART0_rxFlush();
UART0_txFlush();
startAutoBaud();
}
void sdkInit(void)
{
// UART_setPacketInfo(packetInfo);
statusData.have_SSDK_parameters = 0;
ssdk_status.have_parameters = 0;
baudrate.state = 0;
extPositionValid = 0;
config.mode_position_control = HLI_MODE_POSCTRL_OFF;
config.mode_state_estimation = HLI_MODE_STATE_ESTIMATION_OFF;
config.position_control_axis_enable = 0;
// set default packet rates
subscription.imu = HLI_DEFAULT_PERIOD_IMU;
subscription.rcdata = HLI_DEFAULT_PERIOD_RCDATA;
subscription.gps = HLI_DEFAULT_PERIOD_GPS;
subscription.ssdk_debug = HLI_DEFAULT_PERIOD_SSDK_DEBUG;
subscription.ekf_state = HLI_DEFAULT_PERIOD_EKF_STATE;
// register packets to receive
packetExtPosition = registerPacket(HLI_PACKET_ID_POSITION_UPDATE, &ext_position_update);
packetCmdHL = registerPacket(HLI_PACKET_ID_CONTROL_HL, &extPositionCmd);
packetMotors = registerPacket(HLI_PACKET_ID_MOTORS, &motors);
packetCmdLL = registerPacket(HLI_PACKET_ID_CONTROL_LL, &cmdLL);
packetTimeSync = registerPacket(HLI_PACKET_ID_TIMESYNC, &timeSync);
packetSSDKParams = registerPacket(HLI_PACKET_ID_SSDK_PARAMETERS, &ssdk_params); // defined in ert_main.h
packetControlEnable = registerPacket(HLI_PACKET_ID_CONTROL_ENABLE, &controlEnable);
packetBaudrate = registerPacket(HLI_PACKET_ID_BAUDRATE, &baudrate);
packetSubscription = registerPacket(HLI_PACKET_ID_SUBSCRIPTION, &subscription);
packetEkfState = registerPacket(HLI_PACKET_ID_EKF_STATE, &ekfState);
packetConfig = registerPacket(HLI_PACKET_ID_CONFIG, &config);
UART0_rxFlush();
UART0_txFlush();
startAutoBaud();
}
void sdkInit(void)
{
// UART_setPacketInfo(packetInfo);
statusData.have_SSDK_parameters = 0;
ssdk_status.have_parameters = 0;
baudrate.state = 0;
extPositionValid = 0;
hli_config.mode_position_control = HLI_MODE_POSCTRL_OFF;
hli_config.mode_state_estimation = HLI_MODE_STATE_ESTIMATION_OFF;
hli_config.position_control_axis_enable = 0;
hli_config.battery_warning_voltage = BATTERY_WARNING_VOLTAGE;
// set default packet rates
subscription.imu = HLI_DEFAULT_PERIOD_IMU;
subscription.rcdata = HLI_DEFAULT_PERIOD_RCDATA;
subscription.gps = HLI_DEFAULT_PERIOD_GPS;
subscription.ssdk_debug = HLI_DEFAULT_PERIOD_SSDK_DEBUG;
subscription.ekf_state = HLI_DEFAULT_PERIOD_EKF_STATE;
// register packets to receive
packetExtPosition = registerPacket(HLI_PACKET_ID_POSITION_UPDATE, &ext_position_update);
packetCmdHL = registerPacket(HLI_PACKET_ID_CONTROL_HL, &extPositionCmd);
packetMotors = registerPacket(HLI_PACKET_ID_MOTORS, &motors);
packetCmdLL = registerPacket(HLI_PACKET_ID_CONTROL_LL, &cmdLL);
packetTimeSync = registerPacket(HLI_PACKET_ID_TIMESYNC, &timeSync);
packetSSDKParams = registerPacket(HLI_PACKET_ID_SSDK_PARAMETERS, &ssdk_params); // defined in ert_main.h
packetControlEnable = registerPacket(HLI_PACKET_ID_CONTROL_ENABLE, &controlEnable);
packetBaudrate = registerPacket(HLI_PACKET_ID_BAUDRATE, &baudrate);
packetSubscription = registerPacket(HLI_PACKET_ID_SUBSCRIPTION, &subscription);
packetConfig = registerPacket(HLI_PACKET_ID_CONFIG, &hli_config);
packetCamera = registerPacket(HLI_PACKET_ID_CAMERA, &camera);
UART0_rxFlush();
UART0_txFlush();
// init ssdk
onboard_matlab_initialize();
// init dekf, also packet subscription takes place here
DEKF_init(&dekf, &extPosition);
extPosition.bitfield = 0;
extPosition.count = 0;
extPosition.heading = 0;
extPosition.x = 0;
extPosition.y = 0;
extPosition.z = 0;
extPosition.vX = 0;
extPosition.vY = 0;
extPosition.vZ = 0;
extPosition.qualX = 0;
extPosition.qualY = 0;
extPosition.qualZ = 0;
extPosition.qualVx = 0;
extPosition.qualVy = 0;
extPosition.qualVz = 0;
extPositionCmd.heading = 0;
extPositionCmd.bitfield = 0;
extPositionCmd.x = 0;
extPositionCmd.y = 0;
extPositionCmd.z = 0;
extPositionCmd.vZ = 2000;
extPositionCmd.vY = 2000;
extPositionCmd.vZ = 2000;
extPositionCmd.vYaw = 45000;
startAutoBaud();
}
