$search

sdk.c File Reference

#include <mav_common/comm_packets.h>
#include <mav_common/comm.h>
#include <mav_common/comm_types.h>
#include "sdk.h"
#include "main.h"
#include "system.h"
#include "LL_HL_comm.h"
#include "uart.h"
#include "time.h"
#include "irq.h"
#include "LPC214x.h"
#include "gpsmath.h"
Include dependency graph for sdk.c:

Go to the source code of this file.

Defines

#define MAX_TOGGLE_CMD_TIME   2000000

Functions

int checkTxPeriod (uint16_t period, uint16_t phase)
 gets called every sdk loops. Currently, only checks for packets from the PC and starts autobaud in case there wwas no communication in the last 10 s
void feedbackBeep ()
unsigned short isSerialEnabled (void)
void processEngageDisengageTimeouts ()
void processFlightActionRequests ()
void processKF ()
void processLandingThrust ()
void processMotorCommands ()
void processMotorStateChanges ()
void processSendData ()
void SDK_mainloop (void)
void sdkInit (void)
void sendCtrlDebugData (void)
void sendFlightStateData (void)
void sendImuData (void)
void sendMavPoseData (void)
void sendRcData (void)
void sendStatusData (void)
void synchronizeTime (void)
 adjusts HLP time to host PC time
void writeCommand (short pitch, short roll, short yaw, short thrust, short ctrl, short enable)

Variables

float g_cpu_load_sum = 0.0
MAV_CTRL_CFG_PKT g_ctrl_cfg_pkt
PacketInfog_ctrl_cfg_pkt_info
MAV_CTRL_CMD g_ctrl_cmd
MAV_CTRL_DEBUG_PKT g_ctrl_debug_pkt
MAV_CTRL_INPUT_PKT g_ctrl_input_pkt
PacketInfog_ctrl_input_pkt_info
MAV_DES_POSE_PKT g_des_pose_pkt
PacketInfog_des_pose_pkt_info
MAV_DES_VEL_PKT g_des_vel_pkt
PacketInfog_des_vel_pkt_info
MAV_DUMMY_PKT g_dummy_pkt
PacketInfog_dummy_pkt_info
MAV_FLIGHT_ACTION_PKT g_flight_action_pkt
PacketInfog_flight_action_pkt_info
MAV_FLIGHT_STATE_PKT g_flight_state_pkt
MAV_IMU_PKT g_imu_pkt
uint8_t g_kf_x_enabled
uint8_t g_kf_y_enabled
uint8_t g_kf_yaw_enabled
uint8_t g_kf_z_enabled
MAV_HEIGHT_PKT g_mav_height_pkt
PacketInfog_mav_height_pkt_info
MAV_KF_CFG_PKT g_mav_kf_cfg_pkt
PacketInfog_mav_kf_cfg_pkt_info
MAV_POSE2D_PKT g_mav_pose2D_pkt
PacketInfog_mav_pose2D_pkt_info
short g_motors_running
MAV_PID_CFG_PKT g_pid_cfg_pkt
PacketInfog_pid_cfg_pkt_info
MAV_POSE_PKT g_pose_pkt
MAV_RCDATA_PKT g_rcdata_pkt
unsigned int g_sdk_loops
MAV_STATUS_PKT g_status_pkt
volatile int64_t g_timestamp = 0
MAV_TIMESYNC_PKT g_timesync_pkt
PacketInfog_timesync_pkt_info
int64_t g_toggle_motors_start_time
MAV_TX_FREQ_CFG_PKT g_tx_freq_cfg_pkt
PacketInfog_tx_freq_cfg_pkt_info
float g_vz_p_f
struct RO_RC_DATA RO_RC_Data
int64_t time_correction = 0
unsigned short time_step = 2000
int64_t timeOffset = 0
struct WO_CTRL_INPUT WO_CTRL_Input
struct WO_DIRECT_MOTOR_CONTROL WO_Direct_Motor_Control
struct WO_SDK_STRUCT WO_SDK

Define Documentation

#define MAX_TOGGLE_CMD_TIME   2000000

Definition at line 58 of file sdk.c.


Function Documentation

int checkTxPeriod ( uint16_t  period,
uint16_t  phase 
) [inline]

gets called every sdk loops. Currently, only checks for packets from the PC and starts autobaud in case there wwas no communication in the last 10 s

checks if a packet has to be sent

Definition at line 401 of file sdk.c.

void feedbackBeep ( void   )  [inline]

Definition at line 495 of file sdk.c.

unsigned short isSerialEnabled ( void   )  [inline]

Definition at line 328 of file sdk.c.

void processEngageDisengageTimeouts ( void   )  [inline]

Definition at line 595 of file sdk.c.

void processFlightActionRequests ( void   )  [inline]

Definition at line 544 of file sdk.c.

void processKF ( void   )  [inline]

Definition at line 409 of file sdk.c.

void processLandingThrust ( void   )  [inline]

Definition at line 607 of file sdk.c.

void processMotorCommands ( void   )  [inline]

Definition at line 635 of file sdk.c.

void processMotorStateChanges ( void   )  [inline]

Definition at line 523 of file sdk.c.

void processSendData ( void   )  [inline]

Definition at line 679 of file sdk.c.

void SDK_mainloop ( void   ) 

