Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-28 02:33:58 -04:00
Code Issues Packages Projects Releases Wiki Activity

Rover: replace SKIP_GYRO_CAL with INS_GYR_CAL

Browse Source 
Also calibrate gyros during accel trim
This commit is contained in:
Randy Mackay 2015-09-17 21:33:06 +09:00
parent 6da53ae3b2
commit bcc87a9a3b
5 changed files with 12 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
APMrover2/GCS_Mavlink.cpp
View File

@ -145,7 +145,7 @@ void Rover::send_extended_status1(mavlink_channel_t chan)
if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_GPS;
}
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()) {
@ -974,10 +974,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 (rover.g.skip_gyro_cal) {
// start with gyro calibration, otherwise if the user
// has SKIP_GYRO_CAL=1 they don't get to do it
rover.ins.init_gyro();
// start with gyro calibration
rover.ins.init_gyro();
// reset ahrs gyro bias
if (rover.ins.gyro_calibrated_ok_all()) {
rover.ahrs.reset_gyro_drift();
}
if(rover.ins.calibrate_accel(&interact, trim_roll, trim_pitch)) {
// reset ahrs's trim to suggested values from calibration routine
@ -987,6 +988,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
rover.ins.init_gyro();
// accel trim
float trim_roll, trim_pitch;
if(rover.ins.calibrate_trim(trim_roll, trim_pitch)) {

7
APMrover2/Parameters.cpp
View File

@ -76,13 +76,6 @@ const AP_Param::Info Rover::var_info[] PROGMEM = {
// @Values: 0:None,1:Steering
// @Bitmask: 0:Steering
GSCALAR(gcs_pid_mask, "GCS_PID_MASK", 0),
// @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 vehicle 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: MAG_ENABLED
// @DisplayName: Magnetometer (compass) enabled

3
APMrover2/Parameters.h
View File

@ -67,7 +67,7 @@ public:
k_param_serial0_baud_old,
k_param_serial1_baud_old,
k_param_telem_delay,
k_param_skip_gyro_cal,
k_param_skip_gyro_cal, // unused
k_param_gcs2, // stream rates for uartD
k_param_serial2_baud_old,
k_param_serial2_protocol, // deprecated, can be deleted
@ -214,7 +214,6 @@ public:
AP_Int16 sysid_this_mav;
AP_Int16 sysid_my_gcs;
AP_Int8 telem_delay;
AP_Int8 skip_gyro_cal;
#if CLI_ENABLED == ENABLED
AP_Int8 cli_enabled;
#endif

9
APMrover2/system.cpp
View File

@ -405,14 +405,7 @@ void Rover::startup_INS_ground(void)
ahrs.set_fly_forward(true);
ahrs.set_vehicle_class(AHRS_VEHICLE_GROUND);
AP_InertialSensor::Start_style style;
if (g.skip_gyro_cal) {
style = AP_InertialSensor::WARM_START;
} else {
style = AP_InertialSensor::COLD_START;
}
ins.init(style, ins_sample_rate);
ins.init(ins_sample_rate);
ahrs.reset();
}

6
APMrover2/test.cpp
View File

@ -308,8 +308,7 @@ int8_t Rover::test_ins(uint8_t argc, const Menu::arg *argv)
//cliSerial->printf_P(PSTR("Calibrating."));
ahrs.init();
ahrs.set_fly_forward(true);
ins.init(AP_InertialSensor::COLD_START,
ins_sample_rate);
ins.init(ins_sample_rate);
ahrs.reset();
print_hit_enter();
@ -372,8 +371,7 @@ int8_t Rover::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();
int counter = 0;