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

Copter: support AP_AccelCal

Browse Source
This commit is contained in:
bugobliterator 2015-10-16 15:05:53 -07:00 committed by Jonathan Challinger
parent fe62a049bd
commit 9a976963aa
5 changed files with 21 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduCopter/ArduCopter.cpp
View File

@ -135,6 +135,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = {
SCHED_TASK(read_receiver_rssi, 10, 75), SCHED_TASK(read_receiver_rssi, 10, 75),
SCHED_TASK(rpm_update, 10, 200), SCHED_TASK(rpm_update, 10, 200),
SCHED_TASK(compass_cal_update, 100, 100), SCHED_TASK(compass_cal_update, 100, 100),
SCHED_TASK(accel_cal_update, 10, 100),
#if ADSB_ENABLED == ENABLED #if ADSB_ENABLED == ENABLED
SCHED_TASK(adsb_update, 1, 100), SCHED_TASK(adsb_update, 1, 100),
#endif #endif

2
ArduCopter/Copter.h
View File

@ -50,6 +50,7 @@
#include <AP_Baro/AP_Baro.h> #include <AP_Baro/AP_Baro.h>
#include <AP_Compass/AP_Compass.h> // ArduPilot Mega Magnetometer Library #include <AP_Compass/AP_Compass.h> // ArduPilot Mega Magnetometer Library
#include <AP_Math/AP_Math.h> // ArduPilot Mega Vector/Matrix math Library #include <AP_Math/AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <AP_AccelCal/AP_AccelCal.h> // interface and maths for accelerometer calibration
#include <AP_InertialSensor/AP_InertialSensor.h> // ArduPilot Mega Inertial Sensor (accel & gyro) Library #include <AP_InertialSensor/AP_InertialSensor.h> // ArduPilot Mega Inertial Sensor (accel & gyro) Library
#include <AP_AHRS/AP_AHRS.h> #include <AP_AHRS/AP_AHRS.h>
#include <AP_NavEKF/AP_NavEKF.h> #include <AP_NavEKF/AP_NavEKF.h>
@ -1003,6 +1004,7 @@ private:
void run_cli(AP_HAL::UARTDriver *port); void run_cli(AP_HAL::UARTDriver *port);
void init_capabilities(void); void init_capabilities(void);
void dataflash_periodic(void); void dataflash_periodic(void);
void accel_cal_update(void);
public: public:
void mavlink_delay_cb(); void mavlink_delay_cb();

14
ArduCopter/GCS_Mavlink.cpp
View File

@ -1323,20 +1323,14 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
result = MAV_RESULT_UNSUPPORTED; result = MAV_RESULT_UNSUPPORTED;
} else if (is_equal(packet.param5,1.0f)) { } else if (is_equal(packet.param5,1.0f)) {
// 3d accel calibration // 3d accel calibration
result = MAV_RESULT_ACCEPTED;
if (!copter.calibrate_gyros()) { if (!copter.calibrate_gyros()) {
result = MAV_RESULT_FAILED; result = MAV_RESULT_FAILED;
break; break;
} }
// this blocks copter.ins.acal_init();
float trim_roll, trim_pitch; copter.ins.get_acal()->start(this);
AP_InertialSensor_UserInteract_MAVLink interact(this);
if(copter.ins.calibrate_accel(&interact, trim_roll, trim_pitch)) {
// reset ahrs's trim to suggested values from calibration routine
copter.ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0));
result = MAV_RESULT_ACCEPTED;
} else {
result = MAV_RESULT_FAILED;
}
} else if (is_equal(packet.param5,2.0f)) { } else if (is_equal(packet.param5,2.0f)) {
// calibrate gyros // calibrate gyros
if (!copter.calibrate_gyros()) { if (!copter.calibrate_gyros()) {

1
ArduCopter/make.inc
View File

@ -12,6 +12,7 @@ LIBRARIES += AP_Baro
LIBRARIES += AP_Compass LIBRARIES += AP_Compass
LIBRARIES += AP_Math LIBRARIES += AP_Math
LIBRARIES += AP_InertialSensor LIBRARIES += AP_InertialSensor
LIBRARIES += AP_AccelCal
LIBRARIES += AP_AHRS LIBRARIES += AP_AHRS
LIBRARIES += AP_NavEKF LIBRARIES += AP_NavEKF
LIBRARIES += AP_NavEKF2 LIBRARIES += AP_NavEKF2

13
ArduCopter/sensors.cpp
View File

@ -181,6 +181,19 @@ void Copter::compass_cal_update()
} }
} }
void Copter::accel_cal_update()
{
if (hal.util->get_soft_armed()) {
return;
}
ins.acal_update();
// check if new trim values, and set them
float trim_roll, trim_pitch;
if(ins.get_new_trim(trim_roll, trim_pitch)) {
ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0));
}
}
#if EPM_ENABLED == ENABLED #if EPM_ENABLED == ENABLED
// epm update - moves epm pwm output back to neutral after grab or release is completed // epm update - moves epm pwm output back to neutral after grab or release is completed
void Copter::epm_update() void Copter::epm_update()