Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: updates for compass API change

This commit is contained in:
Andrew Tridgell 2013-12-09 14:10:14 +11:00
parent 615e718524
commit ffdcb715c3
5 changed files with 27 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
ArduCopter/GCS_Mavlink.pde
View File

@ -268,7 +268,7 @@ static void NOINLINE send_nav_controller_output(mavlink_channel_t chan)
static void NOINLINE send_ahrs(mavlink_channel_t chan)
{
Vector3f omega_I = ahrs.get_gyro_drift();
const Vector3f &omega_I = ahrs.get_gyro_drift();
mavlink_msg_ahrs_send(
chan,
omega_I.x,
@ -450,8 +450,9 @@ static void NOINLINE send_vfr_hud(mavlink_channel_t chan)
static void NOINLINE send_raw_imu1(mavlink_channel_t chan)
{
Vector3f accel = ins.get_accel();
Vector3f gyro = ins.get_gyro();
const Vector3f &accel = ins.get_accel();
const Vector3f &gyro = ins.get_gyro();
const Vector3f &mag = compass.get_field();
mavlink_msg_raw_imu_send(
chan,
micros(),
@ -461,9 +462,9 @@ static void NOINLINE send_raw_imu1(mavlink_channel_t chan)
gyro.x * 1000.0f,
gyro.y * 1000.0f,
gyro.z * 1000.0f,
compass.mag_x,
compass.mag_y,
compass.mag_z);
mag.x,
mag.y,
mag.z);
}
static void NOINLINE send_raw_imu2(mavlink_channel_t chan)

11
ArduCopter/Log.pde
View File

@ -351,13 +351,14 @@ struct PACKED log_Compass {
// Write a Compass packet
static void Log_Write_Compass()
{
Vector3f mag_offsets = compass.get_offsets();
Vector3f mag_motor_offsets = compass.get_motor_offsets();
const Vector3f &mag_offsets = compass.get_offsets();
const Vector3f &mag_motor_offsets = compass.get_motor_offsets();
const Vector3f &mag = compass.get_field();
struct log_Compass pkt = {
LOG_PACKET_HEADER_INIT(LOG_COMPASS_MSG),
mag_x : compass.mag_x,
mag_y : compass.mag_y,
mag_z : compass.mag_z,
mag_x : (int16_t)mag.x,
mag_y : (int16_t)mag.y,
mag_z : (int16_t)mag.z,
offset_x : (int16_t)mag_offsets.x,
offset_y : (int16_t)mag_offsets.y,
offset_z : (int16_t)mag_offsets.z,

2
ArduCopter/motors.pde
View File

@ -273,7 +273,7 @@ static void pre_arm_checks(bool display_failure)
}
// check for unreasonable mag field length
float mag_field = pythagorous3(compass.mag_x, compass.mag_y, compass.mag_z);
float mag_field = compass.get_field().length();
if (mag_field > COMPASS_MAGFIELD_EXPECTED*1.65 || mag_field < COMPASS_MAGFIELD_EXPECTED*0.35) {
if (display_failure) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Check mag field"));

12
ArduCopter/setup.pde
View File

@ -189,9 +189,7 @@ setup_compassmot(uint8_t argc, const Menu::arg *argv)
}
// store initial x,y,z compass values
compass_base.x = compass.mag_x;
compass_base.y = compass.mag_y;
compass_base.z = compass.mag_z;
compass_base = compass.get_field();
// initialise motor compensation
motor_compensation = Vector3f(0,0,0);
@ -234,18 +232,14 @@ setup_compassmot(uint8_t argc, const Menu::arg *argv)
// if throttle is zero, update base x,y,z values
if( throttle_pct == 0.0f ) {
compass_base.x = compass_base.x * 0.99f + (float)compass.mag_x * 0.01f;
compass_base.y = compass_base.y * 0.99f + (float)compass.mag_y * 0.01f;
compass_base.z = compass_base.z * 0.99f + (float)compass.mag_z * 0.01f;
compass_base = compass_base * 0.99f + compass.get_field() * 0.01f;
// causing printing to happen as soon as throttle is lifted
print_counter = 49;
}else{
// calculate diff from compass base and scale with throttle
motor_impact.x = compass.mag_x - compass_base.x;
motor_impact.y = compass.mag_y - compass_base.y;
motor_impact.z = compass.mag_z - compass_base.z;
motor_impact = compass.get_field() - compass_base;
// throttle based compensation
if( comp_type == AP_COMPASS_MOT_COMP_THROTTLE ) {

19
ArduCopter/test.pde
View File

@ -142,15 +142,16 @@ test_compass(uint8_t argc, const Menu::arg *argv)
counter++;
if (counter>20) {
if (compass.healthy) {
Vector3f maggy = compass.get_offsets();
cliSerial->printf_P(PSTR("Heading: %ld, XYZ: %d, %d, %d,\tXYZoff: %6.2f, %6.2f, %6.2f\n"),
(wrap_360_cd(ToDeg(heading) * 100)) /100,
(int)compass.mag_x,
(int)compass.mag_y,
(int)compass.mag_z,
maggy.x,
maggy.y,
maggy.z);
const Vector3f &mag_ofs = compass.get_offsets();
const Vector3f &mag = compass.get_field();
cliSerial->printf_P(PSTR("Heading: %ld, XYZ: %.0f, %.0f, %.0f,\tXYZoff: %6.2f, %6.2f, %6.2f\n"),
(wrap_360_cd(ToDeg(heading) * 100)) /100,
mag.x,
mag.y,
mag.z,
mag_ofs.x,
mag_ofs.y,
mag_ofs.z);
} else {
cliSerial->println_P(PSTR("compass not healthy"));
}