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

Rover: fixed HIL operation 

only HIL sensors - removed HIL_MODE_ATTITUDE as it didn't exercise
enough of the code
This commit is contained in:
Andrew Tridgell 2013-06-04 11:57:59 +10:00
parent bfa9e04aef
commit feb539bade
9 changed files with 10 additions and 136 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
APMrover2/APMrover2.pde
View File

@ -169,7 +169,7 @@ static GPS *g_gps;
// flight modes convenience array
static AP_Int8 *modes = &g.mode1;
#if CONFIG_ADC == ENABLED
#if CONFIG_HAL_BOARD == HAL_BOARD_APM1
static AP_ADC_ADS7844 adc;
#endif
@ -224,11 +224,7 @@ AP_InertialSensor_Oilpan ins( &adc );
#error Unrecognised CONFIG_INS_TYPE setting.
#endif // CONFIG_INS_TYPE
#if HIL_MODE == HIL_MODE_ATTITUDE
AP_AHRS_HIL ahrs(&ins, g_gps);
#else
AP_AHRS_DCM ahrs(&ins, g_gps);
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
SITL sitl;
@ -550,8 +546,8 @@ static float load;
microseconds)
*/
static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
{ update_GPS, 5, 1200 },
{ navigate, 5, 1000 },
{ update_GPS, 5, 2500 },
{ navigate, 5, 1600 },
{ update_compass, 5, 2000 },
{ update_commands, 5, 1000 },
{ update_logging, 5, 1000 },
@ -638,7 +634,7 @@ static void fast_loop()
// some space available
gcs_send_message(MSG_RETRY_DEFERRED);
#if HIL_MODE == HIL_MODE_SENSORS
#if HIL_MODE != HIL_MODE_DISABLED
// update hil before dcm update
gcs_update();
#endif
@ -650,13 +646,11 @@ static void fast_loop()
// uses the yaw from the DCM to give more accurate turns
calc_bearing_error();
# if HIL_MODE == HIL_MODE_DISABLED
if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST)
Log_Write_Attitude();
if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST)
Log_Write_Attitude();
if (g.log_bitmask & MASK_LOG_IMU)
DataFlash.Log_Write_IMU(&ins);
#endif
if (g.log_bitmask & MASK_LOG_IMU)
DataFlash.Log_Write_IMU(&ins);
// custom code/exceptions for flight modes
// ---------------------------------------

9
APMrover2/GCS_Mavlink.pde
View File

@ -1700,14 +1700,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
ins.set_accel(accels);
compass.setHIL(packet.roll, packet.pitch, packet.yaw);
#if HIL_MODE == HIL_MODE_ATTITUDE
// set AHRS hil sensor
ahrs.setHil(packet.roll,packet.pitch,packet.yaw,packet.rollspeed,
packet.pitchspeed,packet.yawspeed);
#endif
break;
break;
}
#endif // HIL_MODE

6
APMrover2/Log.pde
View File

