void ControlInput::task(void){
control_receiver->execute();
update_status();
//If the control receiver is OK we'll get data and publish:
if(status == STATUS_OK){
//Get Raw controller values:
//HACK-ALERT: ROLL AND YAW ARE ONLY INVERTED DUE TO THE DX6 transmitter!
control_receiver->get_all_channels(raw_values);
raw_values.roll *= -1;
raw_values.yaw *= -1;
//convert to attitude:
control_socket.pitch = convert_joystick_to_degree(raw_values.pitch);
control_socket.roll = convert_joystick_to_degree(raw_values.roll);
control_socket.yaw = convert_joystick_to_degree(raw_values.yaw);
control_socket.throttle = convert_joystick_to_throttle(raw_values.throttle);
//Update switch and clicker:
aux_states[0] = raw_values.aux1;
aux_states[1] = raw_values.aux2;
if(aux_states[0] != last_aux_states[0]) handle_aux1(aux_states[0]);
if(aux_states[1] != last_aux_states[1]) handle_aux2(aux_states[1]);
last_aux_states[0] = aux_states[0];
last_aux_states[1] = aux_states[1];
//If we are in fixed altitude mode, we'll integrate the throttle stick
//to get a new altitude.
if(control_socket.control_mode == CONTROLMODE_FIXED_ALTITUDE){
process_altitude_setpoint();
}
control_socket.altitude = contrain(control_socket.altitude, 0, 300);
}else{
//THE CONTROL RECEIVER ISN'T OK!!
control_socket.reset();
}
control_socket.publish();
check_calibration();
check_arm();
if(debugging_stream) handle_debug_stream();
}
static bool gyroCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bool optional, int &device_id, bool report_fail)
{
bool success = true;
char s[30];
sprintf(s, "%s%u", GYRO_BASE_DEVICE_PATH, instance);
DevHandle h;
DevMgr::getHandle(s, h);
if (!h.isValid()) {
if (!optional) {
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: NO GYRO SENSOR #%u", instance);
}
}
return false;
}
int ret = check_calibration(h, "CAL_GYRO%u_ID", device_id);
if (ret) {
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GYRO #%u UNCALIBRATED", instance);
}
success = false;
goto out;
}
ret = h.ioctl(GYROIOCSELFTEST, 0);
if (ret != OK) {
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GYRO #%u SELFTEST FAILED", instance);
}
success = false;
goto out;
}
out:
DevMgr::releaseHandle(h);
return success;
}
/**
* \brief This is the default water sensing function. Identify
* the number of pads submerged, and run the water sense
* state machine.
* \ingroup EXEC_ROUTINE
*/
void waterSense_exec(void) {
// If we are in a low freq meas rate, we will delay between measurements.
if (wsData.senseDelayUntilNextMeas > 0) {
wsData.senseDelayUntilNextMeas--;
}
// Don't take measurements while the modem is transmitting.
// It can change the measurement because of power draw on the system.
else if (!modemMgr_isAllocated()) {
// Perform the water measurement
wsData.padStats.lastMeasFlowRateInMl = waterSense_takeReading();
// Determine if we need to change the measurement rate.
switch (wsData.currentWaterState) {
default:
case WATER_STATE_INSTALLED_LF:
doLowFreqReading();
break;
case WATER_STATE_INSTALLED_HF:
doHighFreqReading();
break;
}
wsData.currentWaterState = wsData.nextWaterState;
}
// Determine if we should switch pad max/pad min tables
check_calibration();
#if (PERFORM_TEMPERATURE_MEAS==1)
// Take a temperature measurement every minute
if ((wsData.lastTempMeasTime + TIME_60_SECONDS) <= getSecondsSinceBoot()) {
wsData.padStats.tempCelcius = readInternalTemperature();
wsData.lastTempMeasTime = getSecondsSinceBoot();
}
#endif
return;
}
int
LIS3MDL::ioctl(struct file *filp, int cmd, unsigned long arg)
{
unsigned dummy = arg;
switch (cmd) {
case SENSORIOCSPOLLRATE: {
switch (arg) {
/* switching to manual polling */
case SENSOR_POLLRATE_MANUAL:
stop();
_measure_ticks = 0;
return OK;
/* external signalling (DRDY) not supported */
case SENSOR_POLLRATE_EXTERNAL:
/* zero would be bad */
case 0:
return -EINVAL;
/* set default/max polling rate */
case SENSOR_POLLRATE_MAX:
case SENSOR_POLLRATE_DEFAULT: {
/* do we need to start internal polling? */
bool want_start = (_measure_ticks == 0);
/* set interval for next measurement to minimum legal value */
_measure_ticks = USEC2TICK(LIS3MDL_CONVERSION_INTERVAL);
/* if we need to start the poll state machine, do it */
if (want_start) {
start();
}
return OK;
}
/* adjust to a legal polling interval in Hz */
default: {
/* do we need to start internal polling? */
bool want_start = (_measure_ticks == 0);
/* convert hz to tick interval via microseconds */
unsigned ticks = USEC2TICK(1000000 / arg);
/* check against maximum rate */
if (ticks < USEC2TICK(LIS3MDL_CONVERSION_INTERVAL)) {
// RobD: quick fix for Phil's testing return -EINVAL;
}
/* update interval for next measurement */
_measure_ticks = ticks;
/* if we need to start the poll state machine, do it */
if (want_start) {
start();
}
return OK;
}
}
}
case SENSORIOCGPOLLRATE:
if (_measure_ticks == 0) {
return SENSOR_POLLRATE_MANUAL;
}
return 1000000 / TICK2USEC(_measure_ticks);
case SENSORIOCSQUEUEDEPTH: {
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 1) || (arg > 100)) {
return -EINVAL;
}
irqstate_t flags = px4_enter_critical_section();
if (!_reports->resize(arg)) {
px4_leave_critical_section(flags);
return -ENOMEM;
}
px4_leave_critical_section(flags);
return OK;
}
case SENSORIOCRESET:
return reset();
case MAGIOCSSAMPLERATE:
/* same as pollrate because device is in single measurement mode*/
return ioctl(filp, SENSORIOCSPOLLRATE, arg);
case MAGIOCGSAMPLERATE:
/* same as pollrate because device is in single measurement mode*/
return 1000000 / TICK2USEC(_measure_ticks);
case MAGIOCSRANGE:
return set_range(arg);
case MAGIOCGRANGE:
return _range_ga;
case MAGIOCSSCALE:
/* set new scale factors */
memcpy(&_scale, (struct mag_calibration_s *)arg, sizeof(_scale));
/* check calibration, but not actually return an error */
(void)check_calibration();
return 0;
case MAGIOCGSCALE:
/* copy out scale factors */
memcpy((struct mag_calibration_s *)arg, &_scale, sizeof(_scale));
return 0;
case MAGIOCCALIBRATE:
return calibrate(filp, arg);
case MAGIOCEXSTRAP:
return set_excitement(arg);
case MAGIOCSELFTEST:
return check_calibration();
case MAGIOCGEXTERNAL:
DEVICE_DEBUG("MAGIOCGEXTERNAL in main driver");
return _interface->ioctl(cmd, dummy);
case DEVIOCGDEVICEID:
return _interface->ioctl(cmd, dummy);
default:
/* give it to the superclass */
return CDev::ioctl(filp, cmd, arg);
}
}
int
HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg)
{
switch (cmd) {
case SENSORIOCSPOLLRATE: {
switch (arg) {
/* switching to manual polling */
case SENSOR_POLLRATE_MANUAL:
stop();
_measure_ticks = 0;
return OK;
/* external signalling (DRDY) not supported */
case SENSOR_POLLRATE_EXTERNAL:
/* zero would be bad */
case 0:
return -EINVAL;
/* set default/max polling rate */
case SENSOR_POLLRATE_MAX:
case SENSOR_POLLRATE_DEFAULT: {
/* do we need to start internal polling? */
bool want_start = (_measure_ticks == 0);
/* set interval for next measurement to minimum legal value */
_measure_ticks = USEC2TICK(HMC5883_CONVERSION_INTERVAL);
/* if we need to start the poll state machine, do it */
if (want_start)
start();
return OK;
}
/* adjust to a legal polling interval in Hz */
default: {
/* do we need to start internal polling? */
bool want_start = (_measure_ticks == 0);
/* convert hz to tick interval via microseconds */
unsigned ticks = USEC2TICK(1000000 / arg);
/* check against maximum rate */
if (ticks < USEC2TICK(HMC5883_CONVERSION_INTERVAL))
return -EINVAL;
/* update interval for next measurement */
_measure_ticks = ticks;
/* if we need to start the poll state machine, do it */
if (want_start)
start();
return OK;
}
}
}
case SENSORIOCGPOLLRATE:
if (_measure_ticks == 0)
return SENSOR_POLLRATE_MANUAL;
return 1000000/TICK2USEC(_measure_ticks);
case SENSORIOCSQUEUEDEPTH: {
/* add one to account for the sentinel in the ring */
arg++;
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 2) || (arg > 100))
return -EINVAL;
/* allocate new buffer */
struct mag_report *buf = new struct mag_report[arg];
if (nullptr == buf)
return -ENOMEM;
/* reset the measurement state machine with the new buffer, free the old */
stop();
delete[] _reports;
_num_reports = arg;
_reports = buf;
start();
return OK;
}
case SENSORIOCGQUEUEDEPTH:
return _num_reports - 1;
case SENSORIOCRESET:
return reset();
case MAGIOCSSAMPLERATE:
/* same as pollrate because device is in single measurement mode*/
return ioctl(filp, SENSORIOCSPOLLRATE, arg);
case MAGIOCGSAMPLERATE:
/* same as pollrate because device is in single measurement mode*/
return 1000000/TICK2USEC(_measure_ticks);
case MAGIOCSRANGE:
return set_range(arg);
case MAGIOCGRANGE:
return _range_ga;
case MAGIOCSLOWPASS:
case MAGIOCGLOWPASS:
/* not supported, no internal filtering */
return -EINVAL;
case MAGIOCSSCALE:
/* set new scale factors */
memcpy(&_scale, (mag_scale *)arg, sizeof(_scale));
/* check calibration, but not actually return an error */
(void)check_calibration();
return 0;
case MAGIOCGSCALE:
/* copy out scale factors */
memcpy((mag_scale *)arg, &_scale, sizeof(_scale));
return 0;
case MAGIOCCALIBRATE:
return calibrate(filp, arg);
case MAGIOCEXSTRAP:
return set_excitement(arg);
case MAGIOCSELFTEST:
return check_calibration();
case MAGIOCGEXTERNAL:
if (_bus == PX4_I2C_BUS_EXPANSION)
return 1;
else
return 0;
default:
/* give it to the superclass */
return I2C::ioctl(filp, cmd, arg);
}
}
static bool accelerometerCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bool optional, bool dynamic, int &device_id, bool report_fail)
{
bool success = true;
char s[30];
sprintf(s, "%s%u", ACCEL_BASE_DEVICE_PATH, instance);
DevHandle h;
DevMgr::getHandle(s, h);
if (!h.isValid()) {
if (!optional) {
if (report_fail) {
mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: NO ACCEL SENSOR #%u", instance);
}
}
return false;
}
int ret = check_calibration(h, "CAL_ACC%u_ID", device_id);
if (ret) {
if (report_fail) {
mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: ACCEL #%u UNCALIBRATED", instance);
}
success = false;
goto out;
}
ret = h.ioctl(ACCELIOCSELFTEST, 0);
if (ret != OK) {
if (report_fail) {
mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: ACCEL #%u TEST FAILED: %d", instance, ret);
}
success = false;
goto out;
}
#ifdef __PX4_NUTTX
if (dynamic) {
/* check measurement result range */
struct accel_report acc;
ret = h.read(&acc, sizeof(acc));
if (ret == sizeof(acc)) {
/* evaluate values */
float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);
if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) {
if (report_fail) {
mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: ACCEL RANGE, hold still on arming");
}
/* this is frickin' fatal */
success = false;
goto out;
}
} else {
if (report_fail) {
mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: ACCEL READ");
}
/* this is frickin' fatal */
success = false;
goto out;
}
}
#endif
out:
DevMgr::releaseHandle(h);
return success;
}
int
AK8975::ioctl(struct file *filp, int cmd, unsigned long arg)
{
switch (cmd) {
case SENSORIOCSPOLLRATE: {
switch (arg) {
/* switching to manual polling */
case SENSOR_POLLRATE_MANUAL:
stop();
_measure_ticks = 0;
return OK;
/* external signalling (DRDY) not supported */
case SENSOR_POLLRATE_EXTERNAL:
/* zero would be bad */
case 0:
return -EINVAL;
/* set default/max polling rate */
case SENSOR_POLLRATE_MAX:
case SENSOR_POLLRATE_DEFAULT: {
/* do we need to start internal polling? */
bool want_start = (_measure_ticks == 0);
/* set interval for next measurement to minimum legal value */
_measure_ticks = USEC2TICK(AK8975_CONVERSION_INTERVAL);
/* if we need to start the poll state machine, do it */
if (want_start)
start();
return OK;
}
/* adjust to a legal polling interval in Hz */
default: {
/* do we need to start internal polling? */
bool want_start = (_measure_ticks == 0);
/* convert hz to tick interval via microseconds */
unsigned ticks = USEC2TICK(1000000 / arg);
/* check against maximum rate */
if (ticks < USEC2TICK(AK8975_CONVERSION_INTERVAL))
return -EINVAL;
/* update interval for next measurement */
_measure_ticks = ticks;
/* if we need to start the poll state machine, do it */
if (want_start)
start();
return OK;
}
}
}
case SENSORIOCGPOLLRATE:
if (_measure_ticks == 0)
return SENSOR_POLLRATE_MANUAL;
return 1000000/TICK2USEC(_measure_ticks);
case SENSORIOCSQUEUEDEPTH: {
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 1) || (arg > 100))
return -EINVAL;
irqstate_t flags = irqsave();
if (!_reports->resize(arg)) {
irqrestore(flags);
return -ENOMEM;
}
irqrestore(flags);
return OK;
}
case SENSORIOCGQUEUEDEPTH:
return _reports->size();
case SENSORIOCRESET:
return reset();
case MAGIOCSSAMPLERATE:
/* same as pollrate because device is in single measurement mode*/
return ioctl(filp, SENSORIOCSPOLLRATE, arg);
case MAGIOCGSAMPLERATE:
/* same as pollrate because device is in single measurement mode*/
return 1000000/TICK2USEC(_measure_ticks);
case MAGIOCSRANGE:
// not allowed on this sensor
return -EINVAL;
case MAGIOCGRANGE:
return _range_ga;
case MAGIOCSLOWPASS:
case MAGIOCGLOWPASS:
/* not supported, no internal filtering */
return -EINVAL;
case MAGIOCSSCALE:
/* not supported on this sensor */
return -EINVAL;
case MAGIOCGSCALE:
/* copy out scale factors */
memcpy((mag_scale *)arg, &_scale, sizeof(_scale));
return 0;
case MAGIOCCALIBRATE:
/* not supported */
return -EINVAL;
case MAGIOCEXSTRAP:
/* not supported */
return -EINVAL;
case MAGIOCSELFTEST:
return check_calibration();
case MAGIOCGEXTERNAL:
if (_bus == PX4_I2C_BUS_EXPANSION)
return 1;
else
return 0;
default:
/* give it to the superclass */
return I2C::ioctl(filp, cmd, arg);
}
}
