mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-22 00:28:30 -04:00
Rover: updates for new compass API
This commit is contained in:
parent
217f34e155
commit
44dd9ddf74
@ -83,7 +83,6 @@
|
|||||||
# define CONFIG_COMPASS AP_COMPASS_HMC5843
|
# define CONFIG_COMPASS AP_COMPASS_HMC5843
|
||||||
# define CONFIG_PUSHBUTTON DISABLED
|
# define CONFIG_PUSHBUTTON DISABLED
|
||||||
# define CONFIG_RELAY DISABLED
|
# define CONFIG_RELAY DISABLED
|
||||||
# define MAG_ORIENTATION AP_COMPASS_APM2_SHIELD
|
|
||||||
# define CONFIG_SONAR_SOURCE SONAR_SOURCE_ANALOG_PIN
|
# define CONFIG_SONAR_SOURCE SONAR_SOURCE_ANALOG_PIN
|
||||||
# define A_LED_PIN 27
|
# define A_LED_PIN 27
|
||||||
# define B_LED_PIN 26
|
# define B_LED_PIN 26
|
||||||
@ -113,7 +112,6 @@
|
|||||||
# define USB_MUX_PIN -1
|
# define USB_MUX_PIN -1
|
||||||
# define BATTERY_PIN_1 1
|
# define BATTERY_PIN_1 1
|
||||||
# define CURRENT_PIN_1 2
|
# define CURRENT_PIN_1 2
|
||||||
# define MAG_ORIENTATION AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD
|
|
||||||
#elif CONFIG_HAL_BOARD == HAL_BOARD_PX4
|
#elif CONFIG_HAL_BOARD == HAL_BOARD_PX4
|
||||||
# define CONFIG_INS_TYPE CONFIG_INS_PX4
|
# define CONFIG_INS_TYPE CONFIG_INS_PX4
|
||||||
# define CONFIG_COMPASS AP_COMPASS_PX4
|
# define CONFIG_COMPASS AP_COMPASS_PX4
|
||||||
@ -131,7 +129,6 @@
|
|||||||
# define USB_MUX_PIN -1
|
# define USB_MUX_PIN -1
|
||||||
# define BATTERY_PIN_1 -1
|
# define BATTERY_PIN_1 -1
|
||||||
# define CURRENT_PIN_1 -1
|
# define CURRENT_PIN_1 -1
|
||||||
# define MAG_ORIENTATION ROTATION_NONE
|
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
//////////////////////////////////////////////////////////////////////////////
|
//////////////////////////////////////////////////////////////////////////////
|
||||||
@ -271,9 +268,6 @@
|
|||||||
#ifndef MAGNETOMETER
|
#ifndef MAGNETOMETER
|
||||||
# define MAGNETOMETER ENABLED
|
# define MAGNETOMETER ENABLED
|
||||||
#endif
|
#endif
|
||||||
#ifndef MAG_ORIENTATION
|
|
||||||
# define MAG_ORIENTATION AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD
|
|
||||||
#endif
|
|
||||||
|
|
||||||
//////////////////////////////////////////////////////////////////////////////
|
//////////////////////////////////////////////////////////////////////////////
|
||||||
// MODE
|
// MODE
|
||||||
|
@ -440,7 +440,6 @@ static int8_t
|
|||||||
setup_compass(uint8_t argc, const Menu::arg *argv)
|
setup_compass(uint8_t argc, const Menu::arg *argv)
|
||||||
{
|
{
|
||||||
if (!strcmp_P(argv[1].str, PSTR("on"))) {
|
if (!strcmp_P(argv[1].str, PSTR("on"))) {
|
||||||
compass.set_orientation(MAG_ORIENTATION); // set compass's orientation on aircraft
|
|
||||||
if (!compass.init()) {
|
if (!compass.init()) {
|
||||||
cliSerial->println_P(PSTR("Compass initialisation failed!"));
|
cliSerial->println_P(PSTR("Compass initialisation failed!"));
|
||||||
g.compass_enabled = false;
|
g.compass_enabled = false;
|
||||||
|
@ -171,7 +171,6 @@ static void init_ardupilot()
|
|||||||
#endif
|
#endif
|
||||||
|
|
||||||
if (g.compass_enabled==true) {
|
if (g.compass_enabled==true) {
|
||||||
compass.set_orientation(MAG_ORIENTATION); // set compass's orientation on aircraft
|
|
||||||
if (!compass.init()|| !compass.read()) {
|
if (!compass.init()|| !compass.read()) {
|
||||||
cliSerial->println_P(PSTR("Compass initialisation failed!"));
|
cliSerial->println_P(PSTR("Compass initialisation failed!"));
|
||||||
g.compass_enabled = false;
|
g.compass_enabled = false;
|
||||||
|
@ -469,7 +469,6 @@ test_mag(uint8_t argc, const Menu::arg *argv)
|
|||||||
return (0);
|
return (0);
|
||||||
}
|
}
|
||||||
|
|
||||||
compass.set_orientation(MAG_ORIENTATION);
|
|
||||||
if (!compass.init()) {
|
if (!compass.init()) {
|
||||||
cliSerial->println_P(PSTR("Compass initialisation failed!"));
|
cliSerial->println_P(PSTR("Compass initialisation failed!"));
|
||||||
return 0;
|
return 0;
|
||||||
@ -488,8 +487,6 @@ test_mag(uint8_t argc, const Menu::arg *argv)
|
|||||||
int counter = 0;
|
int counter = 0;
|
||||||
float heading = 0;
|
float heading = 0;
|
||||||
|
|
||||||
//cliSerial->printf_P(PSTR("MAG_ORIENTATION: %d\n"), MAG_ORIENTATION);
|
|
||||||
|
|
||||||
print_hit_enter();
|
print_hit_enter();
|
||||||
|
|
||||||
while(1) {
|
while(1) {
|
||||||
|
Loading…
Reference in New Issue
Block a user