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; }