void IMU::InternalInit( SerialPort *pport, uint8_t update_rate_hz, char stream_type ) {
current_stream_type = stream_type;
yaw_offset_degrees = 0;
accel_fsr_g = 2;
gyro_fsr_dps = 2000;
flags = 0;
this->update_rate_hz = update_rate_hz;
m_task = new Task("IMU", (FUNCPTR)imuTask,Task::kDefaultPriority+1);
yaw = 0.0;
pitch = 0.0;
roll = 0.0;
pserial_port = pport;
pserial_port->Reset();
InitIMU();
m_task->Start((uint32_t)this);
}
void IMU::InternalInit( SerialPort::Port port, uint8_t update_rate_hz, char stream_type ) {
serial_port_id = port;
pserial_port = new SerialPort(57600,serial_port_id);
current_stream_type = stream_type;
yaw_offset_degrees = 0;
accel_fsr_g = 2;
gyro_fsr_dps = 2000;
flags = 0;
this->update_rate_hz = update_rate_hz;
m_task = new Task("IMU", (FUNCPTR)imuTask,Task::kDefaultPriority+1);
yaw = 0.0;
pitch = 0.0;
roll = 0.0;
yaw_crossing_count = 0;
yaw_last_direction = 0;
last_yaw_rate = 0.0f;
pserial_port->Reset();
InitIMU();
m_task->Start((uint32_t)this);
}
int main(int argc, char *argv[])
{
long iter, repeat = 0;
double interval_sec = (double)1/20;
struct timespec start, end;
int opt;
/*
* get command line options
*/
while ((opt = getopt(argc, argv, "i:h")) != -1) {
switch (opt) {
case 'i': /* iterations */
repeat = strtol(optarg, NULL, 0);
PDEBUG("repeat=%ld\n", repeat);
break;
case 'h':
usage(argc, argv);
break;
}
}
/* Initialize model */
EKF_IFS_2_initialize();
/* Initialize hardware */
InitIMU(); /* vectornav */
InitSerial(); /* arduino */
clock_gettime(CLOCK_REALTIME, &start);
iter = 0;
while (1) {
double remain_us;
uint64_t tmpdiff;
/* Get sensor data */
GetIMUData(&EKF_IFS_2_U);
/* Get Arduino Data */
GetSerialData(&EKF_IFS_2_U);
/* Get moving points Data */
InitMovingWaypoints(&EKF_IFS_2_U);
/* Get waypoints Data */
InitStaticWaypoints(&EKF_IFS_2_U);
/* Get Servo deflection Data */
InitOther(&EKF_IFS_2_U);
/* Step the model */
EKF_IFS_2_step();
/* Output to the motor controller */
SendSerialData(&EKF_IFS_2_Y);
/* Time book keeping */
clock_gettime(CLOCK_REALTIME, &end);
tmpdiff = get_elapsed(&start, &end);
remain_us = (interval_sec * 1000000 - tmpdiff / 1000);
if (remain_us > 0) {
usleep((useconds_t)remain_us);
}
clock_gettime(CLOCK_REALTIME, &start);
iter++;
PDEBUG("iter %ld took %" PRIu64 "us\n", iter, tmpdiff/1000);
PDEBUG("Out: throttle=%f elevator=%f aileron=%f rudder=%f\n",
EKF_IFS_2_Y.ControlSurfaceCommands.throttle_cmd,
EKF_IFS_2_Y.ControlSurfaceCommands.elevator_cmd,
EKF_IFS_2_Y.ControlSurfaceCommands.aileron_cmd,
EKF_IFS_2_Y.ControlSurfaceCommands.rudder_cmd);
if (iter >= repeat)
break;
}
/* Matfile logging */
rt_StopDataLogging(MATFILE, EKF_IFS_2_M->rtwLogInfo);
/* Terminate model */
EKF_IFS_2_terminate();
/* Close hardware */
CloseIMU();
CloseSerial();
return 0;
}
