int main(int argc, char *argv[])
{
int parameter;
char *sensor_name;
if (check_apm()) {
return 1;
}
if (argc < 2) {
printf("Enter parameter\n");
print_help();
return EXIT_FAILURE;
}
// prevent the error message
opterr=0;
while ((parameter = getopt(argc, argv, "i:h")) != -1) {
switch (parameter) {
case 'i': sensor_name = optarg; break;
case 'h': print_help(); return EXIT_FAILURE;
case '?': printf("Wrong parameter.\n");
print_help();
return EXIT_FAILURE;
}
}
imu = create_inertial_sensor(sensor_name);
if (!imu) {
printf("Wrong sensor name. Select: mpu or lsm\n");
return EXIT_FAILURE;
}
if (!imu->probe()) {
printf("Sensor not enable\n");
return EXIT_FAILURE;
}
//--------------------------- Network setup -------------------------------
sockfd = socket(AF_INET,SOCK_DGRAM,0);
servaddr.sin_family = AF_INET;
if (argc == 5) {
servaddr.sin_addr.s_addr = inet_addr(argv[3]);
servaddr.sin_port = htons(atoi(argv[4]));
} else {
servaddr.sin_addr.s_addr = inet_addr("127.0.0.1");
servaddr.sin_port = htons(7000);
}
//-------------------- IMU setup and main loop ----------------------------
imuSetup();
while(1)
imuLoop();
}
int main(int argc, char **argv)
{
/***********************/
/* Initialize The Node */
/***********************/
ros::init(argc, argv, "imu_lsm9ds1_handler");
ros::NodeHandle n;
ros::Publisher imu_pub = n.advertise<sensor_msgs::Imu>("imu_readings", 1000);
ros::Publisher mf_pub = n.advertise<sensor_msgs::MagneticField>("mag_readings", 1000);
//running rate = 30 Hz
ros::Rate loop_rate(30);
/*************************/
/* Initialize the Sensor */
/*************************/
printf("Selected: LSM9DS1\n");
imu = new LSM9DS1();
/***************/
/* Test Sensor */
/***************/
if (!imu->probe())
{
printf("Sensor not enabled\n");
return EXIT_FAILURE;
}
imuSetup();
while (ros::ok())
{
imuLoop();
sensor_msgs::Imu imu_msg;
sensor_msgs::MagneticField mf_msg;
init_imu_msg(&imu_msg);
init_mf_msg(&mf_msg);
update_imu_msg(&imu_msg, imu);
update_mf_msg(&mf_msg, imu);
imu_pub.publish(imu_msg);
mf_pub.publish(mf_msg);
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
int main(int argc, char *argv[])
{
//-------------------- IMU setup and main loop ----------------------------
imuSetup();
ros::init(argc, argv, "ros_erle_imu_euler");
ros::NodeHandle n;
ros::Publisher imu_euler_pub = n.advertise<ros_erle_imu_euler::euler>("euler", 1000);
ros::Rate loop_rate(50);
while (ros::ok()){
//----------------------- Calculate delta time ----------------------------
gettimeofday(&tv,NULL);
previoustime = currenttime;
currenttime = 1000000 * tv.tv_sec + tv.tv_usec;
dt = (currenttime - previoustime) / 1000000.0;
if(dt < 1/1300.0)
usleep((1/1300.0-dt)*1000000);
gettimeofday(&tv,NULL);
currenttime = 1000000 * tv.tv_sec + tv.tv_usec;
dt = (currenttime - previoustime) / 1000000.0;
//-------- Read raw measurements from the MPU and update AHRS --------------
// Accel + gyro.
imu.getMotion9(&ax, &ay, &az, &gx, &gy, &gz, &mx, &my, &mz);
ahrs.updateIMU(ax, ay, az, gx*0.0175, gy*0.0175, gz*0.0175, dt);
//------------------------ Read Euler angles ------------------------------
ahrs.getEuler(&roll, &pitch, &yaw);
ros_erle_imu_euler::euler msg;
msg.roll = roll;
msg.pitch = pitch;
msg.yaw = yaw;
imu_euler_pub.publish(msg);
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
MainWindow::MainWindow(QWidget *parent) :
QMainWindow(parent), ui(new Ui::MainWindow)
{
ui->setupUi(this);
engScreen = new Dialog();
setCentralWidget(engScreen);
setWindowTitle("6DOFC Engineering Screen");
QObject::connect(ui->actionQuit,SIGNAL(triggered()),this,SLOT(close()));
QObject::connect(ui->actionLog,SIGNAL(triggered()),this,SLOT(log()));
QObject::connect(ui->actionIMU_Setup,SIGNAL(triggered()),this,SLOT(imuSetup()));
}
int main(void) {
halInit();
chSysInit();
sdStart(&SD1, &serialConfig);
pilotSetup();
powerSetup();
motorsSetup();
imuSetup();
pidInit(&pitchPID, 1.0 / 20000.0, 1.0 / 10000, 0);
pidInit(&rollPID, 1.0 / 20000.0, 1.0 / 10000, 0);
pidInit(&yawPID, 1.0 / 20000.0, 1.0 / 10000, 0);
while (TRUE) {
printf("yaw rate %f\r\n", imuGetYawRate());
printf("pitch %f\r\n", imuGetPitch());
printf("roll %f\r\n", imuGetRoll());
// double pitch = pilotGetPitch() - pidUpdate(&pitchPID, 0.0, gyroGetPitchRotation());
// double roll = pilotGetRoll() - pidUpdate(&rollPID, 0, gyroGetRollRotation());
// double yaw = pilotGetYaw() - pidUpdate(&yawPID, 0, gyroGetYawRotation());
// motorsSetControl(CLIP(pitch), CLIP(roll), CLIP(yaw), pilotGetThrottle());
// uint16_t throttle = receiverGetRaw(THROTTLE_CH);
// uint16_t pitch = receiverGetRaw(PITCH_CH);
// uint16_t roll = receiverGetRaw(ROLL_CH);
// uint16_t yaw = receiverGetRaw(YAW_CH);
// printf("%u, %u, %u, %u\r\n", throttle, pitch, roll, yaw);
// printf("%u\r\n", receiverGetRaw(4));
// printf("%f, %f, %f\r\n", receiverGetDouble(PITCH_CH), receiverGetDouble(ROLL_CH), receiverGetDouble(YAW_CH));
// printf("%d, %d, %d\r\n", gyroGetPitchRotation(), gyroGetRollRotation(), gyroGetYawRotation());
// printf("%f, %f, %f\r\n", pitch, roll, yaw);
// printf("%f, %f, %f\r\n", pitchPID.integral, rollPID.integral, yawPID.integral);
chThdSleepSeconds(1);
}
}
int main(int argc, char **argv)
{
// The parameter to this function is the running frequency
if(argc == 2)
{
if(atoi(argv[1]) > 0)
{
freq = atoi(argv[1]);
printf("Frequency selected : %d \n", freq);
}
else
{
printf("Frequency must be more than 0");
return 0;
}
}
/***********************/
/* Initialize The Node */
/***********************/
ros::init(argc, argv, "imu_mpu9250_handler");
ros::NodeHandle n;
ros::Publisher imu_pub = n.advertise<sensor_msgs::Imu>("imu_readings", 1000);
ros::Publisher mf_pub = n.advertise<sensor_msgs::MagneticField>("mag_readings", 1000);
ros::Rate loop_rate(freq);
/*************************/
/* Initialize the Sensor */
/*************************/
printf("Selected: MPU9250\n");
imu = new MPU9250();
/***************/
/* Test Sensor */
/***************/
if (!imu->probe())
{
printf("Sensor not enabled\n");
return EXIT_FAILURE;
}
imuSetup();
while (ros::ok())
{
//acquire data
imuLoop();
//create messages
sensor_msgs::Imu imu_msg;
sensor_msgs::MagneticField mf_msg;
//initialize messages with 0's
init_imu_msg(&imu_msg);
init_mf_msg(&mf_msg);
//put real measured values in the messages
update_imu_msg(&imu_msg, imu);
update_mf_msg(&mf_msg, imu);
//publish the messages on the topics
imu_pub.publish(imu_msg);
mf_pub.publish(mf_msg);
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
int main(void) {
//wakeTime = 0;
//dcCounter = 0;
// Processor Initialization
SetupClock();
SwitchClocks();
SetupPorts();
sclockSetup();
LED_1 = 0;
LED_2 = 0;
LED_3 = 0;
cmdSetup();
radioInit(RADIO_TXPQ_MAX_SIZE, RADIO_RXPQ_MAX_SIZE);
radioSetChannel(RADIO_CHANNEL);
radioSetSrcPanID(RADIO_PAN_ID);
radioSetSrcAddr(RADIO_SRC_ADDR);
dfmemSetup();
uint64_t id = dfmemGetUnqiueID();
telemSetup(); //Timer 5, HW priority 4
mpuSetup();
imuSetup(); //Timer 4, HW priority 3
tiHSetup();
adcSetup();
//AMS Encoders
//encSetup();
//"Open Loop" vibration & jitter generator, AP 2014
//olVibeSetup();
legCtrlSetup(); //Timer 1, HW priority 5
steeringSetup(); //Timer 5, HW priority 4
//Tail control is a special case
//tailCtrlSetup();
//Camera is untested with current code base, AP 12/6/2012
//ovcamSetup();
LED_RED = 1; //Red is use an "alive" indicator
LED_GREEN = 0;
LED_YELLOW = 0;
//Radio startup verification
//if (phyGetState() == 0x16) {
// LED_GREEN = 1;
//}
//Sleeping and low power options
//_VREGS = 1;
//gyroSleep();
/////FUNCTION TEST, NOT FOR PRODUCTION
//olVibeStart();
////////////////////
while (1) {
cmdHandleRadioRxBuffer();
radioProcess();
}
}