SDK_mainloop(void) is triggered @ 1kHz.

WO_(Write Only) data is written to the LL processor after execution of this function.

RO_(Read Only) data is updated before entering this function and can be read to obtain information for supervision or control

WO_ and RO_ structs are defined in sdk.h

The struct LL_1khz_attitude_data (defined in LL_HL_comm.h) can be used to read all sensor data, results of the data fusion and R/C inputs transmitted from the LL-processor. This struct is automatically updated at 1 kHz.

Definition at line 178 of file sdk.c.

void sdkInit ( void   ) 

Definition at line 120 of file sdk.c.

void sendCtrlDebugData ( void   )  [inline]

Definition at line 305 of file sdk.c.

void sendFlightStateData ( void   )  [inline]

Definition at line 287 of file sdk.c.

void sendImuData ( void   )  [inline]

Definition at line 276 of file sdk.c.

void sendMavPoseData ( void   )  [inline]

Definition at line 271 of file sdk.c.

void sendRcData ( void   )  [inline]

Definition at line 292 of file sdk.c.

void sendStatusData ( void   )  [inline]

Definition at line 300 of file sdk.c.

void synchronizeTime ( void   )  [inline]

adjusts HLP time to host PC time

Definition at line 333 of file sdk.c.

void writeCommand ( short  pitch,
short  roll,
short  yaw,
short  thrust,
short  ctrl,
short  enable 
) [inline]

Definition at line 261 of file sdk.c.


Variable Documentation

float g_cpu_load_sum = 0.0

Definition at line 61 of file sdk.c.

MAV_CTRL_CFG_PKT g_ctrl_cfg_pkt

Definition at line 86 of file sdk.c.

Definition at line 87 of file sdk.c.

Definition at line 64 of file sdk.c.

MAV_CTRL_DEBUG_PKT g_ctrl_debug_pkt

Definition at line 67 of file sdk.c.

MAV_CTRL_INPUT_PKT g_ctrl_input_pkt

Definition at line 89 of file sdk.c.

Definition at line 90 of file sdk.c.

MAV_DES_POSE_PKT g_des_pose_pkt

Definition at line 92 of file sdk.c.

Definition at line 93 of file sdk.c.

MAV_DES_VEL_PKT g_des_vel_pkt

Definition at line 95 of file sdk.c.

Definition at line 96 of file sdk.c.

MAV_DUMMY_PKT g_dummy_pkt

Definition at line 71 of file sdk.c.

Definition at line 72 of file sdk.c.

MAV_FLIGHT_ACTION_PKT g_flight_action_pkt

Definition at line 74 of file sdk.c.

Definition at line 75 of file sdk.c.

MAV_FLIGHT_STATE_PKT g_flight_state_pkt

Definition at line 65 of file sdk.c.

MAV_IMU_PKT g_imu_pkt

Definition at line 69 of file sdk.c.

uint8_t g_kf_x_enabled

Definition at line 115 of file sdk.c.

uint8_t g_kf_y_enabled

Definition at line 116 of file sdk.c.

Definition at line 118 of file sdk.c.

uint8_t g_kf_z_enabled

Definition at line 117 of file sdk.c.

MAV_HEIGHT_PKT g_mav_height_pkt

Definition at line 101 of file sdk.c.

Definition at line 102 of file sdk.c.

MAV_KF_CFG_PKT g_mav_kf_cfg_pkt

Definition at line 104 of file sdk.c.

Definition at line 105 of file sdk.c.

MAV_POSE2D_PKT g_mav_pose2D_pkt

Definition at line 98 of file sdk.c.

Definition at line 99 of file sdk.c.

Definition at line 109 of file sdk.c.

MAV_PID_CFG_PKT g_pid_cfg_pkt

Definition at line 83 of file sdk.c.

Definition at line 84 of file sdk.c.

MAV_POSE_PKT g_pose_pkt

Definition at line 68 of file sdk.c.

MAV_RCDATA_PKT g_rcdata_pkt

Definition at line 66 of file sdk.c.

unsigned int g_sdk_loops

Definition at line 60 of file sdk.c.

MAV_STATUS_PKT g_status_pkt

Definition at line 63 of file sdk.c.

volatile int64_t g_timestamp = 0

Definition at line 52 of file sdk.c.

MAV_TIMESYNC_PKT g_timesync_pkt

Definition at line 77 of file sdk.c.

Definition at line 78 of file sdk.c.

Definition at line 110 of file sdk.c.

MAV_TX_FREQ_CFG_PKT g_tx_freq_cfg_pkt

Definition at line 80 of file sdk.c.

Definition at line 81 of file sdk.c.

float g_vz_p_f

Definition at line 8 of file kalman.c.

Definition at line 49 of file sdk.c.

int64_t time_correction = 0

Definition at line 56 of file sdk.c.

unsigned short time_step = 2000

Definition at line 55 of file sdk.c.

int64_t timeOffset = 0

Definition at line 54 of file sdk.c.

Definition at line 48 of file sdk.c.

Definition at line 50 of file sdk.c.

Definition at line 47 of file sdk.c.

 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


ccny_asctec_firmware_2
Author(s): Ivan Dryanovski, Roberto G. Valenti
autogenerated on Tue Mar 5 11:33:42 2013