Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-28 19:48:31 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: replace SKIP_GYRO_CAL with INS_GYR_CAL

Browse Source 
Also calibrate gyros with accel cal and set trim
This commit is contained in:
Randy Mackay 2015-09-17 21:32:06 +09:00
parent 57c5840f0d
commit 6da53ae3b2
5 changed files with 15 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
ArduPlane/GCS_Mavlink.cpp
View File

@ -237,7 +237,7 @@ void Plane::send_extended_status1(mavlink_channel_t chan)
control_sensors_health |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW;
}
#endif
if (!ins.get_gyro_health_all() || (!g.skip_gyro_cal && !ins.gyro_calibrated_ok_all())) {
if (!ins.get_gyro_health_all() || !ins.gyro_calibrated_ok_all()) {
control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_3D_GYRO;
}
if (!ins.get_accel_health_all()) {
@ -1219,10 +1219,11 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
} else if (is_equal(packet.param5,1.0f)) {
float trim_roll, trim_pitch;
AP_InertialSensor_UserInteract_MAVLink interact(this);
if (plane.g.skip_gyro_cal) {
// start with gyro calibration, otherwise if the user
// has SKIP_GYRO_CAL=1 they don't get to do it
plane.ins.init_gyro();
// start with gyro calibration
plane.ins.init_gyro();
// reset ahrs gyro bias
if (plane.ins.gyro_calibrated_ok_all()) {
plane.ahrs.reset_gyro_drift();
}
if(plane.ins.calibrate_accel(&interact, trim_roll, trim_pitch)) {
// reset ahrs's trim to suggested values from calibration routine
@ -1232,6 +1233,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
result = MAV_RESULT_FAILED;
}
} else if (is_equal(packet.param5,2.0f)) {
// start with gyro calibration
plane.ins.init_gyro();
// accel trim
float trim_roll, trim_pitch;
if(plane.ins.calibrate_trim(trim_roll, trim_pitch)) {

7
ArduPlane/Parameters.cpp
View File

@ -128,13 +128,6 @@ const AP_Param::Info Plane::var_info[] PROGMEM = {
// @User: Advanced
GSCALAR(stick_mixing, "STICK_MIXING", STICK_MIXING_FBW),
// @Param: SKIP_GYRO_CAL
// @DisplayName: Skip gyro calibration
// @Description: When enabled this tells the APM to skip the normal gyroscope calibration at startup, and instead use the saved gyro calibration from the last flight. You should only enable this if you are careful to check that your aircraft has good attitude control before flying, as some boards may have significantly different gyro calibration between boots, especially if the temperature changes a lot. If gyro calibration is skipped then APM relies on using the gyro drift detection code to get the right gyro calibration in the few minutes after it boots. This option is mostly useful where the requirement to hold the plane still while it is booting is a significant problem.
// @Values: 0:Disabled,1:Enabled
// @User: Advanced
GSCALAR(skip_gyro_cal, "SKIP_GYRO_CAL", 0),
// @Param: AUTO_FBW_STEER
// @DisplayName: Use FBWA steering in AUTO
// @Description: When enabled this option gives FBWA navigation and steering in AUTO mode. This can be used to allow manual stabilised piloting with waypoint logic for triggering payloads. With this enabled the pilot has the same control over the plane as in FBWA mode, and the normal AUTO navigation is completely disabled. This option is not recommended for normal use.

4
ArduPlane/Parameters.h
View File

@ -91,7 +91,7 @@ public:
k_param_scheduler,
k_param_relay,
k_param_takeoff_throttle_delay,
k_param_skip_gyro_cal,
k_param_skip_gyro_cal, // unused
k_param_auto_fbw_steer,
k_param_waypoint_max_radius,
k_param_ground_steer_alt,
@ -363,8 +363,6 @@ public:
// attitude controller type.
AP_Int8 att_controller;
// skip gyro calibration
AP_Int8 skip_gyro_cal;
AP_Int8 auto_fbw_steer;
// Estimation

16
ArduPlane/system.cpp
View File

@ -276,7 +276,7 @@ void Plane::startup_ground(void)
// Makes the servos wiggle
// step 1 = 1 wiggle
// -----------------------
if (!g.skip_gyro_cal) {
if (ins.gyro_calibration_timing() != AP_InertialSensor::GYRO_CAL_NEVER) {
demo_servos(1);
}
@ -300,7 +300,7 @@ void Plane::startup_ground(void)
// Makes the servos wiggle - 3 times signals ready to fly
// -----------------------
if (!g.skip_gyro_cal) {
if (ins.gyro_calibration_timing() != AP_InertialSensor::GYRO_CAL_NEVER) {
demo_servos(3);
}
@ -566,15 +566,7 @@ void Plane::startup_INS_ground(void)
}
#endif
AP_InertialSensor::Start_style style;
if (g.skip_gyro_cal) {
style = AP_InertialSensor::WARM_START;
arming.set_skip_gyro_cal(true);
} else {
style = AP_InertialSensor::COLD_START;
}
if (style == AP_InertialSensor::COLD_START) {
if (ins.gyro_calibration_timing() != AP_InertialSensor::GYRO_CAL_NEVER) {
gcs_send_text_P(MAV_SEVERITY_ALERT, PSTR("Beginning INS calibration; do not move plane"));
hal.scheduler->delay(100);
}
@ -584,7 +576,7 @@ void Plane::startup_INS_ground(void)
ahrs.set_vehicle_class(AHRS_VEHICLE_FIXED_WING);
ahrs.set_wind_estimation(true);
ins.init(style, ins_sample_rate);
ins.init(ins_sample_rate);
ahrs.reset();
// read Baro pressure at ground

6
ArduPlane/test.cpp
View File

@ -373,8 +373,7 @@ int8_t Plane::test_ins(uint8_t argc, const Menu::arg *argv)
ahrs.set_fly_forward(true);
ahrs.set_wind_estimation(true);
ins.init(AP_InertialSensor::COLD_START,
ins_sample_rate);
ins.init(ins_sample_rate);
ahrs.reset();
print_hit_enter();
@ -435,8 +434,7 @@ int8_t Plane::test_mag(uint8_t argc, const Menu::arg *argv)
ahrs.set_compass(&compass);
// we need the AHRS initialised for this test
ins.init(AP_InertialSensor::COLD_START,
ins_sample_rate);
ins.init(ins_sample_rate);
ahrs.reset();
uint16_t counter = 0;