fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-07 03:30:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merged beta into master

Browse Source
This commit is contained in:
Lorenz Meier
2015-08-01 16:58:02 +02:00
parent 848dd2155a 09f6b88651
commit ea7ae7d019
87 changed files with 1386 additions and 545 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/accelerometer_calibration.cpp
+10 -2
View File
@@ -561,6 +561,7 @@ int do_level_calibration(int mavlink_fd) {
const unsigned cal_time = 5;
const unsigned cal_hz = 100;
const unsigned settle_time = 30;
bool success = false;
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att));
@@ -599,7 +600,15 @@ int do_level_calibration(int mavlink_fd) {
start = hrt_absolute_time();
// average attitude for 5 seconds
while(hrt_elapsed_time(&start) < cal_time * 1000000) {
px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
int pollret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
if (pollret <= 0) {
// attitude estimator is not running
mavlink_and_console_log_critical(mavlink_fd, "attitude estimator not running - check system boot");
mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "level");
goto out;
}
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
roll_mean += att.roll;
pitch_mean += att.pitch;
@@ -608,7 +617,6 @@ int do_level_calibration(int mavlink_fd) {
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 100);
bool success = false;
if (counter > (cal_time * cal_hz / 2 )) {
roll_mean /= counter;
pitch_mean /= counter;
src/modules/commander/airspeed_calibration.cpp
+6 -6
View File
@@ -75,12 +75,12 @@ int do_airspeed_calibration(int mavlink_fd)
{
int result = OK;
unsigned calibration_counter = 0;
const unsigned maxcount = 3000;
const unsigned maxcount = 2400;
/* give directions */
mavlink_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name);
const unsigned calibration_count = 2000;
const unsigned calibration_count = (maxcount * 2) / 3;
int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
struct differential_pressure_s diff_pres;
@@ -204,10 +204,10 @@ int do_airspeed_calibration(int mavlink_fd)
/* just take a few samples and make sure pitot tubes are not reversed, timeout after ~30 seconds */
while (calibration_counter < maxcount) {
if (calibrate_cancel_check(mavlink_fd, cancel_sub)) {
goto error_return;
}
if (calibrate_cancel_check(mavlink_fd, cancel_sub)) {
goto error_return;
}
/* wait blocking for new data */
px4_pollfd_struct_t fds[1];
fds[0].fd = diff_pres_sub;
src/modules/commander/calibration_routines.cpp
+2 -2
View File
@@ -470,8 +470,8 @@ calibrate_return calibrate_from_orientation(int mavlink_fd,
/* inform user about already handled side */
if (side_data_collected[orient]) {
orientation_failures++;
mavlink_and_console_log_info(mavlink_fd, "[cal] %s side completed or not needed", detect_orientation_str(orient));
mavlink_and_console_log_info(mavlink_fd, "[cal] rotate to a pending side");
mavlink_and_console_log_critical(mavlink_fd, "%s side already completed", detect_orientation_str(orient));
mavlink_and_console_log_critical(mavlink_fd, "rotate to a pending side");
continue;
}
src/modules/commander/commander.cpp
+13
View File
@@ -2151,6 +2151,19 @@ int commander_thread_main(int argc, char *argv[])
orb_publish(ORB_ID(vehicle_status), status_pub, &status);
armed.timestamp = now;
/* set prearmed state if safety is off, or safety is not present and 5 seconds passed */
if (safety.safety_switch_available) {
/* safety is off, go into prearmed */
armed.prearmed = safety.safety_off;
} else {
/* safety is not present, go into prearmed
* (all output drivers should be started / unlocked last in the boot process
* when the rest of the system is fully initialized)
*/
armed.prearmed = (hrt_elapsed_time(&commander_boot_timestamp) > 5 * 1000 * 1000);
}
orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
}
src/modules/commander/mag_calibration.cpp
+22 -8
View File
@@ -67,9 +67,11 @@
static const int ERROR = -1;
static const char *sensor_name = "mag";
static const unsigned max_mags = 3;
static constexpr unsigned max_mags = 3;
static constexpr float mag_sphere_radius = 0.2f;
static const unsigned int calibration_sides = 6;
static constexpr unsigned int calibration_sides = 6; ///< The total number of sides
static constexpr unsigned int calibration_total_points = 240; ///< The total points per magnetometer
static constexpr unsigned int calibraton_duration_seconds = 42; ///< The total duration the routine is allowed to take
calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]);
@@ -207,6 +209,10 @@ static bool reject_sample(float sx, float sy, float sz, float x[], float y[], fl
return false;
}
static unsigned progress_percentage(mag_worker_data_t* worker_data) {
return 100 * ((float)worker_data->done_count) / calibration_sides;
}
static calibrate_return mag_calibration_worker(detect_orientation_return orientation, int cancel_sub, void* data)
{
calibrate_return result = calibrate_return_ok;
@@ -226,7 +232,7 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
* for a good result, so we're not constraining the user more than we have to.
*/
hrt_abstime detection_deadline = hrt_absolute_time() + worker_data->calibration_interval_perside_useconds * 2;
hrt_abstime detection_deadline = hrt_absolute_time() + worker_data->calibration_interval_perside_useconds * 5;
hrt_abstime last_gyro = 0;
float gyro_x_integral = 0.0f;
float gyro_y_integral = 0.0f;
@@ -347,8 +353,8 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
// Progress indicator for side
mavlink_and_console_log_info(worker_data->mavlink_fd,
"[cal] %s side calibration: progress <%u>",
detect_orientation_str(orientation),
(unsigned)(100 * ((float)calibration_counter_side / (float)worker_data->calibration_points_perside)));
detect_orientation_str(orientation), progress_percentage(worker_data) +
(unsigned)((100 / calibration_sides) * ((float)calibration_counter_side / (float)worker_data->calibration_points_perside)));
}
} else {
poll_errcount++;
@@ -365,7 +371,7 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] %s side done, rotate to a different side", detect_orientation_str(orientation));
worker_data->done_count++;
mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_QGC_PROGRESS_MSG, 34 * worker_data->done_count);
mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_QGC_PROGRESS_MSG, progress_percentage(worker_data));
}
return result;
@@ -379,8 +385,8 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
worker_data.mavlink_fd = mavlink_fd;
worker_data.done_count = 0;
worker_data.calibration_points_perside = 40;
worker_data.calibration_interval_perside_seconds = 20;
worker_data.calibration_points_perside = calibration_total_points / calibration_sides;
worker_data.calibration_interval_perside_seconds = calibraton_duration_seconds / calibration_sides;
worker_data.calibration_interval_perside_useconds = worker_data.calibration_interval_perside_seconds * 1000 * 1000;
// Collect: Right-side up, Left Side, Nose down
@@ -499,6 +505,10 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
printf("RAW DATA:\n--------------------\n");
for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) {
if (worker_data.calibration_counter_total[cur_mag] == 0) {
continue;
}
printf("RAW: MAG %u with %u samples:\n", (unsigned)cur_mag, (unsigned)worker_data.calibration_counter_total[cur_mag]);
for (size_t i = 0; i < worker_data.calibration_counter_total[cur_mag]; i++) {
@@ -514,6 +524,10 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
printf("CALIBRATED DATA:\n--------------------\n");
for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) {
if (worker_data.calibration_counter_total[cur_mag] == 0) {
continue;
}
printf("Calibrated: MAG %u with %u samples:\n", (unsigned)cur_mag, (unsigned)worker_data.calibration_counter_total[cur_mag]);
for (size_t i = 0; i < worker_data.calibration_counter_total[cur_mag]; i++) {