dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge branch 'params' of github.com:PX4/Firmware

This commit is contained in:
Lorenz Meier 2012-08-28 21:44:38 +02:00
parent 30b670a6ee fa32184973
commit 5b81a51a82
3 changed files with 81 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

63
apps/commander/commander.c
View File

@ -271,25 +271,25 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
/* Get rid of 10% */
const int outlier_margin = (peak_samples) / 10;
int16_t *mag_maxima[3];
mag_maxima[0] = (int16_t*)malloc(peak_samples * sizeof(uint16_t));
mag_maxima[1] = (int16_t*)malloc(peak_samples * sizeof(uint16_t));
mag_maxima[2] = (int16_t*)malloc(peak_samples * sizeof(uint16_t));
float *mag_maxima[3];
mag_maxima[0] = (float*)malloc(peak_samples * sizeof(float));
mag_maxima[1] = (float*)malloc(peak_samples * sizeof(float));
mag_maxima[2] = (float*)malloc(peak_samples * sizeof(float));
int16_t *mag_minima[3];
mag_minima[0] = (int16_t*)malloc(peak_samples * sizeof(uint16_t));
mag_minima[1] = (int16_t*)malloc(peak_samples * sizeof(uint16_t));
mag_minima[2] = (int16_t*)malloc(peak_samples * sizeof(uint16_t));
float *mag_minima[3];
mag_minima[0] = (float*)malloc(peak_samples * sizeof(float));
mag_minima[1] = (float*)malloc(peak_samples * sizeof(float));
mag_minima[2] = (float*)malloc(peak_samples * sizeof(float));
/* initialize data table */
for (int i = 0; i < peak_samples; i++) {
mag_maxima[0][i] = INT16_MIN;
mag_maxima[1][i] = INT16_MIN;
mag_maxima[2][i] = INT16_MIN;
mag_maxima[0][i] = FLT_MIN;
mag_maxima[1][i] = FLT_MIN;
mag_maxima[2][i] = FLT_MIN;
mag_minima[0][i] = INT16_MAX;
mag_minima[1][i] = INT16_MAX;
mag_minima[2][i] = INT16_MAX;
mag_minima[0][i] = FLT_MAX;
mag_minima[1][i] = FLT_MAX;
mag_minima[2][i] = FLT_MAX;
}
mavlink_log_info(mavlink_fd, "[commander] Please rotate in all directions.");
@ -308,23 +308,23 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
for (int i = 0; i < peak_samples; i++) {
/* x minimum */
if (raw.magnetometer_raw[0] < mag_minima[0][i])
mag_minima[0][i] = raw.magnetometer_raw[0];
mag_minima[0][i] = raw.magnetometer_ga[0];
/* y minimum */
if (raw.magnetometer_raw[1] < mag_minima[1][i])
mag_minima[1][i] = raw.magnetometer_raw[1];
mag_minima[1][i] = raw.magnetometer_ga[1];
/* z minimum */
if (raw.magnetometer_raw[2] < mag_minima[2][i])
mag_minima[2][i] = raw.magnetometer_raw[2];
mag_minima[2][i] = raw.magnetometer_ga[2];
/* x maximum */
if (raw.magnetometer_raw[0] > mag_maxima[0][i])
mag_maxima[0][i] = raw.magnetometer_raw[0];
mag_maxima[0][i] = raw.magnetometer_ga[0];
/* y maximum */
if (raw.magnetometer_raw[1] > mag_maxima[1][i])
mag_maxima[1][i] = raw.magnetometer_raw[1];
mag_maxima[1][i] = raw.magnetometer_ga[1];
/* z maximum */
if (raw.magnetometer_raw[2] > mag_maxima[2][i])
mag_maxima[2][i] = raw.magnetometer_raw[2];
mag_maxima[2][i] = raw.magnetometer_ga[2];
}
calibration_counter++;
@ -354,7 +354,7 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
printf("start:\n");
for (int i = 0; i < 10; i++) {
printf("mag min: %d\t%d\t%d\tmag max: %d\t%d\t%d\n",
printf("mag min: %8.4f\t%8.4f\t%8.4f\tmax: %8.4f\t%8.4f\t%8.4f\n",
mag_minima[0][i],
mag_minima[1][i],
mag_minima[2][i],
@ -366,7 +366,7 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
printf("-----\n");
for (int i = (peak_samples - outlier_margin)-10; i < (peak_samples - outlier_margin); i++) {
printf("mag min: %d\t%d\t%d\tmag max: %d\t%d\t%d\n",
printf("mag min: %8.4f\t%8.4f\t%8.4f\tmax: %8.4f\t%8.4f\t%8.4f\n",
mag_minima[0][i],
mag_minima[1][i],
mag_minima[2][i],
@ -397,7 +397,8 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
max_avg[1] /= (peak_samples - outlier_margin);
max_avg[2] /= (peak_samples - outlier_margin);
printf("\nFINAL:\nmag min: %d\t%d\t%d\nmag max: %d\t%d\t%d\n", (int)min_avg[0], (int)min_avg[1], (int)min_avg[2], (int)max_avg[0], (int)max_avg[1], (int)max_avg[2]);
printf("\nFINAL:\nmag min: %8.4f\t%8.4f\t%8.4f\nmax: %8.4f\t%8.4f\t%8.4f\n", (double)min_avg[0],
(double)min_avg[1], (double)min_avg[2], (double)max_avg[0], (double)max_avg[1], (double)max_avg[2]);
float mag_offset[3];
@ -465,9 +466,9 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status)
if (poll(fds, 1, 1000)) {
orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw);
gyro_offset[0] += raw.gyro_raw[0];
gyro_offset[1] += raw.gyro_raw[1];
gyro_offset[2] += raw.gyro_raw[2];
gyro_offset[0] += raw.gyro_rad_s[0];
gyro_offset[1] += raw.gyro_rad_s[1];
gyro_offset[2] += raw.gyro_rad_s[2];
calibration_counter++;
} else {
/* any poll failure for 1s is a reason to abort */
@ -501,8 +502,6 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status)
mavlink_log_info(mavlink_fd, offset_output);
close(sub_sensor_combined);
// XXX Add a parameter changed broadcast notification
}
@ -747,12 +746,12 @@ float battery_remaining_estimate_voltage(int cells, int chemistry, float voltage
initialized = true;
}
float chemistry_voltage_empty[1];
float chemistry_voltage_full[1];
float chemistry_voltage_empty[1] = { 3.2f };
float chemistry_voltage_full[1] = { 4.05f };
if (counter % 100 == 0) {
param_get(bat_volt_empty, chemistry_voltage_empty+0);
param_get(bat_volt_full, chemistry_voltage_full+0);
param_get(bat_volt_empty, &(chemistry_voltage_empty[0]));
param_get(bat_volt_full, &(chemistry_voltage_full[0]));
}
counter++;

18
apps/drivers/mpu6000/mpu6000.cpp
View File

@ -589,9 +589,14 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
return -EINVAL;
case ACCELIOCSSCALE:
/* copy scale in */
memcpy(&_accel_scale, (struct accel_scale*) arg, sizeof(_accel_scale));
return OK;
case ACCELIOCGSCALE:
/* XXX not implemented */
return -EINVAL;
/* copy scale out */
memcpy((struct accel_scale*) arg, &_accel_scale, sizeof(_accel_scale));
return OK;
case ACCELIOCSRANGE:
case ACCELIOCGRANGE:
@ -631,9 +636,14 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
return -EINVAL;
case GYROIOCSSCALE:
/* copy scale in */
memcpy(&_gyro_scale, (struct gyro_scale*) arg, sizeof(_gyro_scale));
return OK;
case GYROIOCGSCALE:
/* XXX not implemented */
return -EINVAL;
/* copy scale out */
memcpy((struct gyro_scale*) arg, &_gyro_scale, sizeof(_gyro_scale));
return OK;
case GYROIOCSRANGE:
case GYROIOCGRANGE:

47
apps/sensors/sensors.cpp
View File

@ -428,33 +428,53 @@ Sensors::parameters_update()
/* min values */
for (unsigned int i = 0; i < nchans; i++) {
param_get(_parameter_handles.min[i], &(_parameters.min[i]));
if (param_get(_parameter_handles.min[i], &(_parameters.min[i])) != OK) {
warnx("Failed getting min for chan %d", i);
}
}
/* trim values */
for (unsigned int i = 0; i < nchans; i++) {
param_get(_parameter_handles.trim[i], &(_parameters.trim[i]));
if (param_get(_parameter_handles.trim[i], &(_parameters.trim[i])) != OK) {
warnx("Failed getting trim for chan %d", i);
}
}
/* max values */
for (unsigned int i = 0; i < nchans; i++) {
param_get(_parameter_handles.max[i], &(_parameters.max[i]));
if (param_get(_parameter_handles.max[i], &(_parameters.max[i])) != OK) {
warnx("Failed getting max for chan %d", i);
}
}
/* channel reverse */
for (unsigned int i = 0; i < nchans; i++) {
param_get(_parameter_handles.rev[i], &(_parameters.rev[i]));
if (param_get(_parameter_handles.rev[i], &(_parameters.rev[i])) != OK) {
warnx("Failed getting rev for chan %d", i);
}
}
/* remote control type */
param_get(_parameter_handles.rc_type, &(_parameters.rc_type));
if (param_get(_parameter_handles.rc_type, &(_parameters.rc_type)) != OK) {
warnx("Failed getting remote control type");
}
/* channel mapping */
param_get(_parameter_handles.rc_map_roll, &(_parameters.rc_map_roll));
param_get(_parameter_handles.rc_map_pitch, &(_parameters.rc_map_pitch));
param_get(_parameter_handles.rc_map_yaw, &(_parameters.rc_map_yaw));
param_get(_parameter_handles.rc_map_throttle, &(_parameters.rc_map_throttle));
param_get(_parameter_handles.rc_map_mode_sw, &(_parameters.rc_map_mode_sw));
if (param_get(_parameter_handles.rc_map_roll, &(_parameters.rc_map_roll)) != OK) {
warnx("Failed getting roll chan index");
}
if (param_get(_parameter_handles.rc_map_pitch, &(_parameters.rc_map_pitch)) != OK) {
warnx("Failed getting pitch chan index");
}
if (param_get(_parameter_handles.rc_map_yaw, &(_parameters.rc_map_yaw)) != OK) {
warnx("Failed getting yaw chan index");
}
if (param_get(_parameter_handles.rc_map_throttle, &(_parameters.rc_map_throttle)) != OK) {
warnx("Failed getting throttle chan index");
}
if (param_get(_parameter_handles.rc_map_mode_sw, &(_parameters.rc_map_mode_sw)) != OK) {
warnx("Failed getting mode sw chan index");
}
/* gyro offsets */
param_get(_parameter_handles.gyro_offset[0], &(_parameters.gyro_offset[0]));
@ -820,6 +840,11 @@ Sensors::parameter_update_poll(bool forced)
_rc.function[2] = _parameters.rc_map_pitch - 1;
_rc.function[3] = _parameters.rc_map_yaw - 1;
_rc.function[4] = _parameters.rc_map_mode_sw - 1;
printf("RAW S: %8.4f MID: %d R: %d P: %d\n", _rc.chan[0].scaling_factor, (int)_rc.chan[0].mid, (int)_rc.function[0], (int)_rc.function[1]);
printf("RAW MAN: %8.4f %8.4f\n", _rc.chan[0].scaled, _rc.chan[1].scaled);
fflush(stdout);
usleep(5000);
}
}
@ -870,7 +895,7 @@ Sensors::ppm_poll()
if (_ppm_last_valid == ppm_last_valid_decode)
return;
/* require at least two chanels to consider the signal valid */
if (ppm_decoded_channels < 2)
if (ppm_decoded_channels < 4)
return;
/* we are accepting this decode */