fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 19:53:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' into beta

Browse Source
This commit is contained in:
Anton Babushkin
2014-01-23 23:10:05 +01:00
parent 55f845888b 4524fe3e48
commit b3d98e4a19
12 changed files with 247 additions and 143 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/boards/px4io-v2/board_config.h
+1 -1
View File
@@ -114,7 +114,7 @@
/* XXX these should be UART pins */
#define GPIO_SBUS_INPUT (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN11)
#define GPIO_SBUS_OUTPUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10)
#define GPIO_SBUS_OENABLE (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN4)
#define GPIO_SBUS_OENABLE (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN4)
/*
* High-resolution timer
src/drivers/boards/px4io-v2/px4iov2_init.c
-2
View File
@@ -124,8 +124,6 @@ __EXPORT void stm32_boardinitialize(void)
stm32_configgpio(GPIO_ADC_VSERVO);
stm32_configgpio(GPIO_SBUS_INPUT); /* xxx alternate function */
stm32_gpiowrite(GPIO_SBUS_OUTPUT, false);
stm32_configgpio(GPIO_SBUS_OUTPUT);
/* sbus output enable is active low - disable it by default */
src/drivers/drv_pwm_output.h
+4
View File
@@ -189,6 +189,10 @@ ORB_DECLARE(output_pwm);
/** get the maximum PWM value the output will send */
#define PWM_SERVO_GET_MAX_PWM _IOC(_PWM_SERVO_BASE, 19)
/** set the number of servos in (unsigned)arg - allows change of
* split between servos and GPIO */
#define PWM_SERVO_SET_COUNT _IOC(_PWM_SERVO_BASE, 20)
/** set a single servo to a specific value */
#define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo)
src/drivers/hmc5883/hmc5883.cpp
+87 -73
View File
@@ -849,42 +849,24 @@ HMC5883::collect()
/* scale values for output */
/*
* 1) Scale raw value to SI units using scaling from datasheet.
* 2) Subtract static offset (in SI units)
* 3) Scale the statically calibrated values with a linear
* dynamically obtained factor
*
* Note: the static sensor offset is the number the sensor outputs
* at a nominally 'zero' input. Therefore the offset has to
* be subtracted.
*
* Example: A gyro outputs a value of 74 at zero angular rate
* the offset is 74 from the origin and subtracting
* 74 from all measurements centers them around zero.
*/
#ifdef PX4_I2C_BUS_ONBOARD
if (_bus == PX4_I2C_BUS_ONBOARD) {
/* to align the sensor axes with the board, x and y need to be flipped */
new_report.x = ((report.y * _range_scale) - _scale.x_offset) * _scale.x_scale;
/* flip axes and negate value for y */
new_report.y = ((-report.x * _range_scale) - _scale.y_offset) * _scale.y_scale;
/* z remains z */
new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale;
} else {
#endif
/* the standard external mag by 3DR has x pointing to the right, y pointing backwards, and z down,
* therefore switch x and y and invert y */
new_report.x = ((-report.y * _range_scale) - _scale.x_offset) * _scale.x_scale;
/* flip axes and negate value for y */
new_report.y = ((report.x * _range_scale) - _scale.y_offset) * _scale.y_scale;
/* z remains z */
new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale;
#ifdef PX4_I2C_BUS_ONBOARD
}
// convert onboard so it matches offboard for the
// scaling below
report.y = -report.y;
report.x = -report.x;
}
#endif
/* the standard external mag by 3DR has x pointing to the
* right, y pointing backwards, and z down, therefore switch x
* and y and invert y */
new_report.x = ((-report.y * _range_scale) - _scale.x_offset) * _scale.x_scale;
/* flip axes and negate value for y */
new_report.y = ((report.x * _range_scale) - _scale.y_offset) * _scale.y_scale;
/* z remains z */
new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale;
if (_mag_topic != -1) {
/* publish it */
orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report);
@@ -910,6 +892,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
struct mag_report report;
ssize_t sz;
int ret = 1;
uint8_t good_count = 0;
// XXX do something smarter here
int fd = (int)enable;
@@ -932,32 +915,17 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
1.0f,
};
float avg_excited[3] = {0.0f, 0.0f, 0.0f};
unsigned i;
float sum_excited[3] = {0.0f, 0.0f, 0.0f};
/* expected axis scaling. The datasheet says that 766 will
* be places in the X and Y axes and 713 in the Z
* axis. Experiments show that in fact 766 is placed in X,
* and 713 in Y and Z. This is relative to a base of 660
* LSM/Ga, giving 1.16 and 1.08 */
float expected_cal[3] = { 1.16f, 1.08f, 1.08f };
warnx("starting mag scale calibration");
/* do a simple demand read */
sz = read(filp, (char *)&report, sizeof(report));
if (sz != sizeof(report)) {
warn("immediate read failed");
ret = 1;
goto out;
}
warnx("current measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z);
warnx("time: %lld", report.timestamp);
warnx("sampling 500 samples for scaling offset");
/* set the queue depth to 10 */
/* don't do this for now, it can lead to a crash in start() respectively work_queue() */
// if (OK != ioctl(filp, SENSORIOCSQUEUEDEPTH, 10)) {
// warn("failed to set queue depth");
// ret = 1;
// goto out;
// }
/* start the sensor polling at 50 Hz */
if (OK != ioctl(filp, SENSORIOCSPOLLRATE, 50)) {
warn("failed to set 2Hz poll rate");
@@ -965,8 +933,9 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
goto out;
}
/* Set to 2.5 Gauss */
if (OK != ioctl(filp, MAGIOCSRANGE, 2)) {
/* Set to 2.5 Gauss. We ask for 3 to get the right part of
* the chained if statement above. */
if (OK != ioctl(filp, MAGIOCSRANGE, 3)) {
warnx("failed to set 2.5 Ga range");
ret = 1;
goto out;
@@ -990,8 +959,8 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
goto out;
}
/* read the sensor 10x and report each value */
for (i = 0; i < 500; i++) {
// discard 10 samples to let the sensor settle
for (uint8_t i = 0; i < 10; i++) {
struct pollfd fds;
/* wait for data to be ready */
@@ -1009,32 +978,69 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
if (sz != sizeof(report)) {
warn("periodic read failed");
ret = -EIO;
goto out;
}
}
} else {
avg_excited[0] += report.x;
avg_excited[1] += report.y;
avg_excited[2] += report.z;
/* read the sensor up to 50x, stopping when we have 10 good values */
for (uint8_t i = 0; i < 50 && good_count < 10; i++) {
struct pollfd fds;
/* wait for data to be ready */
fds.fd = fd;
fds.events = POLLIN;
ret = ::poll(&fds, 1, 2000);
if (ret != 1) {
warn("timed out waiting for sensor data");
goto out;
}
/* now go get it */
sz = ::read(fd, &report, sizeof(report));
if (sz != sizeof(report)) {
warn("periodic read failed");
ret = -EIO;
goto out;
}
float cal[3] = {fabsf(expected_cal[0] / report.x),
fabsf(expected_cal[1] / report.y),
fabsf(expected_cal[2] / report.z)};
if (cal[0] > 0.7f && cal[0] < 1.35f &&
cal[1] > 0.7f && cal[1] < 1.35f &&
cal[2] > 0.7f && cal[2] < 1.35f) {
good_count++;
sum_excited[0] += cal[0];
sum_excited[1] += cal[1];
sum_excited[2] += cal[2];
}
//warnx("periodic read %u", i);
//warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z);
//warnx("cal: %.6f %.6f %.6f", (double)cal[0], (double)cal[1], (double)cal[2]);
}
avg_excited[0] /= i;
avg_excited[1] /= i;
avg_excited[2] /= i;
if (good_count < 5) {
warn("failed calibration");
ret = -EIO;
goto out;
}
warnx("done. Performed %u reads", i);
warnx("measurement avg: %.6f %.6f %.6f", (double)avg_excited[0], (double)avg_excited[1], (double)avg_excited[2]);
#if 0
warnx("measurement avg: %.6f %.6f %.6f",
(double)sum_excited[0]/good_count,
(double)sum_excited[1]/good_count,
(double)sum_excited[2]/good_count);
#endif
float scaling[3];
/* calculate axis scaling */
scaling[0] = fabsf(1.16f / avg_excited[0]);
/* second axis inverted */
scaling[1] = fabsf(1.16f / -avg_excited[1]);
scaling[2] = fabsf(1.08f / avg_excited[2]);
scaling[0] = sum_excited[0] / good_count;
scaling[1] = sum_excited[1] / good_count;
scaling[2] = sum_excited[2] / good_count;
warnx("axes scaling: %.6f %.6f %.6f", (double)scaling[0], (double)scaling[1], (double)scaling[2]);
@@ -1165,6 +1171,8 @@ int HMC5883::set_excitement(unsigned enable)
conf_reg &= ~0x03;
}
// ::printf("set_excitement enable=%d regA=0x%x\n", (int)enable, (unsigned)conf_reg);
ret = write_reg(ADDR_CONF_A, conf_reg);
if (OK != ret)
@@ -1173,6 +1181,8 @@ int HMC5883::set_excitement(unsigned enable)
uint8_t conf_reg_ret;
read_reg(ADDR_CONF_A, conf_reg_ret);
//print_info();
return !(conf_reg == conf_reg_ret);
}
@@ -1211,6 +1221,10 @@ HMC5883::print_info()
perf_print_counter(_comms_errors);
perf_print_counter(_buffer_overflows);
printf("poll interval: %u ticks\n", _measure_ticks);
printf("offsets (%.2f %.2f %.2f)\n", (double)_scale.x_offset, (double)_scale.y_offset, (double)_scale.z_offset);
printf("scaling (%.2f %.2f %.2f) 1/range_scale %.2f range_ga %.2f\n",
(double)_scale.x_scale, (double)_scale.y_scale, (double)_scale.z_scale,
(double)1.0/_range_scale, (double)_range_ga);
_reports->print_info("report queue");
}
src/drivers/px4fmu/fmu.cpp
+34 -1
View File
@@ -1006,6 +1006,40 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
break;
case PWM_SERVO_SET_COUNT: {
/* change the number of outputs that are enabled for
* PWM. This is used to change the split between GPIO
* and PWM under control of the flight config
* parameters. Note that this does not allow for
* changing a set of pins to be used for serial on
* FMUv1
*/
switch (arg) {
case 0:
set_mode(MODE_NONE);
break;
case 2:
set_mode(MODE_2PWM);
break;
case 4:
set_mode(MODE_4PWM);
break;
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
case 6:
set_mode(MODE_6PWM);
break;
#endif
default:
ret = -EINVAL;
break;
}
break;
}
case MIXERIOCRESET:
if (_mixers != nullptr) {
delete _mixers;
@@ -1443,7 +1477,6 @@ void
sensor_reset(int ms)
{
int fd;
int ret;
fd = open(PX4FMU_DEVICE_PATH, O_RDWR);
src/drivers/px4io/px4io.cpp
+34 -15
View File
@@ -1454,8 +1454,10 @@ PX4IO::io_publish_raw_rc()
/* set RSSI */
// XXX the correct scaling needs to be validated here
rc_val.rssi = (_servorail_status.rssi_v / 3.3f) * UINT8_MAX;
if (rc_val.input_source != RC_INPUT_SOURCE_PX4IO_SBUS) {
// XXX the correct scaling needs to be validated here
rc_val.rssi = (_servorail_status.rssi_v / 3.3f) * UINT8_MAX;
}
/* lazily advertise on first publication */
if (_to_input_rc == 0) {
@@ -1808,7 +1810,7 @@ PX4IO::print_status()
printf("\n");
if (raw_inputs > 0) {
if ((flags & PX4IO_P_STATUS_FLAGS_RC_PPM)) {
int frame_len = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_RC_DATA);
printf("RC data (PPM frame len) %u us\n", frame_len);
@@ -2365,8 +2367,10 @@ start(int argc, char *argv[])
/* create the driver - it will set g_dev */
(void)new PX4IO(interface);
if (g_dev == nullptr)
if (g_dev == nullptr) {
delete interface;
errx(1, "driver alloc failed");
}
if (OK != g_dev->init()) {
delete g_dev;
@@ -2642,17 +2646,17 @@ monitor(void)
read(0, &c, 1);
if (cancels-- == 0) {
printf("\033[H"); /* move cursor home and clear screen */
printf("\033[2J\033[H"); /* move cursor home and clear screen */
exit(0);
}
}
if (g_dev != nullptr) {
printf("\033[H"); /* move cursor home and clear screen */
printf("\033[2J\033[H"); /* move cursor home and clear screen */
(void)g_dev->print_status();
(void)g_dev->print_debug();
printf("[ Use 'px4io debug <N>' for more output. Hit <enter> three times to exit monitor mode ]\n");
printf("\n\n\n[ Use 'px4io debug <N>' for more output. Hit <enter> three times to exit monitor mode ]\n");
} else {
errx(1, "driver not loaded, exiting");
@@ -2767,18 +2771,33 @@ px4io_main(int argc, char *argv[])
}
if (g_dev == nullptr) {
warnx("px4io is not started, still attempting upgrade");
} else {
uint16_t arg = atol(argv[2]);
int ret = g_dev->ioctl(nullptr, PX4IO_REBOOT_BOOTLOADER, arg);
if (ret != OK) {
printf("reboot failed - %d\n", ret);
exit(1);
/* allocate the interface */
device::Device *interface = get_interface();
/* create the driver - it will set g_dev */
(void)new PX4IO(interface);
if (g_dev == nullptr) {
delete interface;
errx(1, "driver alloc failed");
}
// tear down the px4io instance
delete g_dev;
if (OK != g_dev->init()) {
warnx("driver init failed, still trying..");
}
}
uint16_t arg = atol(argv[2]);
int ret = g_dev->ioctl(nullptr, PX4IO_REBOOT_BOOTLOADER, arg);
if (ret != OK) {
printf("reboot failed - %d\n", ret);
exit(1);
}
// tear down the px4io instance
delete g_dev;
// upload the specified firmware
const char *fn[2];
fn[0] = argv[3];