@ -175,7 +175,6 @@ struct PACKED log_Performance {
};
// Write a performance monitoring packet. Total length : 19 bytes
#if HIL_MODE != HIL_MODE_ATTITUDE
static void Log_Write_Performance()
{
struct log_Performance pkt = {
@ -194,7 +193,6 @@ static void Log_Write_Performance()
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
#endif
struct PACKED log_Cmd {
LOG_PACKET_HEADER;
@ -258,7 +256,6 @@ struct PACKED log_Control_Tuning {
};
// Write a control tuning packet. Total length : 22 bytes
#if HIL_MODE != HIL_MODE_ATTITUDE
static void Log_Write_Control_Tuning()
{
Vector3f accel = ins.get_accel();
@ -272,7 +269,6 @@ static void Log_Write_Control_Tuning()
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
#endif
struct PACKED log_Nav_Tuning {
LOG_PACKET_HEADER;
@ -350,7 +346,6 @@ struct PACKED log_Sonar {
};
// Write a sonar packet
#if HIL_MODE != HIL_MODE_ATTITUDE
static void Log_Write_Sonar()
{
uint16_t turn_time = 0;
@ -370,7 +365,6 @@ static void Log_Write_Sonar()
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
#endif
struct PACKED log_Current {
LOG_PACKET_HEADER;

2
APMrover2/Parameters.pde
View File

@ -435,11 +435,9 @@ const AP_Param::Info var_info[] PROGMEM = {
// @Path: ../libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp
GOBJECT(sonar2, "SONAR2_", AP_RangeFinder_analog),
#if HIL_MODE == HIL_MODE_DISABLED
// @Group: INS_
// @Path: ../libraries/AP_InertialSensor/AP_InertialSensor.cpp
GOBJECT(ins, "INS_", AP_InertialSensor),
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
// @Group: SIM_

50
APMrover2/config.h
View File

@ -77,13 +77,11 @@
# define CONFIG_RELAY ENABLED
# define BATTERY_PIN_1 0
# define CURRENT_PIN_1 1
# define CONFIG_SONAR_SOURCE SONAR_SOURCE_ADC
#elif CONFIG_HAL_BOARD == HAL_BOARD_APM2
# define CONFIG_INS_TYPE CONFIG_INS_MPU6000
# define CONFIG_COMPASS AP_COMPASS_HMC5843
# define CONFIG_PUSHBUTTON DISABLED
# define CONFIG_RELAY DISABLED
# define CONFIG_SONAR_SOURCE SONAR_SOURCE_ANALOG_PIN
# define A_LED_PIN 27
# define B_LED_PIN 26
# define C_LED_PIN 25
@ -100,7 +98,6 @@
# define CONFIG_COMPASS AP_COMPASS_HIL
# define CONFIG_PUSHBUTTON DISABLED
# define CONFIG_RELAY DISABLED
# define CONFIG_SONAR_SOURCE SONAR_SOURCE_ANALOG_PIN
# define A_LED_PIN 27
# define B_LED_PIN 26
# define C_LED_PIN 25
@ -117,7 +114,6 @@
# define CONFIG_COMPASS AP_COMPASS_PX4
# define CONFIG_PUSHBUTTON DISABLED
# define CONFIG_RELAY DISABLED
# define CONFIG_SONAR_SOURCE SONAR_SOURCE_ANALOG_PIN
# define A_LED_PIN 27
# define B_LED_PIN 26
# define C_LED_PIN 25
@ -138,50 +134,6 @@
# define CONFIG_INS_TYPE CONFIG_INS_OILPAN
#endif
//////////////////////////////////////////////////////////////////////////////
// ADC Enable - used to eliminate for systems which don't have ADC.
//
#ifndef CONFIG_ADC
# if CONFIG_INS_TYPE == CONFIG_INS_OILPAN
# define CONFIG_ADC ENABLED
# else
# define CONFIG_ADC DISABLED
# endif
#endif
#ifndef SONAR_TYPE
# define SONAR_TYPE MAX_SONAR_LV // MAX_SONAR_XL,
#endif
//////////////////////////////////////////////////////////////////////////////
// Sonar
//
#ifndef CONFIG_SONAR_SOURCE
# define CONFIG_SONAR_SOURCE SONAR_SOURCE_ADC
#endif
#if CONFIG_SONAR_SOURCE == SONAR_SOURCE_ADC && CONFIG_ADC == DISABLED
# warning Cannot use ADC for CONFIG_SONAR_SOURCE, becaude CONFIG_ADC is DISABLED
# warning Defaulting CONFIG_SONAR_SOURCE to ANALOG_PIN
# undef CONFIG_SONAR_SOURCE
# define CONFIG_SONAR_SOURCE SONAR_SOURCE_ANALOG_PIN
#endif
#if CONFIG_SONAR_SOURCE == SONAR_SOURCE_ADC
# ifndef CONFIG_SONAR_SOURCE_ADC_CHANNEL
# define CONFIG_SONAR_SOURCE_ADC_CHANNEL 7
# endif
#elif CONFIG_SONAR_SOURCE == SONAR_SOURCE_ANALOG_PIN
# ifndef CONFIG_SONAR_SOURCE_ANALOG_PIN
# define CONFIG_SONAR_SOURCE_ANALOG_PIN 0
# endif
#else
# warning Invalid value for CONFIG_SONAR_SOURCE, disabling sonar
# undef SONAR_ENABLED
# define SONAR_ENABLED DISABLED
#endif
//////////////////////////////////////////////////////////////////////////////
// HIL_MODE OPTIONAL
@ -194,8 +146,6 @@
#define GPS_PROTOCOL GPS_PROTOCOL_HIL
#undef CONFIG_INS_TYPE
#define CONFIG_INS_TYPE CONFIG_INS_STUB
#undef CONFIG_ADC
#define CONFIG_ADC DISABLED
#undef CONFIG_COMPASS
#define CONFIG_COMPASS AP_COMPASS_HIL
#endif

3
APMrover2/defines.h
View File

@ -19,9 +19,6 @@
#define SONAR 0
#define BARO 1
#define SONAR_SOURCE_ADC 1
#define SONAR_SOURCE_ANALOG_PIN 2
// CH 7 control
enum ch7_option {
CH7_DO_NOTHING=0,

7
APMrover2/sensors.pde
View File

@ -11,13 +11,6 @@ static void init_sonar(void)
#endif
}
// Sensors are not available in HIL_MODE_ATTITUDE
#if HIL_MODE != HIL_MODE_ATTITUDE
void ReadSCP1000(void) {}
#endif // HIL_MODE != HIL_MODE_ATTITUDE
/*
read and update the battery
*/

8
APMrover2/system.pde
View File

@ -166,9 +166,7 @@ static void init_ardupilot()
}
#endif
#if HIL_MODE != HIL_MODE_ATTITUDE
#if CONFIG_ADC == ENABLED
#if CONFIG_HAL_BOARD == HAL_BOARD_APM1
adc.Init(); // APM ADC library initialization
#endif
@ -185,7 +183,6 @@ static void init_ardupilot()
// initialise sonar
init_sonar();
#endif
// Do GPS init
g_gps = &g_gps_driver;
// GPS initialisation
@ -386,7 +383,6 @@ static void failsafe_trigger(uint8_t failsafe_type, bool on)
static void startup_INS_ground(bool force_accel_level)
{
#if HIL_MODE != HIL_MODE_ATTITUDE
gcs_send_text_P(SEVERITY_MEDIUM, PSTR("Warming up ADC..."));
mavlink_delay(500);
@ -410,8 +406,6 @@ static void startup_INS_ground(bool force_accel_level)
}
ahrs.reset();
#endif // HIL_MODE_ATTITUDE
digitalWrite(B_LED_PIN, LED_ON); // Set LED B high to indicate INS ready
digitalWrite(A_LED_PIN, LED_OFF);
digitalWrite(C_LED_PIN, LED_OFF);

39
APMrover2/test.pde
View File

@ -9,9 +9,6 @@ static int8_t test_radio(uint8_t argc, const Menu::arg *argv);
static int8_t test_passthru(uint8_t argc, const Menu::arg *argv);
static int8_t test_failsafe(uint8_t argc, const Menu::arg *argv);
static int8_t test_gps(uint8_t argc, const Menu::arg *argv);
#if CONFIG_ADC == ENABLED
static int8_t test_adc(uint8_t argc, const Menu::arg *argv);
#endif
static int8_t test_ins(uint8_t argc, const Menu::arg *argv);
static int8_t test_battery(uint8_t argc, const Menu::arg *argv);
static int8_t test_relay(uint8_t argc, const Menu::arg *argv);
@ -40,21 +37,10 @@ static const struct Menu::command test_menu_commands[] PROGMEM = {
// Tests below here are for hardware sensors only present
// when real sensors are attached or they are emulated
#if HIL_MODE == HIL_MODE_DISABLED
#if CONFIG_ADC == ENABLED
{"adc", test_adc},
#endif
{"gps", test_gps},
{"ins", test_ins},
{"sonartest", test_sonar},
{"compass", test_mag},
#elif HIL_MODE == HIL_MODE_SENSORS
{"adc", test_adc},
{"gps", test_gps},
{"ins", test_ins},
{"compass", test_mag},
#elif HIL_MODE == HIL_MODE_ATTITUDE
#endif
{"logging", test_logging},
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
{"shell", test_shell},
@ -355,29 +341,6 @@ test_logging(uint8_t argc, const Menu::arg *argv)
//-------------------------------------------------------------------------------------------
// tests in this section are for real sensors or sensors that have been simulated
#if HIL_MODE == HIL_MODE_DISABLED || HIL_MODE == HIL_MODE_SENSORS
#if CONFIG_ADC == ENABLED
static int8_t
test_adc(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
adc.Init();
delay(1000);
cliSerial->printf_P(PSTR("ADC\n"));
delay(1000);
while(1){
for (int i=0;i<9;i++) cliSerial->printf_P(PSTR("%.1f\t"),adc.Ch(i));
cliSerial->println();
delay(100);
if(cliSerial->available() > 0){
return (0);
}
}
}
#endif // CONFIG_ADC
static int8_t
test_gps(uint8_t argc, const Menu::arg *argv)
{
@ -544,8 +507,6 @@ test_mag(uint8_t argc, const Menu::arg *argv)
return (0);
}
#endif // HIL_MODE == HIL_MODE_DISABLED || HIL_MODE == HIL_MODE_SENSORS
//-------------------------------------------------------------------------------------------
// real sensors that have not been simulated yet go here