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

Plane: wire up accel calibrator for plane

Browse Source
This commit is contained in:
Siddharth Bharat Purohit 2015-10-21 14:11:52 -07:00 committed by Jonathan Challinger
parent 9a976963aa
commit 78566bda36
5 changed files with 27 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduPlane/ArduPlane.cpp
View File

@ -60,6 +60,7 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = {
SCHED_TASK(update_notify, 50, 300),
SCHED_TASK(read_rangefinder, 50, 500),
SCHED_TASK(compass_cal_update, 50, 100),
SCHED_TASK(accel_cal_update, 10, 100),
#if OPTFLOW == ENABLED
SCHED_TASK(update_optical_flow, 50, 500),
#endif

16
ArduPlane/GCS_Mavlink.cpp
View File

@ -1219,6 +1219,10 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
case MAV_CMD_PREFLIGHT_CALIBRATION:
if(hal.util->get_soft_armed()) {
result = MAV_RESULT_FAILED;
break;
}
plane.in_calibration = true;
if (is_equal(packet.param1,1.0f)) {
plane.ins.init_gyro();
@ -1238,21 +1242,18 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
plane.trim_radio();
result = MAV_RESULT_ACCEPTED;
} else if (is_equal(packet.param5,1.0f)) {
float trim_roll, trim_pitch;
AP_InertialSensor_UserInteract_MAVLink interact(this);
result = MAV_RESULT_ACCEPTED;
// 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
plane.ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0));
result = MAV_RESULT_ACCEPTED;
} else {
result = MAV_RESULT_FAILED;
}
plane.ins.acal_init();
plane.ins.get_acal()->start(this);
} else if (is_equal(packet.param5,2.0f)) {
// start with gyro calibration
plane.ins.init_gyro();
@ -1521,7 +1522,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
}
case MAVLINK_MSG_ID_SET_MODE:
{
handle_set_mode(msg, FUNCTOR_BIND(&plane, &Plane::mavlink_set_mode, bool, uint8_t));

4
ArduPlane/Plane.h
View File

@ -47,6 +47,7 @@
#include <AP_Math/AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <AP_ADC/AP_ADC.h> // ArduPilot Mega Analog to Digital Converter Library
#include <AP_InertialSensor/AP_InertialSensor.h> // Inertial Sensor Library
#include <AP_AccelCal/AP_AccelCal.h> // interface and maths for accelerometer calibration
#include <AP_AHRS/AP_AHRS.h> // ArduPilot Mega DCM Library
#include <RC_Channel/RC_Channel.h> // RC Channel Library
#include <AP_RangeFinder/AP_RangeFinder.h> // Range finder library
@ -702,6 +703,7 @@ private:
// time that rudder arming has been running
uint32_t rudder_arm_timer;
void demo_servos(uint8_t i);
void adjust_nav_pitch_throttle(void);
void update_load_factor(void);
@ -982,11 +984,11 @@ private:
void init_capabilities(void);
void dataflash_periodic(void);
uint16_t throttle_min(void) const;
void do_parachute(const AP_Mission::Mission_Command& cmd);
void parachute_check();
void parachute_release();
bool parachute_manual_release();
void accel_cal_update(void);
public:
void mavlink_delay_cb();

1
ArduPlane/make.inc
View File

@ -8,6 +8,7 @@ LIBRARIES += AP_Compass
LIBRARIES += AP_Math
LIBRARIES += AP_ADC
LIBRARIES += AP_InertialSensor
LIBRARIES += AP_AccelCal
LIBRARIES += AP_AHRS
LIBRARIES += RC_Channel
LIBRARIES += AP_RangeFinder

14
ArduPlane/sensors.cpp
View File

@ -65,6 +65,20 @@ void Plane::compass_cal_update() {
}
}
/*
Accel calibration
*/
void Plane::accel_cal_update() {
if (hal.util->get_soft_armed()) {
return;
}
ins.acal_update();
float trim_roll, trim_pitch;
if(ins.get_new_trim(trim_roll, trim_pitch)) {
ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0));
}
}
/*
ask airspeed sensor for a new value
*/