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

Rover: removed 1D accel calibration

Browse Source
This commit is contained in:
Andrew Tridgell 2015-03-11 10:17:33 +11:00
parent bd4476cb84
commit 29bf3f569e
3 changed files with 30 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
APMrover2/GCS_Mavlink.pde
View File

@ -847,15 +847,37 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
case MAV_CMD_PREFLIGHT_CALIBRATION:
if ((packet.param1 == 1 ||
packet.param2 == 1) &&
packet.param3 == 0) {
startup_INS_ground(true);
if (packet.param1 == 1) {
ins.init_gyro();
if (ins.gyro_calibrated_ok_all()) {
ahrs.reset_gyro_drift();
result = MAV_RESULT_ACCEPTED;
} else {
result = MAV_RESULT_FAILED;
}
} else if (packet.param3 == 1) {
init_barometer();
result = MAV_RESULT_ACCEPTED;
} else if (packet.param4 == 1) {
trim_radio();
result = MAV_RESULT_ACCEPTED;
} else {
} else if (packet.param5 == 1) {
float trim_roll, trim_pitch;
AP_InertialSensor_UserInteract_MAVLink interact(this);
if (g.skip_gyro_cal) {
// start with gyro calibration, otherwise if the user
// has SKIP_GYRO_CAL=1 they don't get to do it
ins.init_gyro();
}
if(ins.calibrate_accel(&interact, trim_roll, trim_pitch)) {
// reset ahrs's trim to suggested values from calibration routine
ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0));
result = MAV_RESULT_ACCEPTED;
} else {
result = MAV_RESULT_FAILED;
}
}
else {
send_text_P(SEVERITY_LOW, PSTR("Unsupported preflight calibration"));
}
break;

14
APMrover2/setup.pde
View File

@ -11,7 +11,6 @@ static int8_t setup_flightmodes (uint8_t argc, const Menu::arg *argv);
static int8_t setup_accel_scale (uint8_t argc, const Menu::arg *argv);
static int8_t setup_set (uint8_t argc, const Menu::arg *argv);
#endif
static int8_t setup_level (uint8_t argc, const Menu::arg *argv);
static int8_t setup_erase (uint8_t argc, const Menu::arg *argv);
static int8_t setup_compass (uint8_t argc, const Menu::arg *argv);
static int8_t setup_declination (uint8_t argc, const Menu::arg *argv);
@ -23,7 +22,6 @@ static const struct Menu::command setup_menu_commands[] PROGMEM = {
{"reset", setup_factory},
{"radio", setup_radio},
{"modes", setup_flightmodes},
{"level", setup_level},
#if !defined( __AVR_ATmega1280__ )
{"accel", setup_accel_scale},
#endif
@ -417,18 +415,6 @@ setup_accel_scale(uint8_t argc, const Menu::arg *argv)
}
#endif
static int8_t
setup_level(uint8_t argc, const Menu::arg *argv)
{
cliSerial->println_P(PSTR("Initialising gyros"));
ahrs.init();
ins.init(AP_InertialSensor::COLD_START,
ins_sample_rate);
ins.init_accel();
ahrs.set_trim(Vector3f(0, 0, 0));
return(0);
}
static int8_t
setup_compass(uint8_t argc, const Menu::arg *argv)
{

13
APMrover2/system.pde
View File

@ -235,7 +235,7 @@ static void startup_ground(void)
//------------------------
//
startup_INS_ground(false);
startup_INS_ground();
// read the radio to set trims
// ---------------------------
@ -368,7 +368,7 @@ static void failsafe_trigger(uint8_t failsafe_type, bool on)
}
}
static void startup_INS_ground(bool force_accel_level)
static void startup_INS_ground(void)
{
gcs_send_text_P(SEVERITY_MEDIUM, PSTR("Warming up ADC..."));
mavlink_delay(500);
@ -383,7 +383,7 @@ static void startup_INS_ground(bool force_accel_level)
ahrs.set_vehicle_class(AHRS_VEHICLE_GROUND);
AP_InertialSensor::Start_style style;
if (g.skip_gyro_cal && !force_accel_level) {
if (g.skip_gyro_cal) {
style = AP_InertialSensor::WARM_START;
} else {
style = AP_InertialSensor::COLD_START;
@ -391,13 +391,6 @@ static void startup_INS_ground(bool force_accel_level)
ins.init(style, ins_sample_rate);
if (force_accel_level) {
// when MANUAL_LEVEL is set to 1 we don't do accelerometer
// levelling on each boot, and instead rely on the user to do
// it once via the ground station
ins.init_accel();
ahrs.set_trim(Vector3f(0, 0, 0));
}
ahrs.reset();
}