/**
* @brief Generates the message for sending the sensor data.
*
* @param[out] Cbuff Pointer to the circular buffer to put the data in.
* @return HAL_FAILED if the message didn't fit or HAL_SUCCESS
* if it did fit.
*/
static bool GenerateGetIMUData(circular_buffer_t *Cbuff)
{
/* Temporary IMU data holder */
static imu_data_t imu_data;
GetIMUData(&imu_data);
return GenerateGenericCommand(Cmd_GetIMUData,
(uint8_t *)&imu_data,
SENSOR_IMU_DATA_SIZE,
Cbuff);
}
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;
}
/**
* @brief Main estimation thread.
*
* @param[in/out] arg Unused.
*/
static THD_FUNCTION(ThreadEstimation, arg)
{
(void)arg;
chRegSetThreadName("Estimation");
tp = chThdGetSelfX();
/* Initialization data */
//uint32_t i;
//quaternion_t q_init = {1.0f, 0.0f, 0.0f, 0.0f};
//vector3f_t am, wb_init = {0.0f, 0.0f, 0.0f}; //, acc_init, mag_init;
/* Event registration for new sensor data */
event_listener_t el;
/* Register to new data from accelerometer and gyroscope */
chEvtRegisterMaskWithFlags(ptrGetNewDataEventSource(),
&el,
EVENT_MASK(0),
ACCGYRO_DATA_AVAILABLE_EVENTMASK |
MAG_DATA_AVAILABLE_EVENTMASK |
BARO_DATA_AVAILABLE_EVENTMASK);
/* Force an initialization of the estimation */
//chEvtAddEvents(ESTIMATION_RESET_EVENTMASK);
//AttitudeEstimationInit(&states, &data, &q_init, &wb_init);
vInitializeMotionCaptureEstimator(&states);
while(1)
{
//if (isnan(states.q.q0) || isnan(states.q.q1) || isnan(states.q.q2) || isnan(states.q.q3) ||
// isnan(states.w.x) || isnan(states.w.y) || isnan(states.w.z) ||
// isnan(states.wb.x) || isnan(states.wb.y) || isnan(states.wb.z))
// AttitudeEstimationInit(&states, &data, &q_init, &wb_init);
/* Check if there has been a request to reset the filter */
//if (chEvtWaitOneTimeout(ESTIMATION_RESET_EVENTMASK, TIME_IMMEDIATE))
// {
/* Initialize the estimation */
//AttitudeEstimationInit(&states, &data, &q_init, &wb_init);
//}
/* Wait for new measurement data */
chEvtWaitAny(ALL_EVENTS);
eventflags_t flags = chEvtGetAndClearFlags(&el);
if (flags & ACCGYRO_DATA_AVAILABLE_EVENTMASK)
{
/* Get sensor data */
GetIMUData(&imu_data);
/* Run estimation */
vInnovateMotionCaptureEstimator(&states,
&imu_data,
SENSOR_ACCGYRO_DT,
0.0007f);
/*InnovateAttitudeEKF(&states,
&data,
imu_data.gyroscope,
imu_data.accelerometer,
imu_data.magnetometer,
0.0f,
0.0f,
ESTIMATION_DT);*/
//states.w.x = -imu_data.gyroscope[0];
//states.w.y = -imu_data.gyroscope[1];
//states.w.z = imu_data.gyroscope[2];
//am.x = -imu_data.accelerometer[0];
//am.y = -imu_data.accelerometer[1];
//am.z = imu_data.accelerometer[2];
//states.q = MadgwickAHRSupdateIMU(states.w,
// am,
// states.q,
// 0.15f,
// dt);
/* Broadcast new estimation available */
chEvtBroadcastFlags(&estimation_events_es,
ESTIMATION_NEW_ESTIMATION_EVENTMASK);
}
}
}
