From 9e986801c99ea8b8ee347685211e9940d2bff79c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 18 Dec 2012 21:15:11 +1100 Subject: [PATCH] Copter: fixed SITL for ArduCopter --- ArduCopter/ArduCopter.pde | 20 +++++++------------ ArduCopter/GCS.h | 12 ++---------- ArduCopter/GCS_Mavlink.pde | 39 +++++++++++--------------------------- ArduCopter/Makefile | 3 --- ArduCopter/Parameters.h | 3 +++ ArduCopter/Parameters.pde | 4 +++- ArduCopter/config.h | 21 +++++++++++++++++++- ArduCopter/defines.h | 3 ++- ArduCopter/motors.pde | 4 ++-- ArduCopter/system.pde | 2 -- 10 files changed, 50 insertions(+), 61 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index ab43abe147..456cad4633 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -67,6 +67,7 @@ #include #include #include +#include // Application dependencies #include // MAVLink GCS definitions @@ -97,6 +98,7 @@ #include // ArduPilot Mega Declination Helper Library #include #include +#include // AP_HAL to Arduino compatibility layer #include "compat.h" @@ -123,15 +125,7 @@ AP_HAL::BetterStream* cliSerial; // AP_HAL instance //////////////////////////////////////////////////////////////////////////////// -#if CONFIG_HAL_BOARD == HAL_BOARD_APM2 -const AP_HAL::HAL& hal = AP_HAL_AVR_APM2; -#elif CONFIG_HAL_BOARD == HAL_BOARD_APM1 -const AP_HAL::HAL& hal = AP_HAL_AVR_APM1; -#elif CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL -const AP_HAL::HAL& hal = AP_HAL_AVR_SITL; -#include -SITL sitl; -#endif +const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER; //////////////////////////////////////////////////////////////////////////////// @@ -192,14 +186,17 @@ AP_ADC_ADS7844 adc; #if CONFIG_IMU_TYPE == CONFIG_IMU_MPU6000 AP_InertialSensor_MPU6000 ins; - #else +#elif CONFIG_IMU_TYPE == CONFIG_IMU_OILPAN AP_InertialSensor_Oilpan ins(&adc); +#elif CONFIG_IMU_TYPE == CONFIG_IMU_SITL +AP_InertialSensor_Stub ins; #endif #if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL // When building for SITL we use the HIL barometer and compass drivers AP_Baro_BMP085_HIL barometer; AP_Compass_HIL compass; +SITL sitl; #else // Otherwise, instantiate a real barometer and compass driver #if CONFIG_BARO == AP_BARO_BMP085 @@ -995,9 +992,6 @@ void loop() } } } else { -#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL - usleep(1000); -#endif if (timer - fast_loopTimer < 9) { // we have some spare cycles available // less than 10ms has passed. We have at least one millisecond diff --git a/ArduCopter/GCS.h b/ArduCopter/GCS.h index 03e840f20e..af1c5c1c42 100644 --- a/ArduCopter/GCS.h +++ b/ArduCopter/GCS.h @@ -61,20 +61,12 @@ public: void send_message(enum ap_message id) { } - /// Send a text message. - /// - /// @param severity A value describing the importance of the message. - /// @param str The text to be sent. - /// - void send_text(gcs_severity severity, const char *str) { - } - /// Send a text message with a PSTR() /// /// @param severity A value describing the importance of the message. /// @param str The text to be sent. /// - void send_text(gcs_severity severity, const prog_char_t *str) { + void send_text_P(gcs_severity severity, const prog_char_t *str) { } // send streams which match frequency range @@ -110,7 +102,7 @@ public: void init(AP_HAL::UARTDriver *port); void send_message(enum ap_message id); void send_text(gcs_severity severity, const char *str); - void send_text(gcs_severity severity, const prog_char_t *str); + void send_text_P(gcs_severity severity, const prog_char_t *str); void data_stream_send(void); void queued_param_send(); void queued_waypoint_send(); diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index 355d509fe6..83c67d999a 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -194,8 +194,10 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack static void NOINLINE send_meminfo(mavlink_channel_t chan) { +#if CONFIG_HAL_BOARD != HAL_BOARD_AVR_SITL extern unsigned __brkval; mavlink_msg_meminfo_send(chan, __brkval, memcheck_available_memory()); +#endif } static void NOINLINE send_location(mavlink_channel_t chan) @@ -252,7 +254,7 @@ static void NOINLINE send_ahrs(mavlink_channel_t chan) ahrs.get_error_yaw()); } -#ifdef DESKTOP_BUILD +#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL // report simulator state static void NOINLINE send_simstate(mavlink_channel_t chan) { @@ -260,7 +262,6 @@ static void NOINLINE send_simstate(mavlink_channel_t chan) } #endif -#ifndef DESKTOP_BUILD static void NOINLINE send_hwstatus(mavlink_channel_t chan) { mavlink_msg_hwstatus_send( @@ -268,8 +269,6 @@ static void NOINLINE send_hwstatus(mavlink_channel_t chan) board_voltage(), hal.i2c->lockup_count()); } -#endif - static void NOINLINE send_gps_raw(mavlink_channel_t chan) { @@ -639,17 +638,15 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id, break; case MSG_SIMSTATE: -#ifdef DESKTOP_BUILD +#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL CHECK_PAYLOAD_SIZE(SIMSTATE); send_simstate(chan); #endif break; case MSG_HWSTATUS: -#ifndef DESKTOP_BUILD CHECK_PAYLOAD_SIZE(HWSTATUS); send_hwstatus(chan); -#endif break; case MSG_RETRY_DEFERRED: @@ -955,13 +952,7 @@ GCS_MAVLINK::send_message(enum ap_message id) } void -GCS_MAVLINK::send_text(gcs_severity severity, const char *str) -{ - mavlink_send_text(chan,severity,str); -} - -void -GCS_MAVLINK::send_text(gcs_severity severity, const prog_char_t *str) +GCS_MAVLINK::send_text_P(gcs_severity severity, const prog_char_t *str) { mavlink_statustext_t m; uint8_t i; @@ -1062,7 +1053,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) uint8_t result = MAV_RESULT_UNSUPPORTED; // do command - send_text(SEVERITY_LOW,PSTR("command received: ")); + send_text_P(SEVERITY_LOW,PSTR("command received: ")); switch(packet.command) { @@ -1642,7 +1633,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) msg->compid, type); - send_text(SEVERITY_LOW,PSTR("flight plan received")); + send_text_P(SEVERITY_LOW,PSTR("flight plan received")); waypoint_receiving = false; // XXX ignores waypoint radius for individual waypoints, can // only set WP_RADIUS parameter @@ -1944,7 +1935,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) mavlink_fence_point_t packet; mavlink_msg_fence_point_decode(msg, &packet); if (packet.count != geofence_limit.fence_total()) { - send_text(SEVERITY_LOW,PSTR("bad fence point")); + send_text_P(SEVERITY_LOW,PSTR("bad fence point")); } else { Vector2l point; point.x = packet.lat*1.0e7; @@ -1960,7 +1951,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) if (mavlink_check_target(packet.target_system, packet.target_component)) break; if (packet.idx >= geofence_limit.fence_total()) { - send_text(SEVERITY_LOW,PSTR("bad fence point")); + send_text_P(SEVERITY_LOW,PSTR("bad fence point")); } else { Vector2l point = geofence_limit.get_fence_point_with_index(packet.idx); mavlink_msg_fence_point_send(chan, 0, 0, packet.idx, geofence_limit.fence_total(), @@ -2111,19 +2102,11 @@ static void gcs_update(void) } } -static void gcs_send_text(gcs_severity severity, const char *str) -{ - gcs0.send_text(severity, str); - if (gcs3.initialised) { - gcs3.send_text(severity, str); - } -} - static void gcs_send_text_P(gcs_severity severity, const prog_char_t *str) { - gcs0.send_text(severity, str); + gcs0.send_text_P(severity, str); if (gcs3.initialised) { - gcs3.send_text(severity, str); + gcs3.send_text_P(severity, str); } } diff --git a/ArduCopter/Makefile b/ArduCopter/Makefile index 71ba27181c..ebac6448da 100644 --- a/ArduCopter/Makefile +++ b/ArduCopter/Makefile @@ -42,9 +42,6 @@ apm2hexa: apm2 apm2beta: EXTRAFLAGS += "-DAPM2_BETA_HARDWARE " apm2beta: apm2 -sitl: HAL_BOARD = "HAL_BOARD_AVR_SITL" -sitl: all - sitl-octa: EXTRAFLAGS += "-DFRAME_CONFIG=OCTA_FRAME " sitl-octa: sitl diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 0b800b6b00..714ce4d2ba 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -58,6 +58,9 @@ public: // simulation k_param_sitl = 10, + // barometer object (needed for SITL) + k_param_barometer, + // Misc // k_param_log_bitmask = 20, diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde index a343b10e55..e7d2c0872f 100644 --- a/ArduCopter/Parameters.pde +++ b/ArduCopter/Parameters.pde @@ -577,10 +577,12 @@ const AP_Param::Info var_info[] PROGMEM = { GOBJECT(camera_mount2, "MNT2_", AP_Mount), #endif -#ifdef DESKTOP_BUILD +#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL GOBJECT(sitl, "SIM_", SITL), #endif + GOBJECT(barometer, "GND_", AP_Baro), + //@Group: LIM_ //@Path: ../libraries/AP_Limits/AP_Limits.cpp,../libraries/AP_Limits/AP_Limit_GPSLock.cpp, ../libraries/AP_Limits/AP_Limit_Geofence.cpp, ../libraries/AP_Limits/AP_Limit_Altitude.cpp, ../libraries/AP_Limits/AP_Limit_Module.cpp GOBJECT(limits, "LIM_", AP_Limits), diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 27611eedb4..b6bc86e55f 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -64,6 +64,13 @@ # else // APM2 Production Hardware (default) # define CONFIG_BARO AP_BARO_MS5611 # endif +#elif CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL + # define CONFIG_IMU_TYPE CONFIG_IMU_SITL + # define CONFIG_PUSHBUTTON DISABLED + # define CONFIG_RELAY DISABLED + # define MAG_ORIENTATION AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD + # define CONFIG_SONAR_SOURCE SONAR_SOURCE_ANALOG_PIN + # define MAGNETOMETER ENABLED #endif ////////////////////////////////////////////////////////////////////////////// @@ -157,6 +164,18 @@ # define USB_MUX_PIN 23 # define BATTERY_VOLT_PIN 1 // Battery voltage on A1 # define BATTERY_CURR_PIN 2 // Battery current on A2 +#elif CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL + # define A_LED_PIN 27 + # define B_LED_PIN 26 + # define C_LED_PIN 25 + # define LED_ON LOW + # define LED_OFF HIGH + # define SLIDE_SWITCH_PIN (-1) + # define PUSHBUTTON_PIN (-1) + # define CLI_SLIDER_ENABLED DISABLED + # define USB_MUX_PIN -1 + # define BATTERY_VOLT_PIN 1 // Battery voltage on A1 + # define BATTERY_CURR_PIN 2 // Battery current on A2 #endif //////////////////////////////////////////////////////////////////////////////// @@ -179,7 +198,7 @@ #define COPTER_LED_6 AN9 // Motor LED #define COPTER_LED_7 AN10 // Motor LED #define COPTER_LED_8 AN11 // Motor LED -#elif CONFIG_HAL_BOARD == HAL_BOARD_APM1 +#elif CONFIG_HAL_BOARD == HAL_BOARD_APM1 || CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL #define COPTER_LED_1 AN8 // Motor or Aux LED #define COPTER_LED_2 AN9 // Motor LED #define COPTER_LED_3 AN10 // Motor or GPS LED diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 7be7d28a2f..5501744be1 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -410,8 +410,9 @@ enum gcs_severity { #define NOINLINE __attribute__((noinline)) // IMU selection -#define CONFIG_IMU_OILPAN 1 +#define CONFIG_IMU_OILPAN 1 #define CONFIG_IMU_MPU6000 2 +#define CONFIG_IMU_SITL 3 #define AP_BARO_BMP085 1 #define AP_BARO_MS5611 2 diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index 90a1080dbb..cfce5a9a1a 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -104,7 +104,7 @@ static void init_arm_motors() failsafe_disable(); //cliSerial->printf("\nARM\n"); -#if HIL_MODE != HIL_MODE_DISABLED || defined(DESKTOP_BUILD) +#if HIL_MODE != HIL_MODE_DISABLED || CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL gcs_send_text_P(SEVERITY_HIGH, PSTR("ARMING MOTORS")); #endif @@ -170,7 +170,7 @@ static void init_arm_motors() static void init_disarm_motors() { -#if HIL_MODE != HIL_MODE_DISABLED || defined(DESKTOP_BUILD) +#if HIL_MODE != HIL_MODE_DISABLED || CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL gcs_send_text_P(SEVERITY_HIGH, PSTR("DISARMING MOTORS")); #endif diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index 5a37fe4a18..62a2d5d090 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -625,7 +625,6 @@ void flash_leds(bool on) digitalWrite(C_LED_PIN, on ? LED_ON : LED_OFF); } -#ifndef DESKTOP_BUILD /* * Read Vcc vs 1.1v internal reference */ @@ -633,7 +632,6 @@ uint16_t board_voltage(void) { return board_vcc_analog_source->read_latest(); } -#endif /* force a software reset of the APM