Initial implementation of application access to the PX4IO relays.

This commit is contained in:
px4dev 2012-12-20 21:31:02 -08:00
parent f40e4d13aa
commit 5b92c51777
3 changed files with 53 additions and 6 deletions

View File

@ -95,7 +95,7 @@ ORB_DECLARE(output_pwm);
* Note that ioctls and ObjDev updates should not be mixed, as the * Note that ioctls and ObjDev updates should not be mixed, as the
* behaviour of the system in this case is not defined. * behaviour of the system in this case is not defined.
*/ */
#define _PWM_SERVO_BASE 0x2700 #define _PWM_SERVO_BASE 0x2a00
/** arm all servo outputs handle by this driver */ /** arm all servo outputs handle by this driver */
#define PWM_SERVO_ARM _IOC(_PWM_SERVO_BASE, 0) #define PWM_SERVO_ARM _IOC(_PWM_SERVO_BASE, 0)

View File

@ -61,6 +61,7 @@
#include <drivers/device/device.h> #include <drivers/device/device.h>
#include <drivers/drv_rc_input.h> #include <drivers/drv_rc_input.h>
#include <drivers/drv_pwm_output.h> #include <drivers/drv_pwm_output.h>
#include <drivers/drv_gpio.h>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include <drivers/drv_mixer.h> #include <drivers/drv_mixer.h>
@ -114,6 +115,8 @@ private:
bool _primary_pwm_device; ///< true if we are the default PWM output bool _primary_pwm_device; ///< true if we are the default PWM output
uint32_t _relays; ///< state of the PX4IO relays, one bit per relay
volatile bool _switch_armed; ///< PX4IO switch armed state volatile bool _switch_armed; ///< PX4IO switch armed state
// XXX how should this work? // XXX how should this work?
@ -185,6 +188,7 @@ PX4IO::PX4IO() :
_t_outputs(-1), _t_outputs(-1),
_mixers(nullptr), _mixers(nullptr),
_primary_pwm_device(false), _primary_pwm_device(false),
_relays(0),
_switch_armed(false), _switch_armed(false),
_send_needed(false), _send_needed(false),
_config_needed(false) _config_needed(false)
@ -497,7 +501,9 @@ PX4IO::io_send()
/* publish as we send */ /* publish as we send */
orb_publish(ORB_ID_VEHICLE_CONTROLS, _t_outputs, &_outputs); orb_publish(ORB_ID_VEHICLE_CONTROLS, _t_outputs, &_outputs);
// XXX relays /* update relays */
for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++)
cmd.relay_state[i] = (_relays & (1<< i)) ? true : false;
/* armed and not locked down */ /* armed and not locked down */
cmd.arm_ok = (_armed.armed && !_armed.lockdown); cmd.arm_ok = (_armed.armed && !_armed.lockdown);
@ -559,6 +565,30 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
*(servo_position_t *)arg = _outputs.output[cmd - PWM_SERVO_GET(0)]; *(servo_position_t *)arg = _outputs.output[cmd - PWM_SERVO_GET(0)];
break; break;
case GPIO_RESET:
_relays = 0;
_send_needed = true;
break;
case GPIO_SET:
case GPIO_CLEAR:
/* make sure only valid bits are being set */
if ((arg & ((1UL << PX4IO_RELAY_CHANNELS) - 1)) != arg) {
ret = EINVAL;
break;
}
if (cmd == GPIO_SET) {
_relays |= arg;
} else {
_relays &= ~arg;
}
_send_needed = true;
break;
case GPIO_GET:
*(uint32_t *)arg = _relays;
break;
case MIXERIOCGETOUTPUTCOUNT: case MIXERIOCGETOUTPUTCOUNT:
*(unsigned *)arg = _max_actuators; *(unsigned *)arg = _max_actuators;
break; break;

View File

@ -178,7 +178,7 @@ comms_handle_command(const void *buffer, size_t length)
system_state.fmu_channel_data[i] = cmd->servo_command[i]; system_state.fmu_channel_data[i] = cmd->servo_command[i];
/* if the IO is armed and the FMU gets disarmed, the IO must also disarm */ /* if the IO is armed and the FMU gets disarmed, the IO must also disarm */
if(system_state.arm_ok && !cmd->arm_ok) { if (system_state.arm_ok && !cmd->arm_ok) {
system_state.armed = false; system_state.armed = false;
} }
@ -191,9 +191,26 @@ comms_handle_command(const void *buffer, size_t length)
// if (!system_state.arm_ok && system_state.armed) // if (!system_state.arm_ok && system_state.armed)
// system_state.armed = false; // system_state.armed = false;
/* XXX do relay changes here */ /* handle relay state changes here */
for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++) for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++) {
system_state.relays[i] = cmd->relay_state[i]; if (system_state.relays[i] != cmd->relay_state[i]) {
switch (i) {
case 0:
POWER_ACC1(cmd->relay_state[i]);
break;
case 1:
POWER_ACC2(cmd->relay_state[i]);
break;
case 2:
POWER_RELAY1(cmd->relay_state[i]);
break;
case 3:
POWER_RELAY2(cmd->relay_state[i]);
break;
}
}
system_state.relays[i] != cmd->relay_state[i]
}
irqrestore(flags); irqrestore(flags);
} }