aboutsummaryrefslogtreecommitdiff
path: root/src/drivers/px4io/px4io.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'src/drivers/px4io/px4io.cpp')
-rw-r--r--src/drivers/px4io/px4io.cpp181
1 files changed, 114 insertions, 67 deletions
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index ae56b70b3..fed83ea1a 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -89,21 +89,61 @@
#define PX4IO_SET_DEBUG _IOC(0xff00, 0)
#define PX4IO_INAIR_RESTART_ENABLE _IOC(0xff00, 1)
+/**
+ * The PX4IO class.
+ *
+ * Encapsulates PX4FMU to PX4IO communications modeled as file operations.
+ */
class PX4IO : public device::I2C
{
public:
+ /**
+ * Constructor.
+ *
+ * Initialize all class variables.
+ */
PX4IO();
+ /**
+ * Destructor.
+ *
+ * Wait for worker thread to terminate.
+ */
virtual ~PX4IO();
+ /**
+ * Initialize the PX4IO class.
+ *
+ * Initialize the physical I2C interface to PX4IO. Retrieve relevant initial system parameters. Initialize PX4IO registers.
+ */
virtual int init();
+ /**
+ * IO Control handler.
+ *
+ * Handle all IOCTL calls to the PX4IO file descriptor.
+ *
+ * @param[in] filp file handle (not used). This function is always called directly through object reference
+ * @param[in] cmd the IOCTL command
+ * @param[in] the IOCTL command parameter (optional)
+ */
virtual int ioctl(file *filp, int cmd, unsigned long arg);
+
+ /**
+ * write handler.
+ *
+ * Handle writes to the PX4IO file descriptor.
+ *
+ * @param[in] filp file handle (not used). This function is always called directly through object reference
+ * @param[in] buffer pointer to the data buffer to be written
+ * @param[in] len size in bytes to be written
+ * @return number of bytes written
+ */
virtual ssize_t write(file *filp, const char *buffer, size_t len);
/**
* Set the update rate for actuator outputs from FMU to IO.
*
- * @param rate The rate in Hz actuator outpus are sent to IO.
+ * @param[in] rate The rate in Hz actuator output are sent to IO.
* Min 10 Hz, max 400 Hz
*/
int set_update_rate(int rate);
@@ -111,29 +151,41 @@ public:
/**
* Set the battery current scaling and bias
*
- * @param amp_per_volt
- * @param amp_bias
+ * @param[in] amp_per_volt
+ * @param[in] amp_bias
*/
void set_battery_current_scaling(float amp_per_volt, float amp_bias);
/**
* Push failsafe values to IO.
*
- * @param vals Failsafe control inputs: in us PPM (900 for zero, 1500 for centered, 2100 for full)
- * @param len Number of channels, could up to 8
+ * @param[in] vals Failsafe control inputs: in us PPM (900 for zero, 1500 for centered, 2100 for full)
+ * @param[in] len Number of channels, could up to 8
*/
int set_failsafe_values(const uint16_t *vals, unsigned len);
/**
- * Print the current status of IO
- */
+ * Print IO status.
+ *
+ * Print all relevant IO status information
+ */
void print_status();
+ /**
+ * Set the DSM VCC is controlled by relay one flag
+ *
+ * @param[in] enable true=DSM satellite VCC is controlled by relay1, false=DSM satellite VCC not controlled
+ */
inline void set_dsm_vcc_ctl(bool enable)
{
_dsm_vcc_ctl = enable;
};
+ /**
+ * Get the DSM VCC is controlled by relay one flag
+ *
+ * @return true=DSM satellite VCC is controlled by relay1, false=DSM satellite VCC not controlled
+ */
inline bool get_dsm_vcc_ctl()
{
return _dsm_vcc_ctl;
@@ -141,52 +193,48 @@ public:
private:
// XXX
- unsigned _max_actuators;
- unsigned _max_controls;
- unsigned _max_rc_input;
- unsigned _max_relays;
- unsigned _max_transfer;
+ unsigned _max_actuators; ///<Maximum # of actuators supported by PX4IO
+ unsigned _max_controls; ///<Maximum # of controls supported by PX4IO
+ unsigned _max_rc_input; ///<Maximum receiver channels supported by PX4IO
+ unsigned _max_relays; ///<Maximum relays supported by PX4IO
+ unsigned _max_transfer; ///<Maximum number of I2C transfers supported by PX4IO
- unsigned _update_interval; ///< subscription interval limiting send rate
+ unsigned _update_interval; ///<Subscription interval limiting send rate
- volatile int _task; ///< worker task
- volatile bool _task_should_exit;
+ volatile int _task; ///<worker task id
+ volatile bool _task_should_exit; ///<worker terminate flag
- int _mavlink_fd;
+ int _mavlink_fd; ///<mavlink file descriptor
- perf_counter_t _perf_update;
+ perf_counter_t _perf_update; ///<local performance counter
/* cached IO state */
- uint16_t _status;
- uint16_t _alarms;
+ uint16_t _status; ///<Various IO status flags
+ uint16_t _alarms; ///<Various IO alarms
/* subscribed topics */
- int _t_actuators; ///< actuator controls topic
- int _t_armed; ///< system armed control topic
- int _t_vstatus; ///< system / vehicle status
- int _t_param; ///< parameter update topic
+ int _t_actuators; ///<actuator controls topic
+ int _t_armed; ///<system armed control topic
+ int _t_vstatus; ///<system / vehicle status
+ int _t_param; ///<parameter update topic
/* advertised topics */
- orb_advert_t _to_input_rc; ///< rc inputs from io
- orb_advert_t _to_actuators_effective; ///< effective actuator controls topic
- orb_advert_t _to_outputs; ///< mixed servo outputs topic
- orb_advert_t _to_battery; ///< battery status / voltage
+ orb_advert_t _to_input_rc; ///<rc inputs from IO topic
+ orb_advert_t _to_actuators_effective; ///<effective actuator controls topic
+ orb_advert_t _to_outputs; ///<mixed servo outputs topic
+ orb_advert_t _to_battery; ///<battery status / voltage topic
- actuator_outputs_s _outputs; ///< mixed outputs
- actuator_controls_effective_s _controls_effective; ///< effective controls
+ actuator_outputs_s _outputs; ///<mixed outputs
+ actuator_controls_effective_s _controls_effective; ///<effective controls
- bool _primary_pwm_device; ///< true if we are the default PWM output
+ bool _primary_pwm_device; ///<true if we are the default PWM output
- float _battery_amp_per_volt;
- float _battery_amp_bias;
- float _battery_mamphour_total;
- uint64_t _battery_last_timestamp;
+ float _battery_amp_per_volt; ///<current sensor amps/volt
+ float _battery_amp_bias; ///<current sensor bias
+ float _battery_mamphour_total;///<amp hours consumed so far
+ uint64_t _battery_last_timestamp;///<last amp hour calculation timestamp
- /**
- * Relay1 is dedicated to controlling DSM receiver power
- */
-
- bool _dsm_vcc_ctl;
+ bool _dsm_vcc_ctl; ///<true if relay 1 controls DSM satellite RX power
/**
* Trampoline to the worker task
@@ -570,9 +618,11 @@ void
PX4IO::task_main()
{
hrt_abstime last_poll_time = 0;
+ int mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0);
log("starting");
+
/*
* Subscribe to the appropriate PWM output topic based on whether we are the
* primary PWM output or not.
@@ -672,6 +722,25 @@ PX4IO::task_main()
*/
if (fds[3].revents & POLLIN) {
parameter_update_s pupdate;
+ int32_t dsm_bind_val;
+ param_t dsm_bind_param;
+
+ // See if bind parameter has been set, and reset it to 0
+ param_get(dsm_bind_param = param_find("RC_DSM_BIND"), &dsm_bind_val);
+ if (dsm_bind_val) {
+ if (!(_status & PX4IO_P_STATUS_FLAGS_ARMED)) {
+ if ((dsm_bind_val == 1) || (dsm_bind_val == 2)) {
+ mavlink_log_info(mavlink_fd, "[IO] binding dsm%c rx", dsm_bind_val == 1 ? '2' : 'x');
+ ioctl(nullptr, DSM_BIND_START, dsm_bind_val == 1 ? 3 : 7);
+ } else {
+ mavlink_log_info(mavlink_fd, "[IO] invalid bind type, bind request rejected");
+ }
+ } else {
+ mavlink_log_info(mavlink_fd, "[IO] system armed, bind request rejected");
+ }
+ dsm_bind_val = 0;
+ param_set(dsm_bind_param, &dsm_bind_val);
+ }
/* copy to reset the notification */
orb_copy(ORB_ID(parameter_update), _t_param, &pupdate);
@@ -1445,16 +1514,11 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down);
usleep(500000);
io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_set_rx_out);
- usleep(1000);
io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_up);
- usleep(100000);
+ usleep(50000);
io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_send_pulses | (arg << 4));
- break;
-
- case DSM_BIND_STOP:
- io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down);
+ usleep(50000);
io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_reinit_uart);
- usleep(500000);
break;
case DSM_BIND_POWER_UP:
@@ -1603,7 +1667,8 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
}
ssize_t
-PX4IO::write(file *filp, const char *buffer, size_t len)
+PX4IO::write(file * /*filp*/, const char *buffer, size_t len)
+/* Make it obvious that file * isn't used here */
{
unsigned count = len / 2;
@@ -1696,30 +1761,12 @@ bind(int argc, char *argv[])
else
errx(1, "unknown parameter %s, use dsm2 or dsmx", argv[2]);
- /* Open console directly to grab CTRL-C signal */
- int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY);
- if (!console)
- errx(1, "failed opening console");
-
warnx("This command will only bind DSM if satellite VCC (red wire) is controlled by relay 1.");
- warnx("Press CTRL-C or 'c' when done.");
g_dev->ioctl(nullptr, DSM_BIND_START, pulses);
- for (;;) {
- usleep(500000L);
- /* Check if user wants to quit */
- char c;
- if (read(console, &c, 1) == 1) {
- if (c == 0x03 || c == 0x63) {
- warnx("Done\n");
- g_dev->ioctl(nullptr, DSM_BIND_STOP, 0);
- g_dev->ioctl(nullptr, DSM_BIND_POWER_UP, 0);
- close(console);
- exit(0);
- }
- }
- }
+ exit(0);
+
}
void
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-08 03:46:49 +0000