int main(int argc, char *argv[]) {
var_getted=0;
angle_pitch = 0;
angle_roll = 0;
angle_yaw = 0;
pthread_t p_acithread;
fd = open("/dev/ttyUSB0", O_RDWR | O_NOCTTY | O_NDELAY | O_NONBLOCK);
struct termios port_settings; // structure to store the port settings in
cfsetispeed(&port_settings, B57600); // set baud rates
port_settings.c_cflag = B57600 | CS8 | CREAD | CLOCAL;
port_settings.c_iflag = IGNPAR;
port_settings.c_oflag = 0;
port_settings.c_lflag = 0;
tcsetattr(fd, TCSANOW, &port_settings); // apply the settings to the port
lcm = lcm_create(NULL);
if(!lcm) return 1;
aciInit();
aciSetSendDataCallback(&transmit);
aciSetVarListUpdateFinishedCallback(&varListUpdateFinished);
aciSetEngineRate(100, 10);
pthread_create( &p_acithread, NULL, aciThread, NULL);
aciGetDeviceVariablesList();
pthread_join( p_acithread, NULL);
lcm_destroy(lcm);
}
int main(int argc, char *argv[]) {
var_getted = 0;
cmd_ready = 0;
cmmd_msg_get = 0;
lcm = lcm_create(NULL);
if(!lcm){
printf("lcm initialize failed!\n");
return 1;
}
pthread_t p_acithread, p_subscribeDMCCmmdThread, p_sendOutDMCcmmdThread, p_publishVarThread, p_fetchDataThread;
fd = open("/dev/ttyUSB0", O_RDWR | O_NOCTTY | O_NDELAY | O_NONBLOCK);
struct termios port_settings; // structure to store the port settings in
cfsetispeed(&port_settings, B57600); // set baud rates
port_settings.c_cflag = B57600 | CS8 | CREAD | CLOCAL;
port_settings.c_iflag = IGNPAR;
port_settings.c_oflag = 0;
port_settings.c_lflag = 0;
tcsetattr(fd, TCSANOW, &port_settings); // apply the settings to the port
aciInit();
aciSetSendDataCallback(&transmit);
aciSetVarListUpdateFinishedCallback(&varListUpdateFinished);
aciSetCmdListUpdateFinishedCallback(&cmdListUpdateFinished);
aciSetEngineRate(100, 10);
//start running acithread
pthread_create(&p_acithread, NULL, aciThread, NULL);
aciGetDeviceVariablesList();
printf("Waiting for variable list.. \n");
while(!var_getted) usleep(1000);
// start fetching data and publish via LCM
pthread_create(&p_fetchDataThread, NULL, fetchDataThread, NULL);
aciGetDeviceCommandsList();
printf("Waiting for command list...\n");
while(!cmd_ready) usleep(1000);
pthread_create(&p_subscribeDMCCmmdThread, NULL, subscribeDMCCmmdThread, NULL);
pthread_create(&p_sendOutDMCcmmdThread, NULL, sendOutDMCcmmdThread, NULL);
pthread_join(p_acithread, NULL);
pthread_join(p_fetchDataThread, NULL);
pthread_join(p_subscribeDMCCmmdThread, NULL);
pthread_join(p_sendOutDMCcmmdThread, NULL);
}
int main(int argc, char *argv[]) {
pthread_t p_viconThread, p_sendCmmdThread, p_publishCmmdThread, p_fetchDataThread;
pthread_t p_acithread;
fd = open("/dev/ttyUSB0", O_RDWR | O_NOCTTY | O_NDELAY | O_NONBLOCK);
struct termios port_settings; // structure to store the port settings in
cfsetispeed(&port_settings, B57600); // set baud rates
port_settings.c_cflag = B57600 | CS8 | CREAD | CLOCAL;
port_settings.c_iflag = IGNPAR;
port_settings.c_oflag = 0;
port_settings.c_lflag = 0;
tcsetattr(fd, TCSANOW, &port_settings); // apply the settings to the port
aciInit();
aciSetSendDataCallback(&transmit);
aciSetVarListUpdateFinishedCallback(&varListUpdateFinished);
aciSetCmdListUpdateFinishedCallback(&cmdListUpdateFinished);
aciSetParamListUpdateFinishedCallback(¶mListUpdateFinished);
aciSetEngineRate(100, 10);
lcm = lcm_create(NULL);
if(!lcm)
return 1;
pthread_create(&p_acithread, NULL, aciThread, NULL);
aciGetDeviceVariablesList();
while(!var_getted) usleep(1000);
aciGetDeviceCommandsList();
printf("Waiting for command list...\n");
while(!cmd_ready) usleep(1000);
printf("starting motors!\n");
//turn on motors()
startMotors();
usleep(1000000);
//start getting data from Vicon
pthread_create(&p_viconThread, NULL, viconStateThread, NULL);
pthread_create(&p_sendCmmdThread, NULL, sendCmmdThread, NULL);
// pthread_create(&p_fetchDataThread, NULL, fetchDataThread, NULL);
pthread_create(&p_publishCmmdThread, NULL, publishCmmdThread, NULL);
pthread_join(p_viconThread, NULL);
pthread_join(p_sendCmmdThread, NULL);
// pthread_join(p_fetchDataThread, NULL);
pthread_join(p_publishCmmdThread, NULL);
pthread_join(p_acithread, NULL);
}
void ACISDK(void)
{
aciInit(1000);
lpc_aci_init();
#ifndef MATLAB
aciSetStartTxCallback(UARTWriteChar);
// Variables
/*
aciPublishVariable(&RO_ALL_Data.UAV_status, VARTYPE_INT16, 0x0001, "UAV_status", "UAV status information","See in wiki");
aciPublishVariable(&RO_ALL_Data.flight_time, VARTYPE_INT16, 0x0002, "flight_time", "Total flight time","s");
aciPublishVariable(&RO_ALL_Data.battery_voltage, VARTYPE_INT16, 0x0003, "battery_voltage", "Battery voltage","mV");
aciPublishVariable(&RO_ALL_Data.HL_cpu_load, VARTYPE_INT16, 0x0004, "HL_cpu_load", "High-level CPU load","Hz");
aciPublishVariable(&RO_ALL_Data.HL_up_time, VARTYPE_INT16, 0x0005, "HL_up_time", "AHigh-level up-time","ms");
aciPublishVariable(&RO_ALL_Data.motor_rpm[0], VARTYPE_UINT8, 0x0100, "motor_rpm[0]", "Quadcopter: front, Hexcopter front-left", "RPM measurements (0..200)");
aciPublishVariable(&RO_ALL_Data.motor_rpm[1], VARTYPE_UINT8, 0x0101, "motor_rpm[1]", "Quadcopter: rear, Hexcopter left", "RPM measurements (0..200)");
aciPublishVariable(&RO_ALL_Data.motor_rpm[2], VARTYPE_UINT8, 0x0102, "motor_rpm[2]", "Quadcopter: left, Hexcopter rear-left", "RPM measurements (0..200)");
aciPublishVariable(&RO_ALL_Data.motor_rpm[3], VARTYPE_UINT8, 0x0103, "motor_rpm[3]", "Quadcopter: right, Hexcopter rear-right", "RPM measurements (0..200)");
aciPublishVariable(&RO_ALL_Data.motor_rpm[4], VARTYPE_UINT8, 0x0104, "motor_rpm[4]", "Quadcopter: N/A, Hexcopter right", "RPM measurements (0..200)");
aciPublishVariable(&RO_ALL_Data.motor_rpm[5], VARTYPE_UINT8, 0x0105, "motor_rpm[5]", "Quadcopter: N/A, Hexcopter front-right", "RPM measurements (0..200)");
aciPublishVariable(&RO_ALL_Data.GPS_latitude, VARTYPE_INT32, 0x0106, "GPS_latitude", "Latitude from the GPS sensor", "degrees * 10^7");
aciPublishVariable(&RO_ALL_Data.GPS_longitude, VARTYPE_INT32, 0x0107, "GPS_longitude", "Longitude from the GPS sensor", "degrees * 10^7");
aciPublishVariable(&RO_ALL_Data.GPS_height, VARTYPE_INT32, 0x0108, "GPS_height", "Height from the GPS sensor", "mm");
aciPublishVariable(&RO_ALL_Data.GPS_speed_x, VARTYPE_INT32, 0x0109, "GPS_speed_x", "Speed in East/West from the GPS sensor", "mm/s");
aciPublishVariable(&RO_ALL_Data.GPS_speed_y, VARTYPE_INT32, 0x010A, "GPS_speed_y", "Speed in North/South from the GPS sensor", "mm/s");
aciPublishVariable(&RO_ALL_Data.GPS_heading, VARTYPE_INT32, 0x010B, "GPS_heading", "Heading from the Compass", "deg * 1000");
aciPublishVariable(&RO_ALL_Data.GPS_position_accuracy, VARTYPE_UINT32, 0x010C, "GPS_position_accuracy", "GPS position accuracy estimate", "mm");
aciPublishVariable(&RO_ALL_Data.GPS_height_accuracy, VARTYPE_UINT32, 0x010D, "GPS_height_accuracy", "GPS height accuracy estimate", "mm");
aciPublishVariable(&RO_ALL_Data.GPS_speed_accuracy, VARTYPE_UINT32, 0x010E, "GPS_speed_accuracy", "GPS speed accuracy estimate", "mm/s");
aciPublishVariable(&RO_ALL_Data.GPS_sat_num, VARTYPE_UINT32, 0x010F, "GPS_sat_num", "Number of satellites used in NAV solution", "count");
aciPublishVariable(&RO_ALL_Data.GPS_status, VARTYPE_INT32, 0x0110, "GPS_status", "GPS status information", "see documentation");
aciPublishVariable(&RO_ALL_Data.GPS_time_of_week, VARTYPE_UINT32, 0x0111, "GPS_time_of_week", "Time of the week (1 week = 604,800 s)", "ms");
aciPublishVariable(&RO_ALL_Data.GPS_week, VARTYPE_UINT16, 0x0112, "GPS_week", "Week counter since 1980", "count");
*/
aciPublishVariable(&RO_ALL_Data.angvel_pitch, VARTYPE_INT32, 0x0200, "angvel_pitch", "Pitch angle velocity", "0.0154 degree/s, ""bias free");
aciPublishVariable(&RO_ALL_Data.angvel_roll, VARTYPE_INT32, 0x0201, "angvel_roll", "Roll angle velocity", "0.0154 degree/s, bias free");
aciPublishVariable(&RO_ALL_Data.angvel_yaw, VARTYPE_INT32, 0x0202, "angvel_yaw", "Yaw angle velocity", "0.0154 degree/s, bias free");
aciPublishVariable(&RO_ALL_Data.acc_x, VARTYPE_INT16, 0x0203, "acc_x", "Acc-sensor output in x, body frame coordinate system","-10000..+10000 = -1g..+1g");
aciPublishVariable(&RO_ALL_Data.acc_y, VARTYPE_INT16, 0x0204, "acc_y", "Acc-sensor output in y, body frame coordinate system","-10000..+10000 = -1g..+1g");
aciPublishVariable(&RO_ALL_Data.acc_z, VARTYPE_INT16, 0x0205, "acc_z", "Acc-sensor output in z, body frame coordinate system","-10000..+10000 = -1g..+1g");
aciPublishVariable(&RO_ALL_Data.Hx, VARTYPE_INT32, 0x0206, "Hx", "Magnetic field sensors output in x", "+-2500 =+- earth field strength");
aciPublishVariable(&RO_ALL_Data.Hy, VARTYPE_INT32, 0x0207, "Hy", "Magnetic field sensors output in y", "+-2500 =+- earth field strength");
aciPublishVariable(&RO_ALL_Data.Hz, VARTYPE_INT32, 0x0208, "Hz", "Magnetic field sensors output in z", "+-2500 =+- earth field strength");
aciPublishVariable(&RO_ALL_Data.angle_pitch, VARTYPE_INT32, 0x0300, "angle_pitch", "Pitch angle derived by by data fusion", "degree*1000");
aciPublishVariable(&RO_ALL_Data.angle_roll, VARTYPE_INT32, 0x0301, "angle_roll", "Roll angle derived by data fusion", "degree*1000");
aciPublishVariable(&RO_ALL_Data.angle_yaw, VARTYPE_INT32, 0x0302, "angle_yaw", "Yaw angle derived by data fusion", "degree*1000");
aciPublishVariable(&RO_ALL_Data.fusion_latitude, VARTYPE_INT32, 0x0303, "fusion_latitude", "Fused latitude with all other sensors (best estimations)", "degrees * 10^7");
aciPublishVariable(&RO_ALL_Data.fusion_longitude, VARTYPE_INT32, 0x0304, "fusion_longitude", "Fused longitude with all other sensors (best estimations)", "degrees * 10^7");
aciPublishVariable(&RO_ALL_Data.fusion_dheight, VARTYPE_INT32, 0x0305, "fusion_dheight", "Difference height after data fusion", "mm/s");
aciPublishVariable(&RO_ALL_Data.fusion_height, VARTYPE_INT32, 0x0306, "fusion_height", "Height after data fusion", "mm");
aciPublishVariable(&RO_ALL_Data.fusion_speed_x, VARTYPE_INT16, 0x0307, "fusion_speed_x", "Fused speed in East/West with all other sensors (best estimations)", "mm/s");
aciPublishVariable(&RO_ALL_Data.fusion_speed_y, VARTYPE_INT16, 0x0308, "fusion_speed_y", "Fused speed in North/South with all other sensors (best estimations)", "mm/s");
aciPublishVariable(&laser_distance, VARTYPE_INT16, 0x0309, "laser measurement", "the 16-bit measured distance", "cm"); // by Xun
/*
aciPublishVariable(&RO_ALL_Data.channel[0], VARTYPE_UINT16, 0x0600, "channel[0]", "Pitch command received from the remote control", "0..4095");
aciPublishVariable(&RO_ALL_Data.channel[1], VARTYPE_UINT16, 0x0601, "channel[1]", "Roll command received from the remote control", "0..4095");
aciPublishVariable(&RO_ALL_Data.channel[2], VARTYPE_UINT16, 0x0602, "channel[2]", "Thrust command received from the remote control", "0..4095");
aciPublishVariable(&RO_ALL_Data.channel[3], VARTYPE_UINT16, 0x0603, "channel[3]", "Yaw command received from the remote control", "0..4095");
aciPublishVariable(&RO_ALL_Data.channel[4], VARTYPE_UINT16, 0x0604, "channel[4]", "Serial interface enable/disable", ">2048 enabled, else disabled");
aciPublishVariable(&RO_ALL_Data.channel[5], VARTYPE_UINT16, 0x0605, "channel[5]", "Manual / height control / GPS + height control", "see documentation");
aciPublishVariable(&RO_ALL_Data.channel[6], VARTYPE_UINT16, 0x0606, "channel[6]", "Custom remote control data","n/a");
aciPublishVariable(&RO_ALL_Data.channel[7], VARTYPE_UINT16, 0x0607, "channel[7]", "Custom remote control data","n/a");
// Commands
aciPublishCommand(&(WO_Direct_Individual_Motor_Control.motor[0]), VARTYPE_UINT8, 0x0500, "DIMC motor[0]", "Direct motor control 1", "0..200 = 0..100 %");
aciPublishCommand(&(WO_Direct_Individual_Motor_Control.motor[1]), VARTYPE_UINT8, 0x0501, "DIMC motor[1]", "Direct motor control 2", "0..200 = 0..100 %");
aciPublishCommand(&(WO_Direct_Individual_Motor_Control.motor[2]), VARTYPE_UINT8, 0x0502, "DIMC motor[2]", "Direct motor control 3", "0..200 = 0..100 %");
aciPublishCommand(&(WO_Direct_Individual_Motor_Control.motor[3]), VARTYPE_UINT8, 0x0503, "DIMC motor[3]", "Direct motor control 4", "0..200 = 0..100 %");
aciPublishCommand(&(WO_Direct_Individual_Motor_Control.motor[4]), VARTYPE_UINT8, 0x0504, "DIMC motor[4]", "Direct motor control 5", "0..200 = 0..100 %");
aciPublishCommand(&(WO_Direct_Individual_Motor_Control.motor[5]), VARTYPE_UINT8, 0x0505, "DIMC motor[5]", "Direct motor control 6", "0..200 = 0..100 %");
aciPublishCommand(&WO_Direct_Motor_Control.pitch, VARTYPE_UINT8, 0x0506, "DMC pitch", "Pitch input (DMC)", "0..200 = - 100..+100%");
aciPublishCommand(&WO_Direct_Motor_Control.roll, VARTYPE_UINT8, 0x0507, "DMC roll", "Roll input (DMC)", "0..200 = - 100..+100%");
aciPublishCommand(&WO_Direct_Motor_Control.yaw, VARTYPE_UINT8, 0x0508, "DMC yaw", "Yaw input (DMC)", "0..200 = - 100..+100%");
aciPublishCommand(&WO_Direct_Motor_Control.thrust, VARTYPE_UINT8, 0x0509, "DMC thrust", "Thrust input (DMC)", "0..200 = 0..100 %");
aciPublishCommand(&WO_CTRL_Input.pitch, VARTYPE_INT16, 0x050A, "CRTL pitch", "Pitch input (CRTL)", "-2047..+2047 (0=neutral)");
aciPublishCommand(&WO_CTRL_Input.roll, VARTYPE_INT16, 0x050B, "CTRL roll", "Roll input (CRTL)", "-2047..+2047 (0=neutral)");
aciPublishCommand(&WO_CTRL_Input.yaw, VARTYPE_INT16, 0x050C, "CTRL yaw", "Yaw input (CRTL)", "-2047..+2047 (0=neutral)");
aciPublishCommand(&WO_CTRL_Input.thrust, VARTYPE_INT16, 0x050D, "CTRL thrust", "Thrust input (CRTL)", "0..4095 = 0..100%");
aciPublishCommand(&WO_CTRL_Input.ctrl, VARTYPE_INT16, 0x050E, "CTRL ctrl", "Control byte for enable different controls", "see documentation");
aciPublishCommand(&WO_SDK.ctrl_mode,VARTYPE_UINT8,0x0600,"ctrl_mode","Control mode setting parameter","0:DIMC, 1: DMC, 2: CRTL, 3: GPS");
aciPublishCommand(&WO_SDK.ctrl_enabled,VARTYPE_UINT8,0x0601,"ctrl_enabled","Control commands are accepted/ignored by LL processor", "0x00: ignored, 0x01: accepted");
*/
aciPublishCommand(&WO_SDK.disable_motor_onoff_by_stick,VARTYPE_UINT8,0x0602,"disable_motor_onoff_by_stick","Setting if motors can be turned on by using the stick input","0x00: disable, 0x01 enable");
/*
// Parameters
aciPublishParameter(&ALARM_battery_warning_voltage_high,VARTYPE_UINT16,0x0001,"battery_warning_voltage_high","First battery warning level","mV");
aciPublishParameter(&ALARM_battery_warning_voltage_low,VARTYPE_UINT16,0x0002,"battery_warning_voltage_low","Second battery warning level","mV");
aciPublishParameter(&buzzer_warnings,VARTYPE_UINT8,0x0003,"buzzer_warnings","Enable/Disable acoustic warnings","");
aciPublishParameter(&PTU_cam_option_4_version,VARTYPE_UINT8,0x0004,"PTU_cam_option_4_version","Version of Pelican/Firefly PanTilt camera mount option 4","1 or 2");
aciPublishParameter(&PTU_cam_angle_roll_offset,VARTYPE_INT32,0x0400,"cam_angle_roll_offset","Camera roll angle offset","0.001deg");
aciPublishParameter(&PTU_cam_angle_pitch_offset,VARTYPE_INT32,0x0401,"cam_angle_pitch_offset","Camera pitch angle offset","0.001deg");
*/
aciPublishParameter(&PTU_enable_plain_ch7_to_servo,VARTYPE_UINT8,0x0005,"PTU_enable_plain_ch7_to_servo","Channel7 mapped directly to servo out","1=enable 0=disable");
#else
// Matlab parameters
aciPublishParameter(&matlab_params.p01,VARTYPE_STRUCT_WITH_SIZE(60),0x0F00,"Matlab Parameter Set 1","Matlab paramters 1..15","");
aciPublishParameter(&matlab_params.p16,VARTYPE_STRUCT_WITH_SIZE(60),0x0F01,"Matlab Parameter Set 2","Matlab paramters 15..30","");
aciPublishParameter(&matlab_params.p30,VARTYPE_STRUCT_WITH_SIZE(48),0x0F02,"Matlab Parameter Set 3","Matlab paramters 30..40 and CRC","");
#endif
//get initial values from flash for all parameters
lpc_aci_ReadParafromFlash();
}
