aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Sub: move common calibration functions up

Browse Source
This commit is contained in:
Peter Barker 2018-03-17 22:56:29 +11:00 committed by Francisco Ferreira
parent 71bc3f5def
commit 5536a321ed
3 changed files with 0 additions and 56 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
ArduSub/GCS_Mavlink.cpp
View File

@ -863,13 +863,6 @@ void GCS_MAVLINK_Sub::handle_change_alt_request(AP_Mission::Mission_Command &cmd
MAV_RESULT GCS_MAVLINK_Sub::_handle_command_preflight_calibration(const mavlink_command_long_t &packet)
{
if (is_equal(packet.param1,1.0f)) {
if (!sub.calibrate_gyros()) {
return MAV_RESULT_FAILED;
}
return MAV_RESULT_ACCEPTED;
}
if (is_equal(packet.param3,1.0f)) {
if (!sub.sensor_health.depth || sub.motors.armed()) {
return MAV_RESULT_FAILED;
@ -878,40 +871,6 @@ MAV_RESULT GCS_MAVLINK_Sub::_handle_command_preflight_calibration(const mavlink_
return MAV_RESULT_ACCEPTED;
}
if (is_equal(packet.param4,1.0f)) {
return MAV_RESULT_UNSUPPORTED;
}
if (is_equal(packet.param5,1.0f)) {
// 3d accel calibration
if (!sub.calibrate_gyros()) {
return MAV_RESULT_FAILED;
}
sub.ins.acal_init();
sub.ins.get_acal()->start(this);
return MAV_RESULT_ACCEPTED;
}
if (is_equal(packet.param5,2.0f)) {
// calibrate gyros
if (!sub.calibrate_gyros()) {
return MAV_RESULT_FAILED;
}
// accel trim
float trim_roll, trim_pitch;
if (!sub.ins.calibrate_trim(trim_roll, trim_pitch)) {
return MAV_RESULT_FAILED;
}
// reset ahrs's trim to suggested values from calibration routine
sub.ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0));
return MAV_RESULT_ACCEPTED;
}
if (is_equal(packet.param5,4.0f)) {
// simple accel calibration
return sub.ins.simple_accel_cal(sub.ahrs);
}
if (is_equal(packet.param6,1.0f)) {
// compassmot calibration
//result = sub.mavlink_compassmot(chan);

1
ArduSub/Sub.h
View File

@ -633,7 +633,6 @@ private:
bool terrain_use();
void init_ardupilot();
void startup_INS_ground();
bool calibrate_gyros();
bool position_ok();
bool ekf_position_ok();
bool optflow_position_ok();

14
ArduSub/system.cpp
View File

@ -230,20 +230,6 @@ void Sub::startup_INS_ground()
}
// calibrate gyros - returns true if successfully calibrated
bool Sub::calibrate_gyros()
{
// gyro offset calibration
sub.ins.init_gyro();
// reset ahrs gyro bias
if (sub.ins.gyro_calibrated_ok_all()) {
sub.ahrs.reset_gyro_drift();
return true;
}
return false;
}
// position_ok - returns true if the horizontal absolute position is ok and home position is set
bool Sub::position_ok()
{