int pthread_getschedparam(pthread_t thread, FAR int *policy, FAR struct sched_param *param)
{
int ret;
sdbg("Thread ID=%d policy=0x%p param=0x%p\n", thread, policy, param);
if (!policy || !param)
{
ret = EINVAL;
}
else
{
/* Get the schedparams of the thread. */
ret = sched_getparam((pid_t)thread, param);
if (ret != OK)
{
ret = EINVAL;
}
/* Return the policy. */
*policy = sched_getscheduler((pid_t)thread);
if (*policy == ERROR)
{
ret = *get_errno_ptr();
}
}
sdbg("Returning %d\n", ret);
return ret;
}
int sem_trywait(FAR sem_t *sem)
{
FAR _TCB *rtcb = (FAR _TCB*)g_readytorun.head;
irqstate_t saved_state;
int ret = ERROR;
/* This API should not be called from interrupt handlers */
DEBUGASSERT(up_interrupt_context() == false)
/* Assume any errors reported are due to invalid arguments. */
*get_errno_ptr() = EINVAL;
if (sem)
{
/* The following operations must be performed with interrupts
* disabled because sem_post() may be called from an interrupt
* handler.
*/
saved_state = irqsave();
/* Any further errors could only be occurred because the semaphore
* is not available.
*/
*get_errno_ptr() = EAGAIN;
/* If the semaphore is available, give it to the requesting task */
if (sem->semcount > 0)
{
/* It is, let the task take the semaphore */
sem->semcount--;
rtcb->waitsem = NULL;
ret = OK;
}
/* Interrupts may now be enabled. */
irqrestore(saved_state);
}
return ret;
}
int timer_delete(timer_t timerid)
{
int ret = timer_release((FAR struct posix_timer_s *)timerid);
if (ret < 0)
{
*get_errno_ptr() = -ret;
return ERROR;
}
return OK;
}
int usrsock_setsockopt(FAR struct usrsock_conn_s *conn, int level, int option,
FAR const void *value, FAR socklen_t value_len)
{
struct usrsock_reqstate_s state = {};
net_lock_t save;
ssize_t ret;
DEBUGASSERT(conn);
save = net_lock();
if (conn->state == USRSOCK_CONN_STATE_UNINITIALIZED ||
conn->state == USRSOCK_CONN_STATE_ABORTED)
{
/* Invalid state or closed by daemon. */
nlldbg("usockid=%d; connect() with uninitialized usrsock.\n",
conn->usockid);
ret = (conn->state == USRSOCK_CONN_STATE_ABORTED) ? -EPIPE : -ECONNRESET;
goto errout_unlock;
}
/* Set up event callback for usrsock. */
ret = usrsock_setup_request_callback(conn, &state, setsockopt_event,
USRSOCK_EVENT_ABORT |
USRSOCK_EVENT_REQ_COMPLETE);
if (ret < 0)
{
ndbg("usrsock_setup_request_callback failed: %d\n", ret);
goto errout_unlock;
}
/* Request user-space daemon to close socket. */
ret = do_setsockopt_request(conn, level, option, value, value_len);
if (ret >= 0)
{
/* Wait for completion of request. */
while (net_lockedwait(&state.recvsem) != OK)
{
DEBUGASSERT(*get_errno_ptr() == EINTR);
}
ret = state.result;
}
usrsock_teardown_request_callback(&state);
errout_unlock:
net_unlock(save);
return ret;
}
int rmdir(FAR const char *pathname)
{
FAR struct inode *inode;
const char *relpath = NULL;
int ret;
/* Get an inode for this file */
inode = inode_find(pathname, &relpath);
if (!inode)
{
/* There is no mountpoint that includes in this path */
ret = ENOENT;
goto errout;
}
/* Verify that the inode is a valid mountpoint. */
if (!INODE_IS_MOUNTPT(inode) || !inode->u.i_mops)
{
ret = ENXIO;
goto errout_with_inode;
}
/* Perform the rmdir operation using the relative path
* at the mountpoint.
*/
if (inode->u.i_mops->rmdir)
{
ret = inode->u.i_mops->rmdir(inode, relpath);
if (ret < 0)
{
ret = -ret;
goto errout_with_inode;
}
}
else
{
ret = ENOSYS;
goto errout_with_inode;
}
/* Successfully removed the directory */
inode_release(inode);
return OK;
errout_with_inode:
inode_release(inode);
errout:
*get_errno_ptr() = ret;
return ERROR;
}
static void uart_takesem(FAR sem_t *sem)
{
while (sem_wait(sem) != 0)
{
/* The only case that an error should occur here is if
* the wait was awakened by a signal.
*/
ASSERT(*get_errno_ptr() == EINTR);
}
}
FAR char *getenv(const char *name)
{
FAR _TCB *rtcb;
FAR environ_t *envp;
FAR char *pvar;
FAR char *pvalue = NULL;
int ret = OK;
/* Verify that a string was passed */
if (!name)
{
ret = EINVAL;
goto errout;
}
/* Get a reference to the thread-private environ in the TCB.*/
sched_lock();
rtcb = (FAR _TCB*)g_readytorun.head;
envp = rtcb->envp;
/* Check if the variable exists */
if ( !envp || (pvar = env_findvar(envp, name)) == NULL)
{
ret = ENOENT;
goto errout_with_lock;
}
/* It does! Get the value sub-string from the name=value string */
pvalue = strchr(pvar, '=');
if (!pvalue)
{
/* The name=value string has no '=' This is a bug! */
ret = EINVAL;
goto errout_with_lock;
}
/* Adjust the pointer so that it points to the value right after the '=' */
pvalue++;
sched_unlock();
return pvalue;
errout_with_lock:
sched_unlock();
errout:
*get_errno_ptr() = ret;
return NULL;
}
void nfs_semtake(struct nfsmount *nmp)
{
/* Take the semaphore (perhaps waiting) */
while (sem_wait(&nmp->nm_sem) != 0)
{
/* The only case that an error should occur here is if
* the wait was awakened by a signal.
*/
ASSERT(*get_errno_ptr() == EINTR);
}
}
static inline void _uip_semtake(sem_t *sem)
{
/* Take the semaphore (perhaps waiting) */
while (uip_lockedwait(sem) != 0)
{
/* The only case that an error should occur here is if
* the wait was awakened by a signal.
*/
ASSERT(*get_errno_ptr() == EINTR);
}
}
void smartfs_semtake(struct smartfs_mountpt_s *fs)
{
/* Take the semaphore (perhaps waiting) */
while (sem_wait(fs->fs_sem) != 0)
{
/* The only case that an error should occur here is if
* the wait was awakened by a signal.
*/
ASSERT(*get_errno_ptr() == EINTR);
}
}
static void _net_semtake(FAR struct socketlist *list)
{
/* Take the semaphore (perhaps waiting) */
while (net_lockedwait(&list->sl_sem) != 0)
{
/* The only case that an error should occr here is if
* the wait was awakened by a signal.
*/
ASSERT(*get_errno_ptr() == EINTR);
}
}
off_t telldir(FAR DIR *dirp)
{
struct fs_dirent_s *idir = (struct fs_dirent_s *)dirp;
if (!idir || !idir->fd_root)
{
*get_errno_ptr() = EBADF;
return (off_t)-1;
}
/* Just return the current position */
return idir->fd_position;
}
static inline ssize_t file_write(int fd, FAR const void *buf, size_t nbytes)
{
FAR struct filelist *list;
FAR struct file *this_file;
FAR struct inode *inode;
int ret;
int err;
/* Get the thread-specific file list */
list = sched_getfiles();
if (!list)
{
err = EMFILE;
goto errout;
}
/* Was this file opened for write access? */
this_file = &list->fl_files[fd];
if ((this_file->f_oflags & O_WROK) == 0)
{
err = EBADF;
goto errout;
}
/* Is a driver registered? Does it support the write method? */
inode = this_file->f_inode;
if (!inode || !inode->u.i_ops || !inode->u.i_ops->write)
{
err = EBADF;
goto errout;
}
/* Yes, then let the driver perform the write */
ret = inode->u.i_ops->write(this_file, buf, nbytes);
if (ret < 0)
{
err = -ret;
goto errout;
}
return ret;
errout:
*get_errno_ptr() = err;
return ERROR;
}
static int up_ioctl(FAR struct file *filep, int cmd, unsigned long arg)
{
*get_errno_ptr() = ENOTTY;
return ERROR;
}
static int fat_attrib(const char *path, fat_attrib_t *retattrib,
fat_attrib_t setbits, fat_attrib_t clearbits)
{
struct fat_mountpt_s *fs;
struct fat_dirinfo_s dirinfo;
FAR struct inode *inode;
const char *relpath = NULL;
uint8_t *direntry;
uint8_t oldattributes;
uint8_t newattributes;
int ret;
/* Get an inode for this file */
inode = inode_find(path, &relpath);
if (!inode)
{
/* There is no mountpoint that includes in this path */
ret = ENOENT;
goto errout;
}
/* Verify that the inode is a valid mountpoint. */
if (!INODE_IS_MOUNTPT(inode) || !inode->u.i_mops || !inode->i_private)
{
ret = ENXIO;
goto errout_with_inode;
}
/* Get the mountpoint private data from the inode structure */
fs = inode->i_private;
/* Check if the mount is still healthy */
fat_semtake(fs);
ret = fat_checkmount(fs);
if (ret != OK)
{
goto errout_with_semaphore;
}
/* Find the file/directory entry for the oldrelpath */
ret = fat_finddirentry(fs, &dirinfo, relpath);
if (ret != OK)
{
/* Some error occurred -- probably -ENOENT */
goto errout_with_semaphore;
}
/* Make sure that we found some valid file or directory */
if (dirinfo.fd_root)
{
/* Ooops.. we found the root directory */
ret = EACCES;
goto errout_with_semaphore;
}
/* Get the current attributes */
direntry = &fs->fs_buffer[dirinfo.fd_seq.ds_offset];
oldattributes = DIR_GETATTRIBUTES(direntry);
newattributes = oldattributes;
/* Set or clear any bits as requested */
newattributes &= ~(clearbits & (FATATTR_READONLY|FATATTR_HIDDEN|FATATTR_SYSTEM|FATATTR_ARCHIVE));
newattributes |= (setbits & (FATATTR_READONLY|FATATTR_HIDDEN|FATATTR_SYSTEM|FATATTR_ARCHIVE));
/* Did any thingchange? */
if (newattributes != oldattributes)
{
DIR_PUTATTRIBUTES(direntry, newattributes);
fs->fs_dirty = true;
ret = fat_updatefsinfo(fs);
if (ret != OK)
{
ret = -ret;
goto errout_with_semaphore;
}
}
/* Success */
if (retattrib)
{
*retattrib = newattributes;
}
fat_semgive(fs);
inode_release(inode);
return OK;
errout_with_semaphore:
fat_semgive(fs);
errout_with_inode:
inode_release(inode);
errout:
*get_errno_ptr() = ret;
return ERROR;
}
int sensors_main(int argc, char *argv[])
{
/* inform about start */
printf("[sensors] Initializing..\n");
fflush(stdout);
int ret = OK;
/* start sensor reading */
if (sensors_init() != OK) {
fprintf(stderr, "[sensors] ERROR: Failed to initialize all sensors\n");
/* Clean up */
close(fd_gyro);
close(fd_accelerometer);
close(fd_magnetometer);
close(fd_barometer);
close(fd_adc);
fprintf(stderr, "[sensors] rebooting system.\n");
fflush(stderr);
fflush(stdout);
usleep(100000);
/* Sensors are critical, immediately reboot system on failure */
reboot();
/* Not ever reaching here */
} else {
/* flush stdout from init routine */
fflush(stdout);
}
bool gyro_healthy = false;
bool acc_healthy = false;
bool magn_healthy = false;
bool baro_healthy = false;
bool adc_healthy = false;
bool hil_enabled = false; /**< HIL is disabled by default */
bool publishing = false; /**< the app is not publishing by default, only if HIL is disabled on first run */
int magcounter = 0;
int barocounter = 0;
int adccounter = 0;
unsigned int mag_fail_count = 0;
unsigned int mag_success_count = 0;
unsigned int baro_fail_count = 0;
unsigned int baro_success_count = 0;
unsigned int gyro_fail_count = 0;
unsigned int gyro_success_count = 0;
unsigned int acc_fail_count = 0;
unsigned int acc_success_count = 0;
unsigned int adc_fail_count = 0;
unsigned int adc_success_count = 0;
ssize_t ret_gyro;
ssize_t ret_accelerometer;
ssize_t ret_magnetometer;
ssize_t ret_barometer;
ssize_t ret_adc;
int nsamples_adc;
int16_t buf_gyro[3];
int16_t buf_accelerometer[3];
int16_t buf_magnetometer[7];
float buf_barometer[3];
int16_t mag_offset[3] = {0, 0, 0};
int16_t acc_offset[3] = {0, 0, 0};
int16_t gyro_offset[3] = {0, 0, 0};
bool mag_calibration_enabled = false;
#pragma pack(push,1)
struct adc_msg4_s {
uint8_t am_channel1; /**< The 8-bit ADC Channel 1 */
int32_t am_data1; /**< ADC convert result 1 (4 bytes) */
uint8_t am_channel2; /**< The 8-bit ADC Channel 2 */
int32_t am_data2; /**< ADC convert result 2 (4 bytes) */
uint8_t am_channel3; /**< The 8-bit ADC Channel 3 */
int32_t am_data3; /**< ADC convert result 3 (4 bytes) */
uint8_t am_channel4; /**< The 8-bit ADC Channel 4 */
int32_t am_data4; /**< ADC convert result 4 (4 bytes) */
};
#pragma pack(pop)
struct adc_msg4_s buf_adc;
size_t adc_readsize = 1 * sizeof(struct adc_msg4_s);
float battery_voltage_conversion;
battery_voltage_conversion = global_data_parameter_storage->pm.param_values[PARAM_BATTERYVOLTAGE_CONVERSION];
if (-1 == (int)battery_voltage_conversion) {
/* default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */
battery_voltage_conversion = 3.3f * 52.0f / 5.0f / 4095.0f;
}
#ifdef CONFIG_HRT_PPM
int ppmcounter = 0;
#endif
/* initialize to 100 to execute immediately */
int paramcounter = 100;
int excessive_readout_time_counter = 0;
int read_loop_counter = 0;
/* Empty sensor buffers, avoid junk values */
/* Read first two values of each sensor into void */
(void)read(fd_gyro, buf_gyro, sizeof(buf_gyro));
(void)read(fd_accelerometer, buf_accelerometer, sizeof(buf_accelerometer));
(void)read(fd_magnetometer, buf_magnetometer, sizeof(buf_magnetometer));
if (fd_barometer > 0)(void)read(fd_barometer, buf_barometer, sizeof(buf_barometer));
struct sensor_combined_s raw = {
.timestamp = hrt_absolute_time(),
.gyro_raw = {buf_gyro[0], buf_gyro[1], buf_gyro[2]},
.gyro_raw_counter = 0,
.gyro_rad_s = {0, 0, 0},
.accelerometer_raw = {buf_accelerometer[0], buf_accelerometer[1], buf_accelerometer[2]},
.accelerometer_raw_counter = 0,
.accelerometer_m_s2 = {0, 0, 0},
.magnetometer_raw = {buf_magnetometer[0], buf_magnetometer[1], buf_magnetometer[2]},
.magnetometer_raw_counter = 0,
.baro_pres_mbar = 0,
.baro_alt_meter = 0,
.baro_temp_celcius = 0,
.battery_voltage_v = BAT_VOL_INITIAL,
.adc_voltage_v = {0, 0 , 0},
.baro_raw_counter = 0,
.battery_voltage_counter = 0,
.battery_voltage_valid = false,
};
/* condition to wait for */
pthread_mutex_init(&sensors_read_ready_mutex, NULL);
pthread_cond_init(&sensors_read_ready, NULL);
/* advertise the topic and make the initial publication */
sensor_pub = orb_advertise(ORB_ID(sensor_combined), &raw);
publishing = true;
/* advertise the rc topic */
struct rc_channels_s rc;
memset(&rc, 0, sizeof(rc));
int rc_pub = orb_advertise(ORB_ID(rc_channels), &rc);
/* subscribe to system status */
struct vehicle_status_s vstatus;
memset(&vstatus, 0, sizeof(vstatus));
int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
printf("[sensors] rate: %u Hz\n", (unsigned int)(1000000 / SENSOR_INTERVAL_MICROSEC));
struct hrt_call sensors_hrt_call;
/* Enable high resolution timer callback to unblock main thread, run after 2 ms */
hrt_call_every(&sensors_hrt_call, 2000, SENSOR_INTERVAL_MICROSEC, &sensors_timer_loop, NULL);
while (1) {
pthread_mutex_lock(&sensors_read_ready_mutex);
struct timespec time_to_wait = {0, 0};
/* Wait 2 seconds until timeout */
time_to_wait.tv_nsec = 0;
time_to_wait.tv_sec = time(NULL) + 2;
if (pthread_cond_timedwait(&sensors_read_ready, &sensors_read_ready_mutex, &time_to_wait) == OK) {
pthread_mutex_unlock(&sensors_read_ready_mutex);
bool gyro_updated = false;
bool acc_updated = false;
bool magn_updated = false;
bool baro_updated = false;
bool adc_updated = false;
/* store the time closest to all measurements */
uint64_t current_time = hrt_absolute_time();
raw.timestamp = current_time;
if (paramcounter == 100) {
// XXX paramcounter is not a good name, rename / restructure
// XXX make counter ticks dependent on update rate of sensor main loop
/* Check HIL state */
orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus);
/* switching from non-HIL to HIL mode */
//printf("[sensors] Vehicle mode: %i \t AND: %i, HIL: %i\n", vstatus.mode, vstatus.mode & VEHICLE_MODE_FLAG_HIL_ENABLED, hil_enabled);
if ((vstatus.mode & VEHICLE_MODE_FLAG_HIL_ENABLED) && !hil_enabled) {
hil_enabled = true;
publishing = false;
int ret = close(sensor_pub);
printf("[sensors] Closing sensor pub: %i \n", ret);
/* switching from HIL to non-HIL mode */
} else if (!publishing && !hil_enabled) {
/* advertise the topic and make the initial publication */
sensor_pub = orb_advertise(ORB_ID(sensor_combined), &raw);
hil_enabled = false;
publishing = true;
}
/* Update RC scalings and function mappings */
rc.chan[0].scaling_factor = (10000 / ((global_data_parameter_storage->pm.param_values[PARAM_RC1_MAX] - global_data_parameter_storage->pm.param_values[PARAM_RC1_MIN]) / 2)
* global_data_parameter_storage->pm.param_values[PARAM_RC1_REV]);
rc.chan[0].mid = (uint16_t)global_data_parameter_storage->pm.param_values[PARAM_RC1_TRIM];
rc.chan[1].scaling_factor = (10000 / ((global_data_parameter_storage->pm.param_values[PARAM_RC2_MAX] - global_data_parameter_storage->pm.param_values[PARAM_RC2_MIN]) / 2)
* global_data_parameter_storage->pm.param_values[PARAM_RC2_REV]);
rc.chan[1].mid = (uint16_t)global_data_parameter_storage->pm.param_values[PARAM_RC2_TRIM];
rc.chan[2].scaling_factor = (10000 / ((global_data_parameter_storage->pm.param_values[PARAM_RC3_MAX] - global_data_parameter_storage->pm.param_values[PARAM_RC3_MIN]) / 2)
* global_data_parameter_storage->pm.param_values[PARAM_RC3_REV]);
rc.chan[2].mid = (uint16_t)global_data_parameter_storage->pm.param_values[PARAM_RC3_TRIM];
rc.chan[3].scaling_factor = (10000 / ((global_data_parameter_storage->pm.param_values[PARAM_RC4_MAX] - global_data_parameter_storage->pm.param_values[PARAM_RC4_MIN]) / 2)
* global_data_parameter_storage->pm.param_values[PARAM_RC4_REV]);
rc.chan[3].mid = (uint16_t)global_data_parameter_storage->pm.param_values[PARAM_RC4_TRIM];
rc.chan[4].scaling_factor = (10000 / ((global_data_parameter_storage->pm.param_values[PARAM_RC5_MAX] - global_data_parameter_storage->pm.param_values[PARAM_RC5_MIN]) / 2)
* global_data_parameter_storage->pm.param_values[PARAM_RC5_REV]);
rc.chan[4].mid = (uint16_t)global_data_parameter_storage->pm.param_values[PARAM_RC5_TRIM];
rc.function[0] = global_data_parameter_storage->pm.param_values[PARAM_THROTTLE_CHAN] - 1;
rc.function[1] = global_data_parameter_storage->pm.param_values[PARAM_ROLL_CHAN] - 1;
rc.function[2] = global_data_parameter_storage->pm.param_values[PARAM_PITCH_CHAN] - 1;
rc.function[3] = global_data_parameter_storage->pm.param_values[PARAM_YAW_CHAN] - 1;
rc.function[4] = global_data_parameter_storage->pm.param_values[PARAM_OVERRIDE_CHAN] - 1;
gyro_offset[0] = global_data_parameter_storage->pm.param_values[PARAM_SENSOR_GYRO_XOFFSET];
gyro_offset[1] = global_data_parameter_storage->pm.param_values[PARAM_SENSOR_GYRO_YOFFSET];
gyro_offset[2] = global_data_parameter_storage->pm.param_values[PARAM_SENSOR_GYRO_ZOFFSET];
mag_offset[0] = global_data_parameter_storage->pm.param_values[PARAM_SENSOR_MAG_XOFFSET];
mag_offset[1] = global_data_parameter_storage->pm.param_values[PARAM_SENSOR_MAG_YOFFSET];
mag_offset[2] = global_data_parameter_storage->pm.param_values[PARAM_SENSOR_MAG_ZOFFSET];
paramcounter = 0;
}
paramcounter++;
/* try reading gyro */
uint64_t start_gyro = hrt_absolute_time();
ret_gyro = read(fd_gyro, buf_gyro, sizeof(buf_gyro));
int gyrotime = hrt_absolute_time() - start_gyro;
if (gyrotime > 500) printf("GYRO (pure read): %d us\n", gyrotime);
/* GYROSCOPE */
if (ret_gyro != sizeof(buf_gyro)) {
gyro_fail_count++;
if ((((gyro_fail_count % 20) == 0) || (gyro_fail_count > 20 && gyro_fail_count < 100)) && (int)*get_errno_ptr() != EAGAIN) {
fprintf(stderr, "[sensors] L3GD20 ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr()));
}
if (gyro_healthy && gyro_fail_count >= GYRO_HEALTH_COUNTER_LIMIT_ERROR) {
// global_data_send_subsystem_info(&gyro_present_enabled);
gyro_healthy = false;
gyro_success_count = 0;
}
} else {
gyro_success_count++;
if (!gyro_healthy && gyro_success_count >= GYRO_HEALTH_COUNTER_LIMIT_OK) {
// global_data_send_subsystem_info(&gyro_present_enabled_healthy);
gyro_healthy = true;
gyro_fail_count = 0;
}
gyro_updated = true;
}
gyrotime = hrt_absolute_time() - start_gyro;
if (gyrotime > 500) printf("GYRO (complete): %d us\n", gyrotime);
/* try reading acc */
uint64_t start_acc = hrt_absolute_time();
ret_accelerometer = read(fd_accelerometer, buf_accelerometer, sizeof(buf_accelerometer));
/* ACCELEROMETER */
if (ret_accelerometer != sizeof(buf_accelerometer)) {
acc_fail_count++;
if (acc_fail_count & 0b1000 || (acc_fail_count > 20 && acc_fail_count < 100)) {
fprintf(stderr, "[sensors] BMA180 ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr()));
}
if (acc_healthy && acc_fail_count >= ACC_HEALTH_COUNTER_LIMIT_ERROR) {
// global_data_send_subsystem_info(&acc_present_enabled);
gyro_healthy = false;
acc_success_count = 0;
}
} else {
acc_success_count++;
if (!acc_healthy && acc_success_count >= ACC_HEALTH_COUNTER_LIMIT_OK) {
// global_data_send_subsystem_info(&acc_present_enabled_healthy);
acc_healthy = true;
acc_fail_count = 0;
}
acc_updated = true;
}
int acctime = hrt_absolute_time() - start_acc;
if (acctime > 500) printf("ACC: %d us\n", acctime);
/* MAGNETOMETER */
if (magcounter == 4) { /* 120 Hz */
uint64_t start_mag = hrt_absolute_time();
/* start calibration mode if requested */
if (!mag_calibration_enabled && vstatus.preflight_mag_calibration) {
ioctl(fd_magnetometer, HMC5883L_CALIBRATION_ON, 0);
printf("[sensors] enabling mag calibration mode\n");
mag_calibration_enabled = true;
} else if (mag_calibration_enabled && !vstatus.preflight_mag_calibration) {
ioctl(fd_magnetometer, HMC5883L_CALIBRATION_OFF, 0);
printf("[sensors] disabling mag calibration mode\n");
mag_calibration_enabled = false;
}
ret_magnetometer = read(fd_magnetometer, buf_magnetometer, sizeof(buf_magnetometer));
int errcode_mag = (int) * get_errno_ptr();
int magtime = hrt_absolute_time() - start_mag;
if (magtime > 2000) {
printf("MAG (pure read): %d us\n", magtime);
}
if (ret_magnetometer != sizeof(buf_magnetometer)) {
mag_fail_count++;
if (mag_fail_count & 0b1000 || (mag_fail_count > 20 && mag_fail_count < 100)) {
fprintf(stderr, "[sensors] HMC5883L ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr()));
}
if (magn_healthy && mag_fail_count >= MAGN_HEALTH_COUNTER_LIMIT_ERROR) {
// global_data_send_subsystem_info(&magn_present_enabled);
magn_healthy = false;
mag_success_count = 0;
}
} else {
mag_success_count++;
if (!magn_healthy && mag_success_count >= MAGN_HEALTH_COUNTER_LIMIT_OK) {
// global_data_send_subsystem_info(&magn_present_enabled_healthy);
magn_healthy = true;
mag_fail_count = 0;
}
magn_updated = true;
}
magtime = hrt_absolute_time() - start_mag;
if (magtime > 2000) {
printf("MAG (overall time): %d us\n", magtime);
fprintf(stderr, "[sensors] TIMEOUT HMC5883L ERROR #%d: %s\n", errcode_mag, strerror(errcode_mag));
}
magcounter = 0;
}
magcounter++;
/* BAROMETER */
if (barocounter == 5 && (fd_barometer > 0)) { /* 100 Hz */
uint64_t start_baro = hrt_absolute_time();
*get_errno_ptr() = 0;
ret_barometer = read(fd_barometer, buf_barometer, sizeof(buf_barometer));
if (ret_barometer != sizeof(buf_barometer)) {
baro_fail_count++;
if ((baro_fail_count & 0b1000 || (baro_fail_count > 20 && baro_fail_count < 100)) && (int)*get_errno_ptr() != EAGAIN) {
fprintf(stderr, "[sensors] MS5611 ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr()));
}
if (baro_healthy && baro_fail_count >= BARO_HEALTH_COUNTER_LIMIT_ERROR) {
/* switched from healthy to unhealthy */
baro_healthy = false;
baro_success_count = 0;
// global_data_send_subsystem_info(&baro_present_enabled);
}
} else {
baro_success_count++;
if (!baro_healthy && baro_success_count >= MAGN_HEALTH_COUNTER_LIMIT_OK) {
/* switched from unhealthy to healthy */
baro_healthy = true;
baro_fail_count = 0;
// global_data_send_subsystem_info(&baro_present_enabled_healthy);
}
baro_updated = true;
}
barocounter = 0;
int barotime = hrt_absolute_time() - start_baro;
if (barotime > 2000) printf("BARO: %d us\n", barotime);
}
barocounter++;
/* ADC */
if (adccounter == 5) {
ret_adc = read(fd_adc, &buf_adc, adc_readsize);
nsamples_adc = ret_adc / sizeof(struct adc_msg_s);
if (ret_adc < 0 || nsamples_adc * sizeof(struct adc_msg_s) != ret_adc) {
adc_fail_count++;
if ((adc_fail_count & 0b1000 || adc_fail_count < 10) && (int)*get_errno_ptr() != EAGAIN) {
fprintf(stderr, "[sensors] ADC ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr()));
}
if (adc_healthy && adc_fail_count >= ADC_HEALTH_COUNTER_LIMIT_ERROR) {
adc_healthy = false;
adc_success_count = 0;
}
} else {
adc_success_count++;
if (!adc_healthy && adc_success_count >= ADC_HEALTH_COUNTER_LIMIT_OK) {
adc_healthy = true;
adc_fail_count = 0;
}
adc_updated = true;
}
adccounter = 0;
}
adccounter++;
#ifdef CONFIG_HRT_PPM
bool ppm_updated = false;
/* PPM */
if (ppmcounter == 5) {
/* require at least two channels
* to consider the signal valid
*/
if (ppm_decoded_channels > 1 && (hrt_absolute_time() - ppm_last_valid_decode) < 45000) {
/* Read out values from HRT */
for (int i = 0; i < ppm_decoded_channels; i++) {
rc.chan[i].raw = ppm_buffer[i];
/* Set the range to +-, then scale up */
rc.chan[i].scale = (ppm_buffer[i] - rc.chan[i].mid) * rc.chan[i].scaling_factor;
}
rc.chan_count = ppm_decoded_channels;
rc.timestamp = ppm_last_valid_decode;
/* publish a few lines of code later if set to true */
ppm_updated = true;
}
ppmcounter = 0;
}
ppmcounter++;
#endif
/* Copy values of gyro, acc, magnetometer & barometer */
/* GYROSCOPE */
if (gyro_updated) {
/* copy sensor readings to global data and transform coordinates into px4fmu board frame */
raw.gyro_raw[0] = ((buf_gyro[1] == -32768) ? -32767 : buf_gyro[1]); // x of the board is y of the sensor
/* assign negated value, except for -SHORT_MAX, as it would wrap there */
raw.gyro_raw[1] = ((buf_gyro[0] == -32768) ? 32767 : -buf_gyro[0]); // y on the board is -x of the sensor
raw.gyro_raw[2] = ((buf_gyro[2] == -32768) ? -32767 : buf_gyro[2]); // z of the board is -z of the sensor
/* scale measurements */
// XXX request scaling from driver instead of hardcoding it
/* scaling calculated as: raw * (1/(32768*(500/180*PI))) */
raw.gyro_rad_s[0] = (raw.gyro_raw[0] - gyro_offset[0]) * 0.000266316109f;
raw.gyro_rad_s[1] = (raw.gyro_raw[1] - gyro_offset[1]) * 0.000266316109f;
raw.gyro_rad_s[2] = (raw.gyro_raw[2] - gyro_offset[2]) * 0.000266316109f;
raw.gyro_raw_counter++;
}
/* ACCELEROMETER */
if (acc_updated) {
/* copy sensor readings to global data and transform coordinates into px4fmu board frame */
/* assign negated value, except for -SHORT_MAX, as it would wrap there */
raw.accelerometer_raw[0] = (buf_accelerometer[1] == -32768) ? 32767 : -buf_accelerometer[1]; // x of the board is -y of the sensor
raw.accelerometer_raw[1] = (buf_accelerometer[0] == -32768) ? -32767 : buf_accelerometer[0]; // y on the board is x of the sensor
raw.accelerometer_raw[2] = (buf_accelerometer[2] == -32768) ? -32767 : buf_accelerometer[2]; // z of the board is z of the sensor
// XXX read range from sensor
float range_g = 4.0f;
/* scale from 14 bit to m/s2 */
raw.accelerometer_m_s2[0] = (((raw.accelerometer_raw[0] - acc_offset[0]) * range_g) / 8192.0f) / 9.81f;
raw.accelerometer_m_s2[1] = (((raw.accelerometer_raw[1] - acc_offset[1]) * range_g) / 8192.0f) / 9.81f;
raw.accelerometer_m_s2[2] = (((raw.accelerometer_raw[2] - acc_offset[2]) * range_g) / 8192.0f) / 9.81f;
raw.accelerometer_raw_counter++;
}
/* MAGNETOMETER */
if (magn_updated) {
/* copy sensor readings to global data and transform coordinates into px4fmu board frame */
/* assign negated value, except for -SHORT_MAX, as it would wrap there */
raw.magnetometer_raw[0] = (buf_magnetometer[1] == -32768) ? 32767 : -buf_magnetometer[1]; // x of the board is -y of the sensor
raw.magnetometer_raw[1] = (buf_magnetometer[0] == -32768) ? -32767 : buf_magnetometer[0]; // y on the board is x of the sensor
raw.magnetometer_raw[2] = (buf_magnetometer[2] == -32768) ? -32767 : buf_magnetometer[2]; // z of the board is z of the sensor
// XXX Read out mag range via I2C on init, assuming 0.88 Ga and 12 bit res here
raw.magnetometer_ga[0] = ((raw.magnetometer_raw[0] - mag_offset[0]) / 4096.0f) * 0.88f;
raw.magnetometer_ga[1] = ((raw.magnetometer_raw[1] - mag_offset[1]) / 4096.0f) * 0.88f;
raw.magnetometer_ga[2] = ((raw.magnetometer_raw[2] - mag_offset[2]) / 4096.0f) * 0.88f;
/* store mode */
raw.magnetometer_mode = buf_magnetometer[3];
raw.magnetometer_raw_counter++;
}
/* BAROMETER */
if (baro_updated) {
/* copy sensor readings to global data and transform coordinates into px4fmu board frame */
raw.baro_pres_mbar = buf_barometer[0]; // Pressure in mbar
raw.baro_alt_meter = buf_barometer[1]; // Altitude in meters
raw.baro_temp_celcius = buf_barometer[2]; // Temperature in degrees celcius
raw.baro_raw_counter++;
}
/* ADC */
if (adc_updated) {
/* copy sensor readings to global data*/
if (ADC_BATTERY_VOLATGE_CHANNEL == buf_adc.am_channel1) {
/* Voltage in volts */
raw.battery_voltage_v = (BAT_VOL_LOWPASS_1 * (raw.battery_voltage_v + BAT_VOL_LOWPASS_2 * (uint16_t)(buf_adc.am_data1 * battery_voltage_conversion)));
if ((buf_adc.am_data1 * battery_voltage_conversion) < VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS) {
raw.battery_voltage_valid = false;
raw.battery_voltage_v = 0.f;
} else {
raw.battery_voltage_valid = true;
}
raw.battery_voltage_counter++;
}
}
uint64_t total_time = hrt_absolute_time() - current_time;
/* Inform other processes that new data is available to copy */
if ((gyro_updated || acc_updated || magn_updated || baro_updated) && publishing) {
/* Values changed, publish */
orb_publish(ORB_ID(sensor_combined), sensor_pub, &raw);
}
#ifdef CONFIG_HRT_PPM
if (ppm_updated) {
orb_publish(ORB_ID(rc_channels), rc_pub, &rc);
}
#endif
if (total_time > 2600) {
excessive_readout_time_counter++;
}
if (total_time > 2600 && excessive_readout_time_counter > 100 && excessive_readout_time_counter % 100 == 0) {
fprintf(stderr, "[sensors] slow update (>2600 us): %d us (#%d)\n", (int)total_time, excessive_readout_time_counter);
} else if (total_time > 6000) {
if (excessive_readout_time_counter < 100 || excessive_readout_time_counter % 100 == 0) fprintf(stderr, "[sensors] WARNING: Slow update (>6000 us): %d us (#%d)\n", (int)total_time, excessive_readout_time_counter);
}
read_loop_counter++;
#ifdef CONFIG_SENSORS_DEBUG_ENABLED
if (read_loop_counter % 1000 == 0) printf("[sensors] read loop counter: %d\n", read_loop_counter);
fflush(stdout);
if (sensors_timer_loop_counter % 1000 == 0) printf("[sensors] timer/trigger loop counter: %d\n", sensors_timer_loop_counter);
#endif
}
}
/* Never really getting here */
printf("[sensors] sensor readout stopped\n");
close(fd_gyro);
close(fd_accelerometer);
close(fd_magnetometer);
close(fd_barometer);
close(fd_adc);
printf("[sensors] exiting.\n");
return ret;
}
/**
* Initialize all sensor drivers.
*
* @return 0 on success, != 0 on failure
*/
static int sensors_init(void)
{
printf("[sensors] Sensor configuration..\n");
/* open magnetometer */
fd_magnetometer = open("/dev/hmc5883l", O_RDONLY);
if (fd_magnetometer < 0) {
fprintf(stderr, "[sensors] HMC5883L open fail (err #%d): %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr()));
fflush(stderr);
/* this sensor is critical, exit on failed init */
errno = ENOSYS;
return ERROR;
} else {
printf("[sensors] HMC5883L open ok\n");
}
/* open barometer */
fd_barometer = open("/dev/ms5611", O_RDONLY);
if (fd_barometer < 0) {
fprintf(stderr, "[sensors] MS5611 open fail (err #%d): %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr()));
fflush(stderr);
} else {
printf("[sensors] MS5611 open ok\n");
}
/* open gyro */
fd_gyro = open("/dev/l3gd20", O_RDONLY);
if (fd_gyro < 0) {
fprintf(stderr, "[sensors] L3GD20 open fail (err #%d): %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr()));
fflush(stderr);
/* this sensor is critical, exit on failed init */
errno = ENOSYS;
return ERROR;
} else {
printf("[sensors] L3GD20 open ok\n");
}
/* open accelerometer */
fd_accelerometer = open("/dev/bma180", O_RDONLY);
if (fd_accelerometer < 0) {
fprintf(stderr, "[sensors] BMA180: open fail (err #%d): %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr()));
fflush(stderr);
/* this sensor is critical, exit on failed init */
errno = ENOSYS;
return ERROR;
} else {
printf("[sensors] BMA180 open ok\n");
}
/* open adc */
fd_adc = open("/dev/adc0", O_RDONLY | O_NONBLOCK);
if (fd_adc < 0) {
fprintf(stderr, "[sensors] ADC: open fail (err #%d): %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr()));
fflush(stderr);
/* this sensor is critical, exit on failed init */
errno = ENOSYS;
return ERROR;
} else {
printf("[sensors] ADC open ok\n");
}
/* configure gyro */
if (ioctl(fd_gyro, L3GD20_SETRATE, L3GD20_RATE_760HZ_LP_30HZ) || ioctl(fd_gyro, L3GD20_SETRANGE, L3GD20_RANGE_500DPS)) {
fprintf(stderr, "[sensors] L3GD20 configuration (ioctl) fail (err #%d): %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr()));
fflush(stderr);
/* this sensor is critical, exit on failed init */
errno = ENOSYS;
return ERROR;
} else {
printf("[sensors] L3GD20 configuration ok\n");
}
/* XXX Add IOCTL configuration of remaining sensors */
printf("[sensors] All sensors configured\n");
return OK;
}
int wd_start(WDOG_ID wdog, int delay, wdentry_t wdentry, int argc, ...)
{
va_list ap;
FAR wdog_t *curr;
FAR wdog_t *prev;
FAR wdog_t *next;
int32_t now;
irqstate_t saved_state;
int i;
/* Verify the wdog */
if (!wdog || argc > CONFIG_MAX_WDOGPARMS || delay < 0)
{
*get_errno_ptr() = EINVAL;
return ERROR;
}
/* Check if the watchdog has been started. If so, stop it.
* NOTE: There is a race condition here... the caller may receive
* the watchdog between the time that wd_start is called and
* the critical section is established.
*/
saved_state = irqsave();
if (wdog->active)
{
wd_cancel(wdog);
}
/* Save the data in the watchdog structure */
wdog->func = wdentry; /* Function to execute when delay expires */
up_getpicbase(&wdog->picbase);
wdog->argc = argc;
va_start(ap, argc);
for (i = 0; i < argc; i++)
{
wdog->parm[i] = va_arg(ap, uint32_t);
}
#ifdef CONFIG_DEBUG
for (; i < CONFIG_MAX_WDOGPARMS; i++)
{
wdog->parm[i] = 0;
}
#endif
va_end(ap);
/* Calculate delay+1, forcing the delay into a range that we can handle */
if (delay <= 0)
{
delay = 1;
}
else if (++delay <= 0)
{
delay--;
}
/* Do the easy case first -- when the watchdog timer queue is empty. */
if (g_wdactivelist.head == NULL)
{
sq_addlast((FAR sq_entry_t*)wdog,&g_wdactivelist);
}
/* There are other active watchdogs in the timer queue */
else
{
now = 0;
prev = curr = (FAR wdog_t*)g_wdactivelist.head;
/* Advance to positive time */
while ((now += curr->lag) < 0 && curr->next)
{
prev = curr;
curr = curr->next;
}
/* Advance past shorter delays */
while (now <= delay && curr->next)
{
prev = curr;
curr = curr->next;
now += curr->lag;
}
/* Check if the new wdog must be inserted before the curr. */
if (delay < now)
{
/* The relative delay time is smaller or equal to the current delay
* time, so decrement the current delay time by the new relative
* delay time.
*/
delay -= (now - curr->lag);
curr->lag -= delay;
/* Insert the new watchdog in the list */
if (curr == (FAR wdog_t*)g_wdactivelist.head)
{
sq_addfirst((FAR sq_entry_t*)wdog, &g_wdactivelist);
}
else
{
sq_addafter((FAR sq_entry_t*)prev, (FAR sq_entry_t*)wdog,
&g_wdactivelist);
}
}
/* The new watchdog delay time is greater than the curr delay time,
* so the new wdog must be inserted after the curr. This only occurs
* if the wdog is to be added to the end of the list.
*/
else
{
delay -= now;
if (!curr->next)
{
sq_addlast((FAR sq_entry_t*)wdog, &g_wdactivelist);
}
else
{
next = curr->next;
next->lag -= delay;
sq_addafter((FAR sq_entry_t*)curr, (FAR sq_entry_t*)wdog,
&g_wdactivelist);
}
}
}
/* Put the lag into the watchdog structure and mark it as active. */
wdog->lag = delay;
wdog->active = true;
irqrestore(saved_state);
return OK;
}
int psock_bind(FAR struct socket *psock, const struct sockaddr *addr,
socklen_t addrlen)
{
#ifdef CONFIG_NET_PKT
FAR const struct sockaddr_ll *lladdr = (const struct sockaddr_ll *)addr;
#endif
socklen_t minlen;
int err;
int ret = OK;
/* Verify that the psock corresponds to valid, allocated socket */
if (!psock || psock->s_crefs <= 0)
{
err = ENOTSOCK;
goto errout;
}
/* Verify that a valid address has been provided */
switch (addr->sa_family)
{
#ifdef CONFIG_NET_IPv4
case AF_INET:
minlen = sizeof(struct sockaddr_in);
break;
#endif
#ifdef CONFIG_NET_IPv6
case AF_INET6:
minlen = sizeof(struct sockaddr_in6);
break;
#endif
#ifdef CONFIG_NET_LOCAL
case AF_LOCAL:
minlen = sizeof(sa_family_t);
break;
#endif
#ifdef CONFIG_NET_PKT
case AF_PACKET:
minlen = sizeof(struct sockaddr_ll);
break;
#endif
default:
ndbg("ERROR: Unrecognized address family: %d\n", addr->sa_family);
err = EAFNOSUPPORT;
goto errout;
}
if (addrlen < minlen)
{
ndbg("ERROR: Invalid address length: %d < %d\n", addrlen, minlen);
err = EBADF;
goto errout;
}
/* Perform the binding depending on the protocol type */
switch (psock->s_type)
{
#ifdef CONFIG_NET_PKT
case SOCK_RAW:
ret = pkt_bind(psock->s_conn, lladdr);
break;
#endif
/* Bind a stream socket which may either be TCP/IP or a local, Unix
* domain socket.
*/
#if defined(CONFIG_NET_TCP) || defined(CONFIG_NET_LOCAL_STREAM)
case SOCK_STREAM:
{
#ifdef CONFIG_NET_LOCAL_STREAM
#ifdef CONFIG_NET_TCP
/* Is this a Unix domain socket? */
if (psock->s_domain == PF_LOCAL)
#endif
{
/* Bind the Unix domain connection structure */
ret = psock_local_bind(psock, addr, addrlen);
}
#endif /* CONFIG_NET_LOCAL_STREAM */
#ifdef CONFIG_NET_TCP
#ifdef CONFIG_NET_LOCAL_STREAM
else
#endif
{
/* Bind the TCP/IP connection structure */
ret = tcp_bind(psock->s_conn, addr);
}
#endif /* CONFIG_NET_TCP */
/* Mark the socket bound */
if (ret >= 0)
{
psock->s_flags |= _SF_BOUND;
}
}
break;
#endif /* CONFIG_NET_TCP || CONFIG_NET_LOCAL_STREAM */
/* Bind a datagram socket which may either be TCP/IP or a local, Unix
* domain socket.
*/
#if defined(CONFIG_NET_UDP) || defined(CONFIG_NET_LOCAL_DGRAM)
case SOCK_DGRAM:
{
#ifdef CONFIG_NET_LOCAL_DGRAM
#ifdef CONFIG_NET_UDP
/* Is this a Unix domain socket? */
if (psock->s_domain == PF_LOCAL)
#endif
{
/* Bind the Unix domain connection structure */
ret = psock_local_bind(psock, addr, addrlen);
}
#endif /* CONFIG_NET_LOCAL_DGRAM */
#ifdef CONFIG_NET_UDP
#ifdef CONFIG_NET_LOCAL_DGRAM
else
#endif
{
/* Bind the UDPP/IP connection structure */
ret = udp_bind(psock->s_conn, addr);
}
#endif /* CONFIG_NET_UDP */
/* Mark the socket bound */
if (ret >= 0)
{
psock->s_flags |= _SF_BOUND;
}
}
break;
#endif /* CONFIG_NET_UDP || CONFIG_NET_LOCAL_DGRAM */
default:
err = EBADF;
goto errout;
}
/* Was the bind successful */
if (ret < 0)
{
err = -ret;
goto errout;
}
return OK;
errout:
*get_errno_ptr() = err;
return ERROR;
}
off_t lseek(int fd, off_t offset, int whence)
{
FAR struct filelist *list;
FAR struct file *filep;
FAR struct inode *inode;
int err;
/* Did we get a valid file descriptor? */
if ((unsigned int)fd >= CONFIG_NFILE_DESCRIPTORS)
{
err = EBADF;
goto errout;
}
/* Get the thread-specific file list */
list = sched_getfiles();
if (!list)
{
err = EMFILE;
goto errout;
}
/* Is a driver registered? */
filep = &list->fl_files[fd];
inode = filep->f_inode;
if (inode && inode->u.i_ops)
{
/* Does it support the seek method */
if (inode->u.i_ops->seek)
{
/* Yes, then let it perform the seek */
err = (int)inode->u.i_ops->seek(filep, offset, whence);
if (err < 0)
{
err = -err;
goto errout;
}
}
else
{
/* No... there are a couple of default actions we can take */
switch (whence)
{
case SEEK_CUR:
offset += filep->f_pos;
case SEEK_SET:
if (offset >= 0)
{
filep->f_pos = offset; /* Might be beyond the end-of-file */
break;
}
else
{
err = EINVAL;
goto errout;
}
break;
case SEEK_END:
err = ENOSYS;
goto errout;
default:
err = EINVAL;
goto errout;
}
}
}
return filep->f_pos;
errout:
*get_errno_ptr() = err;
return (off_t)ERROR;
}
int usrsock_socket(int domain, int type, int protocol, FAR struct socket *psock)
{
struct usrsock_reqstate_s state = {};
FAR struct usrsock_conn_s *conn;
net_lock_t save;
int err;
/* Allocate the usrsock socket connection structure and save in the new
* socket instance.
*/
conn = usrsock_alloc();
if (!conn)
{
/* Failed to reserve a connection structure */
return -ENOMEM;
}
save = net_lock();
/* Set up event callback for usrsock. */
err = usrsock_setup_request_callback(conn, &state, socket_event,
USRSOCK_EVENT_ABORT |
USRSOCK_EVENT_REQ_COMPLETE);
if (err < 0)
{
goto errout_free_conn;
}
/* Request user-space daemon for new socket. */
err = do_socket_request(conn, domain, type, protocol);
if (err < 0)
{
goto errout_teardown_callback;
}
/* Wait for completion of request. */
while (net_lockedwait(&state.recvsem) != OK)
{
DEBUGASSERT(*get_errno_ptr() == EINTR);
}
if (state.result < 0)
{
err = state.result;
goto errout_teardown_callback;
}
psock->s_type = SOCK_USRSOCK_TYPE;
psock->s_domain = PF_USRSOCK_DOMAIN;
conn->type = type;
psock->s_conn = conn;
conn->crefs = 1;
usrsock_teardown_request_callback(&state);
net_unlock(save);
return OK;
errout_teardown_callback:
usrsock_teardown_request_callback(&state);
errout_free_conn:
usrsock_free(conn);
net_unlock(save);
return err;
}
int bind(int sockfd, const struct sockaddr *addr, socklen_t addrlen)
{
FAR struct socket *psock = sockfd_socket(sockfd);
#if defined(CONFIG_NET_TCP) || defined(CONFIG_NET_UDP)
#ifdef CONFIG_NET_IPv6
FAR const struct sockaddr_in6 *inaddr = (const struct sockaddr_in6 *)addr;
#else
FAR const struct sockaddr_in *inaddr = (const struct sockaddr_in *)addr;
#endif
#endif
int err;
int ret;
/* Verify that the sockfd corresponds to valid, allocated socket */
if (!psock || psock->s_crefs <= 0)
{
err = EBADF;
goto errout;
}
/* Verify that a valid address has been provided */
#ifdef CONFIG_NET_IPv6
if (addr->sa_family != AF_INET6 || addrlen < sizeof(struct sockaddr_in6))
#else
if (addr->sa_family != AF_INET || addrlen < sizeof(struct sockaddr_in))
#endif
{
err = EBADF;
goto errout;
}
/* Perform the binding depending on the protocol type */
switch (psock->s_type)
{
#ifdef CONFIG_NET_TCP
case SOCK_STREAM:
ret = uip_tcpbind(psock->s_conn, inaddr);
psock->s_flags |= _SF_BOUND;
break;
#endif
#ifdef CONFIG_NET_UDP
case SOCK_DGRAM:
ret = uip_udpbind(psock->s_conn, inaddr);
break;
#endif
default:
err = EBADF;
goto errout;
}
/* Was the bind successful */
if (ret < 0)
{
err = -ret;
goto errout;
}
return OK;
errout:
*get_errno_ptr() = err;
return ERROR;
}
int ioctl(int fd, int req, unsigned long arg)
{
int err;
#if CONFIG_NFILE_DESCRIPTORS > 0
FAR struct filelist *list;
FAR struct file *this_file;
FAR struct inode *inode;
int ret = OK;
/* Did we get a valid file descriptor? */
if ((unsigned int)fd >= CONFIG_NFILE_DESCRIPTORS)
#endif
{
/* Perform the socket ioctl */
#if defined(CONFIG_NET) && CONFIG_NSOCKET_DESCRIPTORS > 0
if ((unsigned int)fd < (CONFIG_NFILE_DESCRIPTORS+CONFIG_NSOCKET_DESCRIPTORS))
{
return netdev_ioctl(fd, req, arg);
}
else
#endif
{
err = EBADF;
goto errout;
}
}
#if CONFIG_NFILE_DESCRIPTORS > 0
/* Get the thread-specific file list */
list = sched_getfiles();
if (!list)
{
err = EMFILE;
goto errout;
}
/* Is a driver registered? Does it support the ioctl method? */
this_file = &list->fl_files[fd];
inode = this_file->f_inode;
if (inode && inode->u.i_ops && inode->u.i_ops->ioctl)
{
/* Yes, then let it perform the ioctl */
ret = (int)inode->u.i_ops->ioctl(this_file, req, arg);
if (ret < 0)
{
err = -ret;
goto errout;
}
}
return ret;
#endif
errout:
*get_errno_ptr() = err;
return ERROR;
}
int stat(const char *path, FAR struct stat *buf)
{
FAR struct inode *inode;
const char *relpath = NULL;
int ret = OK;
/* Sanity checks */
if (!path || !buf)
{
ret = EFAULT;
goto errout;
}
if (!path[0])
{
ret = ENOENT;
goto errout;
}
/* Check for the fake root directory (which has no inode) */
if (strcmp(path, "/") == 0)
{
return statroot(buf);
}
/* Get an inode for this file */
inode = inode_find(path, &relpath);
if (!inode)
{
/* This name does not refer to a psudeo-inode and there is no
* mountpoint that includes in this path.
*/
ret = ENOENT;
goto errout;
}
/* The way we handle the stat depends on the type of inode that we
* are dealing with.
*/
#ifndef CONFIG_DISABLE_MOUNTPOINT
if (INODE_IS_MOUNTPT(inode))
{
/* The node is a file system mointpoint. Verify that the mountpoint
* supports the stat() method
*/
if (inode->u.i_mops && inode->u.i_mops->stat)
{
/* Perform the rewinddir() operation */
ret = inode->u.i_mops->stat(inode, relpath, buf);
}
}
else
#endif
{
/* The node is part of the root psuedo file system */
ret = statpsuedo(inode, buf);
}
/* Check if the stat operation was successful */
if (ret < 0)
{
ret = -ret;
goto errout_with_inode;
}
/* Successfully stat'ed the file */
inode_release(inode);
return OK;
/* Failure conditions always set the errno appropriately */
errout_with_inode:
inode_release(inode);
errout:
*get_errno_ptr() = ret;
return ERROR;
}
ssize_t send(int sockfd, const void *buf, size_t len, int flags)
{
FAR struct socket *psock = sockfd_socket(sockfd);
struct send_s state;
uip_lock_t save;
int err;
int ret = OK;
/* Verify that the sockfd corresponds to valid, allocated socket */
if (!psock || psock->s_crefs <= 0)
{
err = EBADF;
goto errout;
}
/* If this is an un-connected socket, then return ENOTCONN */
if (psock->s_type != SOCK_STREAM || !_SS_ISCONNECTED(psock->s_flags))
{
err = ENOTCONN;
goto errout;
}
/* Set the socket state to sending */
psock->s_flags = _SS_SETSTATE(psock->s_flags, _SF_SEND);
/* Perform the TCP send operation */
/* Initialize the state structure. This is done with interrupts
* disabled because we don't want anything to happen until we
* are ready.
*/
save = uip_lock();
memset(&state, 0, sizeof(struct send_s));
(void)sem_init(&state. snd_sem, 0, 0); /* Doesn't really fail */
state.snd_sock = psock; /* Socket descriptor to use */
state.snd_buflen = len; /* Number of bytes to send */
state.snd_buffer = buf; /* Buffer to send from */
if (len > 0)
{
struct uip_conn *conn = (struct uip_conn*)psock->s_conn;
/* Allocate resources to receive a callback */
state.snd_cb = uip_tcpcallbackalloc(conn);
if (state.snd_cb)
{
/* Get the initial sequence number that will be used */
state.snd_isn = uip_tcpgetsequence(conn->sndseq);
/* There is no outstanding, unacknowledged data after this
* initial sequence number.
*/
conn->unacked = 0;
/* Update the initial time for calculating timeouts */
#if defined(CONFIG_NET_SOCKOPTS) && !defined(CONFIG_DISABLE_CLOCK)
state.snd_time = clock_systimer();
#endif
/* Set up the callback in the connection */
state.snd_cb->flags = UIP_ACKDATA|UIP_REXMIT|UIP_POLL|UIP_CLOSE|UIP_ABORT|UIP_TIMEDOUT;
state.snd_cb->priv = (void*)&state;
state.snd_cb->event = send_interrupt;
/* Notify the device driver of the availaibilty of TX data */
netdev_txnotify(&conn->ripaddr);
/* Wait for the send to complete or an error to occur: NOTES: (1)
* uip_lockedwait will also terminate if a signal is received, (2) interrupts
* may be disabled! They will be re-enabled while the task sleeps and
* automatically re-enabled when the task restarts.
*/
ret = uip_lockedwait(&state. snd_sem);
/* Make sure that no further interrupts are processed */
uip_tcpcallbackfree(conn, state.snd_cb);
}
}
sem_destroy(&state. snd_sem);
uip_unlock(save);
/* Set the socket state to idle */
psock->s_flags = _SS_SETSTATE(psock->s_flags, _SF_IDLE);
/* Check for a errors. Errors are signaled by negative errno values
* for the send length
*/
if (state.snd_sent < 0)
{
err = state.snd_sent;
goto errout;
}
/* If uip_lockedwait failed, then we were probably reawakened by a signal. In
* this case, uip_lockedwait will have set errno appropriately.
*/
if (ret < 0)
{
err = -ret;
goto errout;
}
/* Return the number of bytes actually sent */
return state.snd_sent;
errout:
*get_errno_ptr() = err;
return ERROR;
}
FAR struct dirent *readdir(DIR *dirp)
{
FAR struct fs_dirent_s *idir = (struct fs_dirent_s *)dirp;
#ifndef CONFIG_DISABLE_MOUNTPOINT
struct inode *inode;
#endif
int ret;
/* Sanity checks */
if (!idir || !idir->fd_root)
{
ret = EBADF;
goto errout;
}
/* The way we handle the readdir depends on the type of inode
* that we are dealing with.
*/
#ifndef CONFIG_DISABLE_MOUNTPOINT
inode = idir->fd_root;
if (INODE_IS_MOUNTPT(inode) && !DIRENT_ISPSUEDONODE(idir->fd_flags))
{
/* The node is a file system mointpoint. Verify that the mountpoint
* supports the readdir() method
*/
if (!inode->u.i_mops || !inode->u.i_mops->readdir)
{
ret = EACCES;
goto errout;
}
/* Perform the readdir() operation */
ret = inode->u.i_mops->readdir(inode, idir);
}
else
#endif
{
/* The node is part of the root psuedo file system */
ret = readpsuedodir(idir);
}
/* ret < 0 is an error. Special case: ret = -ENOENT is end of file */
if ( ret < 0)
{
if (ret == -ENOENT)
{
ret = OK;
}
else
{
ret = -ret;
}
goto errout;
}
/* Success */
idir->fd_position++;
return &idir->fd_dir;
errout:
*get_errno_ptr() = ret;
return NULL;
}
