|
|
|
@ -1,6 +1,6 @@
@@ -1,6 +1,6 @@
|
|
|
|
|
/****************************************************************************
|
|
|
|
|
* |
|
|
|
|
* Copyright (C) 2013 PX4 Development Team. All rights reserved. |
|
|
|
|
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. |
|
|
|
|
* |
|
|
|
|
* Redistribution and use in source and binary forms, with or without |
|
|
|
|
* modification, are permitted provided that the following conditions |
|
|
|
@ -64,9 +64,9 @@ int do_airspeed_calibration(int mavlink_fd)
@@ -64,9 +64,9 @@ int do_airspeed_calibration(int mavlink_fd)
|
|
|
|
|
{ |
|
|
|
|
/* give directions */ |
|
|
|
|
mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); |
|
|
|
|
mavlink_log_info(mavlink_fd, "don't move system"); |
|
|
|
|
mavlink_log_info(mavlink_fd, "ensure airspeed sensor is not registering wind"); |
|
|
|
|
|
|
|
|
|
const int calibration_count = 2500; |
|
|
|
|
const int calibration_count = 2000; |
|
|
|
|
|
|
|
|
|
int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); |
|
|
|
|
struct differential_pressure_s diff_pres; |
|
|
|
@ -85,13 +85,15 @@ int do_airspeed_calibration(int mavlink_fd)
@@ -85,13 +85,15 @@ int do_airspeed_calibration(int mavlink_fd)
|
|
|
|
|
if (fd > 0) { |
|
|
|
|
if (OK == ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) { |
|
|
|
|
paramreset_successful = true; |
|
|
|
|
} else { |
|
|
|
|
mavlink_log_critical(mavlink_fd, "airspeed offset zero failed"); |
|
|
|
|
} |
|
|
|
|
close(fd); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
if (!paramreset_successful) { |
|
|
|
|
warn("WARNING: failed to set scale / offsets for airspeed sensor"); |
|
|
|
|
mavlink_log_critical(mavlink_fd, "could not reset dpress sensor"); |
|
|
|
|
warn("FAILED to set scale / offsets for airspeed"); |
|
|
|
|
mavlink_log_critical(mavlink_fd, "dpress reset failed"); |
|
|
|
|
mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); |
|
|
|
|
return ERROR; |
|
|
|
|
} |
|
|
|
@ -107,7 +109,7 @@ int do_airspeed_calibration(int mavlink_fd)
@@ -107,7 +109,7 @@ int do_airspeed_calibration(int mavlink_fd)
|
|
|
|
|
|
|
|
|
|
if (poll_ret) { |
|
|
|
|
orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); |
|
|
|
|
diff_pres_offset += diff_pres.differential_pressure_pa; |
|
|
|
|
diff_pres_offset += diff_pres.differential_pressure_raw_pa; |
|
|
|
|
calibration_counter++; |
|
|
|
|
|
|
|
|
|
if (calibration_counter % (calibration_count / 20) == 0) |
|
|
|
|