inline void watchdog(void) { static uint32_t lastTxPackets = 0; // check if a valid packet arrived in the HLI_COMMUNICATION_TIMEOUT s if ((sdkLoops % (ControllerCyclesPerSecond * HLI_COMMUNICATION_TIMEOUT)) == 0) { if (UART_rxGoodPacketCount == lastTxPackets) { startAutoBaud(); } lastTxPackets = UART_rxGoodPacketCount; } }
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(); }