int io_rm_dev(io_t * io, int fd){
if(io_dev_list_empty(io)){
err_check(ERROR_FAIL,"Device not found (empty list)");
}
io_dev_t * dev = io->dev_list.first;
io_dev_t * dev_prev = NULL;
do{
if(dev->fd == fd)
break;
dev_prev = dev;
dev = dev->next;
}while(dev != NULL);
if(dev == NULL){
err_check(ERROR_FAIL,"Device not found");
}
if(dev_prev == NULL){
// Remove the first element of the list
io->dev_list.first = dev->next;
}else{
// skip the one that will be deleted, but keep the tail
dev_prev->next = dev->next;
}
free(dev);
--io->dev_list.dev_count;
return ERROR_OK;
}
/**
* Integrate calibration data into IMU.
* Replaces previous calibration, if any existed.
*
* @param imu
* @param timestamp on the last sample used for the calibration
*
* @return error code.
*/
int imu_comm_calibration_finish(imu_t * imu, struct timeval calibration_end_time){
int retval;
if(imu->status != IMU_COMM_STATE_CALIBRATING){
err_check(ERROR_IMU_STATUS,"Cannot finish calibration, IMU is not calibrating!");
}
if(imu->calibration_counter != 0){
err_check(ERROR_IMU_STATUS,"Not enough samples gathered!");
}
struct timeval calibration_duration;
retval = uquad_timeval_substract(&calibration_duration,calibration_end_time,calibration_start_time);
if(retval < 0){
err_check(ERROR_FAIL,"Calibration duration should not be negative!");
}
//TODO check if calib took too long, and refuse it if so
//define "too long"
imu->calibration_counter = -1;
int i;
for(i=0;i<IMU_SENSOR_COUNT;++i){
imu->null_estimates.xyzrpy[i] = ((double)calibration_tmp[i])/IMU_COMM_CALIBRATION_NULL_SIZE;
}
imu->null_estimates.timestamp = calibration_end_time;
imu->is_calibrated = true;
imu->status = IMU_COMM_STATE_RUNNING;
return ERROR_OK;
}
static msg_t SdThread(void *arg){
chRegSetThreadName("MicroSD");
(void)arg;
FRESULT err;
uint32_t clusters;
FATFS *fsp;
FIL Log;
msg_t id;
/* wait until card not ready */
NOT_READY:
while (!sdcIsCardInserted(&SDCD1))
chThdSleepMilliseconds(SDC_POLLING_INTERVAL);
chThdSleepMilliseconds(SDC_POLLING_INTERVAL);
if (!sdcIsCardInserted(&SDCD1))
goto NOT_READY;
else
insert_handler();
/* fs mounted? */
if (!fs_ready)
return RDY_RESET;
/* are we have at least 16MB of free space? */
err = f_getfree("/", &clusters, &fsp);
err_check();
if ((clusters * (uint32_t)SDC_FS.csize * (uint32_t)MMCSD_BLOCK_SIZE) < (16*1024*1024))
return RDY_RESET;
/* open file for writing log */
char namebuf[MAX_FILENAME_SIZE];
name_from_time(namebuf);
err = f_open(&Log, namebuf, FA_WRITE | FA_CREATE_ALWAYS);
err_check();
/* main write cycle
* This writer waits msg_t with mavlink message ID. Based on that ID it
* will pack extern mavlink struct with proper packing function. */
setGlobalFlag(GlobalFlags.logger_ready);
while (TRUE){
/* wait ID */
if (logwriter_mb.fetch(&id, TIME_INFINITE) == RDY_OK){
if (!sdcIsCardInserted(&SDCD1)){
clearGlobalFlag(GlobalFlags.logger_ready);
remove_handler();
goto NOT_READY;
}
err = WriteLog(&Log, id, &fresh_data);
err_check();
}
err = fs_sync(&Log);
err_check();
}
return 0;
}
/**
* Return file descriptor corresponding to the IMU.
* This should be used when polling devices from the main control loop.
*
* @param imu
* @param fds file descriptor is returned here
*
* @return error code
*/
int imu_comm_get_fds(imu_t * imu,int * fds){
if(imu->device == NULL){
err_check(ERROR_NULL_POINTER,"Cannot get fds, device set to NULL");
}
if(fds == NULL){
err_check(ERROR_NULL_POINTER,"Cannot return fds in NULL pointer");
}
*fds = fileno(imu->device);
return ERROR_OK;
}
static int io_get_dev_list(io_t * io, io_dev_t ** dev){
if(io_dev_list_empty(io)){
err_check(ERROR_FAIL,"Cannot get IO list! List is empty!");
}
*dev = io->dev_list.first;
return ERROR_OK;
}
int main(void){
int s, people, collisions;
double probability;
errno = 0; /* Initialize error number to 0 */
srand(time(0)); /* Initialize rand() seed */
printf("\nInput number of people involved in study SIMULATION:\n");
s = scanf("%d", &people); /* Input # of people in the study */
if(people > 4096){ /* Number of people too large for buffer */
printf("\nToo many individuals for buffer, try smaller value:\n");
s = scanf("%d", &people);
}/*if*/
if(s != 1){ /* scanf input error catching */
printf("\nInput Error\n");
exit (0);
}/*if*/
collisions = birth_function(people);
err_check(errno);/*Error Catching*/
probability = collisions/(people-1.0);/* First Individual cannot be
considered a trial */
probability = probability*100;
printf("\nThe Estimated Probabilty of Collison is %f%%\n\n", probability);
return 0;
}/*main*/
/**
* Checks if reading/writing will block.
* Writing should not be a problem, hw buffers should handle it.
* If attempting to read and there is no data available, we do not want to
* lock up the sys, that is the purpose of 'select'.
*
* @param device attemping to read or write to.
* @param check_read if true then checks if reading locks, if false check writing.
* @param ready answer returned here
*
* @return error code
*/
int imu_comm_check_io_locks(FILE * device, uquad_bool_t * read_ok, uquad_bool_t * write_ok){
fd_set rfds,wfds;
struct timeval tv;
int retval, fd = fileno(device);
FD_ZERO(&rfds);
FD_SET(fd,&rfds);
FD_ZERO(&wfds);
FD_SET(fd,&wfds);
// Set time waiting time to zero
tv.tv_sec = 0;
tv.tv_usec = 0;
// Check if we read/write without locking
retval = select(fd+1, \
(read_ok != NULL)?&rfds:NULL, \
(write_ok != NULL)?&wfds:NULL, \
NULL,&tv);
if(read_ok != NULL){
*read_ok = ((retval > 0) && FD_ISSET(fd,&rfds)) ? true:false;
}
if(write_ok != NULL)
*write_ok = ((retval >0) && FD_ISSET(fd,&wfds)) ? true:false;
if (retval < 0){
err_check(ERROR_IO,"select() failed!");
}else{
// retval == 0 <--> No data available
}
return ERROR_OK;
}
int imu_comm_get_fs(imu_t * imu, int * fs_index){
if(fs_index == NULL){
err_check(ERROR_NULL_POINTER,"Cannot return value, null pointer as argument.");
}
*fs_index = imu->settings.fs;
return ERROR_OK;
}
/**
* Reads 1 byte, expecting it to be IMU_FRAME_INIT_CHAR.
* NOTE: Assumes device can be read without blocking.
*
* @param imu
*
* @return error code
*/
static int imu_comm_get_sync(imu_t * imu, uquad_bool_t * in_sync){
int retval,i;
*in_sync = false;
unsigned char tmp = '@';// Anything diff from IMU_FRAME_INIT_CHAR
for(i=0;i<IMU_DEFAULT_FRAME_SIZE_BYTES;++i){
retval = fread(&tmp,IMU_INIT_END_SIZE,1,imu->device);
if(retval < 0){
err_check(ERROR_IO,"Read error: failed to get sync char...");
}else{
if(retval > 0){
if(tmp == IMU_FRAME_INIT_CHAR){
// No error printing, leave that for upper level
*in_sync = true;
return ERROR_OK;
}
}else{
// retval == 0
*in_sync = false;
return ERROR_READ_SYNC;
}
}
}
// If we read 0 then there is no data available, so no sync and no error.
// Set retval to ERROR_OK, otherwise it'll be # of bytes read
return ERROR_READ_TIMEOUT;
}
/**
* Get IMU calibration.
* Currently only calibration is null estimation.
* //TODO:
* - gain
* - non linearity
*
* @param imu
* @param calibration return data here (check return error code before using)
*
* @return error code
*/
int imu_comm_calibration_get(imu_t * imu, imu_null_estimates_t * calibration){
int retval,i;
if(!imu_comm_calibration_is_calibrated(imu)){
err_check(ERROR_IMU_STATUS,"IMU is not calibrated");
}
if(calibration == NULL){
err_check(ERROR_NULL_POINTER,"Cannot return data in null pointer!");
}
for(i=0;i<IMU_SENSOR_COUNT;++i){
calibration->xyzrpy[i] = imu->null_estimates.xyzrpy[i];
}
calibration->timestamp = imu->null_estimates.timestamp;
return ERROR_OK;
}
int mot_send(uquad_mot_t *mot, double *w)
{
struct timeval tmp_tv, diff_tv;
int retval = ERROR_OK;
static uint8_t buff_out[MOT_C];
// swap order of 0x6b and 0x6a
buff_out[0] = mot->i2c_target[0];
buff_out[1] = mot->i2c_target[2];
buff_out[2] = mot->i2c_target[1];
buff_out[3] = mot->i2c_target[3];
retval = gettimeofday(&tmp_tv,NULL);
err_check_std(retval);
retval = uquad_timeval_substract(&diff_tv,tmp_tv,mot->last_set);
if(diff_tv.tv_usec < MOT_UPDATE_MAX_US && diff_tv.tv_sec < 1)
{
log_tv_only(stderr,diff_tv);
err_check(ERROR_MOT_SATURATE,"Cannot change speed so often!");//TODO restore!
}
retval = uquad_kmsgq_send(mot->kmsgq, buff_out, MOT_C);
err_propagate(retval);
retval = gettimeofday(&mot->last_set,NULL);
err_check_std(retval);
// update current i2c
memcpy(mot->i2c_curr,mot->i2c_target,MOT_C*sizeof(uint8_t));
// update current speed
memcpy(mot->w_curr->m_full,w,MOT_C*sizeof(double));
return ERROR_OK;
}
int main(int argc, char *argv[])
{
int
portno,
retval;
uquad_bool_t
udp = false;
if (argc > 4)
{
err_check(ERROR_INVALID_ARG, USAGE);
}
if (argc > 3)
{
udp = atoi(argv[3]);
}
if (argc > 2)
{
portno = atoi(argv[2]);
}
else
{
err_log_num("Using default port:", CHECK_NET_PORT);
portno = CHECK_NET_PORT;
}
if (argc <= 1)
{
err_log_str("Using default checknet IP:", CHECK_NET_SERVER_IP);
}
retval = uquad_check_net_client((argc > 1)?argv[1]:CHECK_NET_SERVER_IP,
portno,
udp);
if(retval < 0)
{
err_check(retval, "client() failed!");
}
else
{
child_pid = retval;
retval = ERROR_OK;
err_log_num("Client running! Child pid:",child_pid);
for(;;)
sleep(INT_MAX);
}
return retval;
}
/**
* Set accelerometer sensitivity.
* IMU must be idle, and will be left idle.
*
* @param imu
* @param new_value
*
* @return error code
*/
int imu_comm_set_acc_sens(imu_t * imu, int new_value){
int retval;
if((new_value<0) || (new_value > IMU_SENS_OPT_COUNT)){
err_check(ERROR_INVALID_ARG,"Invalid value for acc sensitivity");
}
// IMU should be idle
if(imu->status != IMU_COMM_STATE_IDLE){
err_check(ERROR_IMU_STATUS,"IMU must be idle to set acc sens");
}
// Set new acc sens
retval = imu_comm_send_cmd(imu,imu_sens_opt[new_value]);
err_propagate(retval);
// Update struct value
imu->settings.acc_sens = new_value;
return retval;
}
/**
* IMU fw accepts commands while in idle mode.
*
* @param imu
* @param cmd to send to the imu
*
* @return error code
*/
static int imu_comm_send_cmd(imu_t * imu, unsigned char cmd){
uquad_bool_t ready = false;
int retval;
retval = imu_comm_check_io_locks(imu->device, NULL, &ready);
err_propagate(retval);
if(!ready){
// cannot write
err_check(ERROR_WRITE,"Write error: Writing command would lock.");
}else{
// issue command
retval = fprintf(imu->device,"%c\n",cmd);
if(retval<0){
err_check(ERROR_WRITE,"Write error: Failed to send cmd to IMU");
}
}
return ERROR_OK;
}
/**
* Set sampling frequency (fs)
* IMU must be idle, and will be left idle.
*
* @param imu
* @param new_value
*
* @return error code
*/
int imu_comm_set_fs(imu_t * imu, int new_value){
int retval;
if((new_value<0) || (new_value > IMU_FS_OPT_COUNT)){
err_check(ERROR_INVALID_ARG,"Invalid value for sampling frequency");
}
// IMU should be idle
if(imu->status != IMU_COMM_STATE_IDLE){
err_check(ERROR_IMU_STATUS,"IMU must be idle to set fs");
}
// Set new fs
retval = imu_comm_send_cmd(imu,imu_fs_opt[new_value]);
err_propagate(retval);
// Update struct value
imu->settings.fs = new_value;
imu->settings.T = (double)1/imu_fs_values[new_value];
return retval;
}
static int io_dev_init(io_dev_t * dev, int fd){
if(dev == NULL){
err_check(ERROR_NULL_POINTER,"Cannot init dev, dev is null.");
}
dev->next = NULL;
dev->ready_read = false;
dev->ready_write = false;
dev->fd = fd;
return ERROR_OK;
}
int imu_comm_get_avg(imu_t * imu, imu_measurements_t * measurements){
int retval, i;
if(imu_comm_avg_ready(imu)){
retval = imu_comm_data2measurements(imu, &imu->avg, measurements);
err_propagate(retval);
imu->avg_ready = 0;
return ERROR_OK;
}
err_check(ERROR_IMU_AVG_NOT_ENOUGH,"Not enough samples to average");
}
/**
* Abort current IMU calibration process. All progress will be lost.
* If a previous calibration existed, it will be preserved.
*
* @param imu
*
* @return error code
*/
int imu_comm_calibration_abort(imu_t * imu){
if(imu->status != IMU_COMM_STATE_CALIBRATING){
err_check(ERROR_IMU_STATUS,"Cannot abort calibration, IMU is not calibrating!");
}
// Restore IMU status:
// if we were calibrating then we were running before
imu->status = IMU_COMM_STATE_RUNNING;
return ERROR_OK;
}
int pp_update_setpoint(path_planner_t *pp, uquad_mat_t *x, double w_hover, uquad_bool_t *ctrl_outdated)
{
if(pp == NULL || x == NULL || ctrl_outdated == NULL)
{
err_check(ERROR_NULL_POINTER,"Invalid argument.");
}
if (pp->pt == HOVER)
{
pp->sp->w->m_full[0] = w_hover;
pp->sp->w->m_full[1] = w_hover;
pp->sp->w->m_full[2] = w_hover;
pp->sp->w->m_full[3] = w_hover;
*ctrl_outdated = false;
}
else
{
err_check(ERROR_FAIL, "Not implemented!");
}
return ERROR_OK;
}
int main(int argc, char *argv[]){
errno = 0;/*Initialize error number to 0*/
int check = 0, i=0, temp, N = atoi(argv[1]);
double point_array[N], sum = 0, ave, *p;
if(argc != 2){
printf("\nN-Point Average: Input Error\n\nUsage: {Program_Name} N_value\n\n");
exit(0);
}
printf("\n\n%d-Point Average\n\nInput Data\n", N);
for(i; i < N; i++){/* Initialize array and compute initial Point Averages*/
temp = i;
if(scanf("%lf", &point_array[i]) == 1){
for(temp; temp+1 > 0; --temp){
sum += point_array[temp];
ave = sum/(i+1);
}/*for*/
printf("\nAverage = %lf",ave);
sum = 0;
}/*if*/
else{ /* Input is a smaller number than the Point average */
err_check(errno);
printf("The # of inputs are less than the desired %d-point average\n", N);
printf("The current average is %lf\n", ave);
exit(0);
}/*else*/
}/*for*/
p = point_array; /* p = point_array[0] */
N_point(N, p);
err_check(errno);
return 0;
}/*main*/
/**
* Casts counts to doubles, still in counts.
*
* @param frame
* @param data
*
* @return error code
*/
static int imu_comm_raw2double_raw(imu_frame_t * frame, imu_data_t * data){
int retval, i;
if(frame == NULL || data == NULL){
err_check(ERROR_NULL_POINTER,"Non null pointers required as args...");
}
for (i=0; i<IMU_SENSOR_COUNT; ++i){
data->xyzrpy[i] = (double) frame->raw[i];
}
data->timestamp = frame->timestamp;
data->count = frame->count;
return ERROR_OK;
}
void dev_init(struct wiimote_dev const *dev) {
int ret;
int fd = dev->uinput;
#define _BUTTON(n, bt) do { \
set_key(bt); \
iev[n].type = EV_KEY; \
iev[n].code = bt; \
} while (0)
set_ev(EV_KEY);
BUTTONS;
set_ev(EV_SYN);
#undef _BUTTON
set_ev(EV_ABS);
set_abs(ABS_X, -AXIS_MAX, AXIS_MAX, 2, 4);
iev[11].type = EV_ABS;
iev[11].code = ABS_X;
set_abs(ABS_Y, -AXIS_MAX, AXIS_MAX, 2, 4);
iev[12].type = EV_ABS;
iev[12].code = ABS_Y;
iev[13].type = EV_SYN;
iev[13].code = iev[13].value = 0;
snprintf(padmode.name, UINPUT_MAX_NAME_SIZE, XWII_NAME_CORE " in gamepad mode");
padmode.id.bustype = BUS_VIRTUAL;
/*
padmode.id.vendor = 0;
padmode.id.product = 0;
padmode.id.version = 0;
*/
ret = write(fd, &padmode, sizeof(padmode));
err_check(ret, "set dev properties");
ret = ioctl(fd, UI_DEV_CREATE);
err_check(ret, "create device");
}
/**
* Initiate IMU calibration.
* NOTE: Assumes sensors are not being excited, ie, imu is staying completely still.
*
* @param imu
*
* @return
*/
int imu_comm_calibration_start(imu_t * imu){
if(imu->status != IMU_COMM_STATE_RUNNING){
err_check(ERROR_IMU_STATUS,"IMU must be running to calibrate!");
}
imu->status = IMU_COMM_STATE_CALIBRATING;
int i;
// clear tmp data
for(i=0;i<IMU_SENSOR_COUNT;++i){
calibration_tmp[i] = 0;
}
imu->calibration_counter = IMU_COMM_CALIBRATION_NULL_SIZE;
return ERROR_OK;
}
static void wiimote_key(struct wiimote_dev *dev, struct xwii_event const *ev)
{
unsigned int code;
unsigned int state;
code = ev->v.key.code;
state = ev->v.key.state;
if (code > XWII_KEY_TWO)
return;
if (state > 1)
return;
iev[code].value = state;
if (dev->uinput > 0) {
int ret = write(dev->uinput, iev + code, sizeof(*iev));
err_check(ret, "report button");
ret = write(dev->uinput, iev + 13, sizeof(*iev));
err_check(ret, "report btn SYN");
} else {
fputs("nowhere to report butto presses to\n", stderr);
}
}
int io_get_dev(io_t * io, int fd, io_dev_t ** dev){
int retval;
io_dev_t * dev_curr = NULL;
retval = io_get_dev_list(io,&dev_curr);
err_propagate(retval);
while(dev_curr != NULL){
if(fd == dev_curr->fd){
*dev = dev_curr;
return ERROR_OK;
}
dev_curr = dev_curr->next;
}
err_check(ERROR_IO_DEV_NOT_FOUND,"The fd supplied is not registered...");
}
/**
* Add frame info to build up calibration.
* Will divide by frame count after done gathering, to avoid loosing info.
*
* @param imu
* @param new_frame frame to add to calib
*
* @return error code
*/
int imu_comm_calibration_add_frame(imu_t * imu, imu_frame_t * new_frame){
int retval;
if(imu->status != IMU_COMM_STATE_CALIBRATING){
err_check(ERROR_IMU_STATUS,"Cannot add frames, IMU is not calibrating!");
}
if(imu->calibration_counter <= 0){
err_check(ERROR_FAIL,"Invalid value for calibration_counter");
}
if(imu->calibration_counter == IMU_COMM_CALIBRATION_NULL_SIZE){
calibration_start_time = new_frame->timestamp;
}
int i;
for(i=0;i<IMU_SENSOR_COUNT;++i){
calibration_tmp[i] += new_frame->raw[i];
}
if(--imu->calibration_counter == 0){
retval = imu_comm_calibration_finish(imu,new_frame->timestamp);
err_propagate(retval);
}
return ERROR_OK;
}
static void wiimote_accel(struct wiimote_dev *dev, struct xwii_event const *ev)
{
iev[11].value = -(ev->v.abs[0].y);
iev[12].value = -(ev->v.abs[0].x);
CLIP_AXIS(iev[11].value);
CLIP_AXIS(iev[12].value);
if (dev->uinput > 0) {
int ret = write(dev->uinput, iev + 11, sizeof(*iev));
err_check(ret, "report accel X");
ret = write(dev->uinput, iev + 12, sizeof(*iev));
err_check(ret, "report accel Y");
ret = write(dev->uinput, iev + 13, sizeof(*iev));
err_check(ret, "report accel SYN");
#if 0
printf("reported J (%d, %d) from ev (%d, %d)\n",
iev[11].value, iev[12].value,
-(ev->v.abs[0].y), ev->v.abs[0].x);
#endif
} else {
fputs("nowhere to report accel to\n", stderr);
}
}
/**
* If unread data exists, gets the latest value of the sensor readings, in ADC counts.
*
* Decrements the unread count.
*
* @param imu
* @param data Answer is returned here
*
* @return error code
*/
int imu_comm_get_data_raw_latest_unread(imu_t * imu, imu_data_t * data){
int retval = ERROR_OK;
if(imu->unread_data <= 0){
err_check(ERROR_FAIL,"No unread data available.");
}
imu_frame_t * frame = imu->frame_buffer + frame_circ_index(imu);
data->timestamp = frame->timestamp;
int i;
for(i=0;i<IMU_SENSOR_COUNT;++i){
data->xyzrpy[i] = frame->raw[i];
}
imu->unread_data -= 1;
return retval;
}
/**
* If unread data exists, then calculates the latest value of the sensor readings
* from the RAW data, using current imu calibration.
* This requires a reasonable calibration.
*
* Decrements the unread count.
*
* @param imu
* @param measurements Answer is returned here
*
* @return error code
*/
int imu_comm_get_measurements_latest_unread(imu_t * imu, imu_measurements_t * measurements){
int retval = ERROR_OK;
if(imu->unread_data <= 0){
err_check(ERROR_FAIL,"No unread data available.");
}
imu_frame_t * frame = imu->frame_buffer + frame_circ_index(imu);
imu_data_t data;
retval = imu_comm_raw2double_raw(frame,&data);
err_propagate(retval);
retval = imu_comm_data2measurements(imu,&data,measurements);
err_propagate(retval);
imu->unread_data -= 1;
return retval;
}
int main(void){
int i = 0, s;
errno = 0; /* Initialize error number to 0 */
/* Initialize and populateArray */
for(; i < N; i++){
s = scanf("%lf", &val[i]);
if(s != 1){ printf("\nInput Error\n"); exit(0);}/*if*/
}/*for*/
N_filter();
err_check(errno);
return 0;
}/*main*/