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

Copter: pre-arm check for mag field length

Browse Source
This commit is contained in:
Randy Mackay 2013-05-31 12:23:19 +09:00
parent a1821c89e7
commit 7b50ecc73c
3 changed files with 23 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
ArduCopter/config.h
View File

@ -421,6 +421,17 @@
# define MAGNETOMETER ENABLED
#endif
// expected magnetic field strength
#if CONFIG_HAL_BOARD == HAL_BOARD_APM2
#ifndef COMPASS_MAGFIELD_EXPECTED
# define COMPASS_MAGFIELD_EXPECTED 330 // maximum mag field length for pre-arm checks
#endif
#else // APM1, PX4, SITL
#ifndef COMPASS_MAGFIELD_EXPECTED
#define COMPASS_MAGFIELD_EXPECTED 530 // maximum mag field length for pre-arm checks
#endif
#endif
//////////////////////////////////////////////////////////////////////////////
// OPTICAL_FLOW
#if defined( __AVR_ATmega2560__ ) // determines if optical flow code is included

11
ArduCopter/motors.pde
View File

@ -246,6 +246,17 @@ static void pre_arm_checks(bool display_failure)
return;
}
// check for unreasonable mag field length
float mag_field = pythagorous3(compass.mag_x, compass.mag_y, compass.mag_z);
if (mag_field > COMPASS_MAGFIELD_EXPECTED*1.5 || mag_field < COMPASS_MAGFIELD_EXPECTED*0.5) {
if (display_failure) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Check mag field"));
}
return;
}else{
cliSerial->printf_P(PSTR("\nMagField: %4.2f vs %4.2f ~ %4.2f"),mag_field, (float)COMPASS_MAGFIELD_EXPECTED*1.5, COMPASS_MAGFIELD_EXPECTED*0.5);
}
// barometer health check
if(!barometer.healthy) {
if (display_failure) {

2
ArduCopter/setup.pde
View File

@ -660,7 +660,7 @@ setup_compassmot(uint8_t argc, const Menu::arg *argv)
// calculate and display interference compensation at full throttle as % of total mag field
if (comp_type == AP_COMPASS_MOT_COMP_THROTTLE) {
// interference is impact@fullthrottle / mag field * 100
interference_pct = motor_compensation.length() / 330.0f * 100.0f;
interference_pct = motor_compensation.length() / (float)COMPASS_MAGFIELD_EXPECTED * 100.0f;
}else{
// interference is impact/amp * (max current seen / max throttle seen) / mag field * 100
interference_pct = motor_compensation.length() * (current_amps_max/throttle_pct_max) / 330.0f * 100.0f;