aboutsummaryrefslogtreecommitdiff
path: root/src/drivers
diff options
context:
space:
mode:
Diffstat (limited to 'src/drivers')
-rw-r--r--src/drivers/l3gd20/l3gd20.cpp55
-rw-r--r--src/drivers/px4io/px4io.cpp135
-rw-r--r--src/drivers/rgbled/rgbled.cpp10
3 files changed, 143 insertions, 57 deletions
diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp
index 42a0c264c..05739f04f 100644
--- a/src/drivers/l3gd20/l3gd20.cpp
+++ b/src/drivers/l3gd20/l3gd20.cpp
@@ -71,6 +71,12 @@
#endif
static const int ERROR = -1;
+/* Orientation on board */
+#define SENSOR_BOARD_ROTATION_000_DEG 0
+#define SENSOR_BOARD_ROTATION_090_DEG 1
+#define SENSOR_BOARD_ROTATION_180_DEG 2
+#define SENSOR_BOARD_ROTATION_270_DEG 3
+
/* SPI protocol address bits */
#define DIR_READ (1<<7)
#define DIR_WRITE (0<<7)
@@ -186,6 +192,7 @@ private:
unsigned _current_rate;
unsigned _current_range;
+ unsigned _orientation;
perf_counter_t _sample_perf;
@@ -283,6 +290,7 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) :
_gyro_topic(-1),
_current_rate(0),
_current_range(0),
+ _orientation(SENSOR_BOARD_ROTATION_270_DEG),
_sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")),
_gyro_filter_x(250, 30),
_gyro_filter_y(250, 30),
@@ -363,8 +371,23 @@ L3GD20::probe()
(void)read_reg(ADDR_WHO_AM_I);
/* verify that the device is attached and functioning, accept L3GD20 and L3GD20H */
- if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM || read_reg(ADDR_WHO_AM_I) == WHO_I_AM_H)
+ if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM) {
+
+ #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
+ _orientation = SENSOR_BOARD_ROTATION_270_DEG;
+ #elif CONFIG_ARCH_BOARD_PX4FMU_V2
+ _orientation = SENSOR_BOARD_ROTATION_270_DEG;
+ #else
+ #error This driver needs a board selection, either CONFIG_ARCH_BOARD_PX4FMU_V1 or CONFIG_ARCH_BOARD_PX4FMU_V2
+ #endif
+ return OK;
+ }
+
+
+ if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM_H) {
+ _orientation = SENSOR_BOARD_ROTATION_180_DEG;
return OK;
+ }
return -EIO;
}
@@ -717,9 +740,33 @@ L3GD20::measure()
*/
report->timestamp = hrt_absolute_time();
- /* swap x and y and negate y */
- report->x_raw = raw_report.y;
- report->y_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x);
+ switch (_orientation) {
+
+ case SENSOR_BOARD_ROTATION_000_DEG:
+ /* keep axes in place */
+ report->x_raw = raw_report.x;
+ report->y_raw = raw_report.y;
+ break;
+
+ case SENSOR_BOARD_ROTATION_090_DEG:
+ /* swap x and y */
+ report->x_raw = raw_report.y;
+ report->y_raw = raw_report.x;
+ break;
+
+ case SENSOR_BOARD_ROTATION_180_DEG:
+ /* swap x and y and negate both */
+ report->x_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x);
+ report->y_raw = ((raw_report.y == -32768) ? 32767 : -raw_report.y);
+ break;
+
+ case SENSOR_BOARD_ROTATION_270_DEG:
+ /* swap x and y and negate y */
+ report->x_raw = raw_report.y;
+ report->y_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x);
+ break;
+ }
+
report->z_raw = raw_report.z;
report->x = ((report->x_raw * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 045b3ebcb..8318238e2 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -94,21 +94,62 @@ extern device::Device *PX4IO_serial_interface() weak_function;
#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::CDev
{
public:
+ /**
+ * Constructor.
+ *
+ * Initialize all class variables.
+ */
PX4IO(device::Device *interface);
+
+ /**
+ * 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 outpus are sent to IO.
* Min 10 Hz, max 400 Hz
*/
int set_update_rate(int rate);
@@ -116,16 +157,16 @@ 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);
@@ -149,11 +190,21 @@ public:
*/
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;
@@ -163,60 +214,50 @@ private:
device::Device *_interface;
// XXX
- unsigned _hardware;
- unsigned _max_actuators;
- unsigned _max_controls;
- unsigned _max_rc_input;
- unsigned _max_relays;
- unsigned _max_transfer;
+ unsigned _hardware; ///< Hardware revision
+ 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_actuator_armed; ///< system armed control topic
int _t_vehicle_control_mode; ///< vehicle control mode topic
- int _t_param; ///< parameter update topic
+ 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_safety; ///< status of safety
-
- actuator_outputs_s _outputs; ///< mixed outputs
- actuator_controls_effective_s _controls_effective; ///< effective controls
+ 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
+ orb_advert_t _to_safety; ///< status of safety
- bool _primary_pwm_device; ///< true if we are the default PWM output
+ actuator_outputs_s _outputs; ///<mixed outputs
+ actuator_controls_effective_s _controls_effective; ///<effective controls
- float _battery_amp_per_volt;
- float _battery_amp_bias;
- float _battery_mamphour_total;
- uint64_t _battery_last_timestamp;
+ bool _primary_pwm_device; ///<true if we are the default PWM output
- /**
- * Relay1 is dedicated to controlling DSM receiver power
- */
-
- bool _dsm_vcc_ctl;
-
- /**
- * System armed
- */
+ 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
- bool _system_armed;
+ bool _dsm_vcc_ctl; ///<true if relay 1 controls DSM satellite RX power
/**
* Trampoline to the worker task
@@ -388,8 +429,7 @@ PX4IO::PX4IO(device::Device *interface) :
_battery_amp_bias(0),
_battery_mamphour_total(0),
_battery_last_timestamp(0),
- _dsm_vcc_ctl(false),
- _system_armed(false)
+ _dsm_vcc_ctl(false)
{
/* we need this potentially before it could be set in task_main */
g_dev = this;
@@ -732,7 +772,7 @@ PX4IO::task_main()
// 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 (!_system_armed) {
+ if (!(_status & PX4IO_P_STATUS_FLAGS_OUTPUTS_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);
@@ -853,8 +893,6 @@ PX4IO::io_set_arming_state()
uint16_t set = 0;
uint16_t clear = 0;
- _system_armed = armed.armed;
-
if (armed.armed && !armed.lockdown) {
set |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
} else {
@@ -1731,7 +1769,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;
diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp
index f2543a33c..467379a77 100644
--- a/src/drivers/rgbled/rgbled.cpp
+++ b/src/drivers/rgbled/rgbled.cpp
@@ -456,7 +456,8 @@ rgbled_main(int argc, char *argv[])
int rgbledadr = ADDR; /* 7bit */
int ch;
- while ((ch = getopt(argc, argv, "a:b:")) != EOF) {
+ /* jump over start/off/etc and look at options first */
+ while ((ch = getopt(argc-1, &argv[1], "a:b:")) != EOF) {
switch (ch) {
case 'a':
rgbledadr = strtol(optarg, NULL, 0);
@@ -468,9 +469,8 @@ rgbled_main(int argc, char *argv[])
rgbled_usage();
}
}
- argc -= optind;
- argv += optind;
- const char *verb = argv[0];
+
+ const char *verb = argv[1];
int fd;
int ret;
@@ -549,7 +549,7 @@ rgbled_main(int argc, char *argv[])
if (fd == -1) {
errx(1, "Unable to open " RGBLED_DEVICE_PATH);
}
- if (argc < 4) {
+ if (argc < 5) {
errx(1, "Usage: rgbled rgb <red> <green> <blue>");
}
rgbled_rgbset_t v;
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-18 05:24:08 +0000