Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-09 09:28:31 -04:00
Code Issues Packages Projects Releases Wiki Activity

ArduCopter: allow DMP to run in parallel with DCM

Browse Source 
Parallel DMP can be enabled by #define SECONDARY_DMP_ENABLED in APM_Config.h
New DMP dataflash log type added to allow easy comparison with DCM
This commit is contained in:
rmackay9 2012-09-30 00:25:40 +09:00
parent 670fcbb634
commit 95763e610b
10 changed files with 101 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduCopter/APM_Config.h
View File

@ -15,6 +15,7 @@
//#define MAG_ORIENTATION AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD
//#define HIL_MODE HIL_MODE_ATTITUDE
//#define DMP_ENABLED ENABLED
//#define SECONDARY_DMP_ENABLED ENABLED // allows running DMP in parallel with DCM for testing purposes
//#define FRAME_CONFIG QUAD_FRAME
/*

21
ArduCopter/ArduCopter.pde
View File

@ -272,6 +272,11 @@ AP_AHRS_MPU6000 ahrs(&imu, g_gps, &ins); // only works with APM2
AP_AHRS_DCM ahrs(&imu, g_gps);
#endif
// ahrs2 object is the secondary ahrs to allow running DMP in parallel with DCM
#if SECONDARY_DMP_ENABLED == ENABLED && CONFIG_APM_HARDWARE == APM_HARDWARE_APM2
AP_AHRS_MPU6000 ahrs2(&imu, g_gps, &ins); // only works with APM2
#endif
#elif HIL_MODE == HIL_MODE_SENSORS
// sensor emulators
AP_ADC_HIL adc;
@ -1187,8 +1192,12 @@ static void medium_loop()
}
if(motors.armed()) {
if (g.log_bitmask & MASK_LOG_ATTITUDE_MED)
if (g.log_bitmask & MASK_LOG_ATTITUDE_MED) {
Log_Write_Attitude();
#if SECONDARY_DMP_ENABLED == ENABLED
Log_Write_DMP();
#endif
}
if (g.log_bitmask & MASK_LOG_MOTORS)
Log_Write_Motors();
@ -1288,8 +1297,12 @@ static void fifty_hz_loop()
#endif
# if HIL_MODE == HIL_MODE_DISABLED
if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST && motors.armed())
if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST && motors.armed()) {
Log_Write_Attitude();
#if SECONDARY_DMP_ENABLED == ENABLED
Log_Write_DMP();
#endif
}
if (g.log_bitmask & MASK_LOG_RAW && motors.armed())
Log_Write_Raw();
@ -2089,6 +2102,10 @@ static void read_AHRS(void)
ahrs.update();
omega = imu.get_gyro();
#if SECONDARY_DMP_ENABLED == ENABLED
ahrs2.update();
#endif
}
static void update_trig(void){

46
ArduCopter/Log.pde
View File

@ -942,6 +942,44 @@ static void Log_Read_PID()
temp7); // gain
}
// Write a DMP attitude packet. Total length : 16 bytes
static void Log_Write_DMP()
{
#if SECONDARY_DMP_ENABLED == ENABLED
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_DMP_MSG);
DataFlash.WriteInt((int16_t)ahrs.roll_sensor); // 1
DataFlash.WriteInt((int16_t)ahrs2.roll_sensor); // 2
DataFlash.WriteInt((int16_t)ahrs.pitch_sensor); // 3
DataFlash.WriteInt((int16_t)ahrs2.pitch_sensor); // 4
DataFlash.WriteInt((uint16_t)ahrs.yaw_sensor); // 5
DataFlash.WriteInt((uint16_t)ahrs2.yaw_sensor); // 6
DataFlash.WriteByte(END_BYTE);
#endif
}
// Read an attitude packet
static void Log_Read_DMP()
{
int16_t temp1 = DataFlash.ReadInt();
int16_t temp2 = DataFlash.ReadInt();
int16_t temp3 = DataFlash.ReadInt();
int16_t temp4 = DataFlash.ReadInt();
uint16_t temp5 = DataFlash.ReadInt();
uint16_t temp6 = DataFlash.ReadInt();
// 1 2 3 4 5 6
Serial.printf_P(PSTR("DMP, %d, %d, %d, %d, %u, %u\n"),
(int)temp1,
(int)temp2,
(int)temp3,
(int)temp4,
(unsigned)temp5,
(unsigned)temp6);
}
// Read the DataFlash log memory
static void Log_Read(int16_t start_page, int16_t end_page)
{
@ -1069,6 +1107,10 @@ static int16_t Log_Read_Process(int16_t start_page, int16_t end_page)
case LOG_ITERM_MSG:
Log_Read_Iterm();
break;
case LOG_DMP_MSG:
Log_Read_DMP();
break;
}
break;
case 3:
@ -1122,7 +1164,9 @@ static void Log_Write_Motors() {
}
static void Log_Write_Performance() {
}
static void Log_Write_PID() {
static void Log_Write_PID(int8_t pid_id, int32_t error, int32_t p, int32_t i, int32_t d, int32_t output, float gain) {
}
static void Log_Write_DMP() {
}
static int8_t process_logs(uint8_t argc, const Menu::arg *argv) {
return 0;

9
ArduCopter/Parameters.pde
View File

@ -373,6 +373,15 @@ static void load_parameters(void)
ahrs._kp_yaw.set_and_save(0.1);
}
#if SECONDARY_DMP_ENABLED == ENABLED
if (!ahrs2._kp.load()) {
ahrs2._kp.set(0.1);
}
if (!ahrs2._kp_yaw.load()) {
ahrs2._kp_yaw.set(0.1);
}
#endif
if (!g.format_version.load() ||
g.format_version != Parameters::k_format_version) {

5
ArduCopter/config.h
View File

@ -1061,6 +1061,11 @@
# define DMP_ENABLED DISABLED
#endif
// experimental mpu6000 DMP code
#ifndef SECONDARY_DMP_ENABLED
# define SECONDARY_DMP_ENABLED DISABLED
#endif
#ifndef ALTERNATIVE_YAW_MODE
# define ALTERNATIVE_YAW_MODE DISABLED
#endif

6
ArduCopter/control_modes.pde
View File

@ -165,6 +165,9 @@ static void auto_trim()
if( motors.armed() ) {
// set high AHRS gains to force accelerometers to impact attitude estimate
ahrs.set_fast_gains(true);
#if SECONDARY_DMP_ENABLED == ENABLED
ahrs2.set_fast_gains(true);
#endif
}
trim_accel();
@ -183,6 +186,9 @@ static void auto_trim()
// go back to normal AHRS gains
if( motors.armed() ) {
ahrs.set_fast_gains(false);
#if SECONDARY_DMP_ENABLED == ENABLED
ahrs2.set_fast_gains(false);
#endif
}
//Serial.println("Done");

1
ArduCopter/defines.h
View File

@ -296,6 +296,7 @@ enum gcs_severity {
#define LOG_DATA_MSG 0x0D
#define LOG_PID_MSG 0x0E
#define LOG_ITERM_MSG 0x0F
#define LOG_DMP_MSG 0x10
#define LOG_INDEX_MSG 0xF0
#define MAX_NUM_LOGS 50

6
ArduCopter/motors.pde
View File

@ -161,6 +161,9 @@ static void init_arm_motors()
// go back to normal AHRS gains
ahrs.set_fast_gains(false);
#if SECONDARY_DMP_ENABLED == ENABLED
ahrs2.set_fast_gains(false);
#endif
}
@ -187,6 +190,9 @@ static void init_disarm_motors()
// setup fast AHRS gains to get right attitude
ahrs.set_fast_gains(true);
#if SECONDARY_DMP_ENABLED == ENABLED
ahrs2.set_fast_gains(true);
#endif
}
/*****************************************

3
ArduCopter/sensors.pde
View File

@ -43,6 +43,9 @@ static void init_compass()
return;
}
ahrs.set_compass(&compass);
#if SECONDARY_DMP_ENABLED == ENABLED
ahrs2.set_compass(&compass);
#endif
}
static void init_optflow()

6
ArduCopter/system.pde
View File

@ -367,6 +367,12 @@ static void startup_ground(void)
// setup fast AHRS gains to get right attitude
ahrs.set_fast_gains(true);
#if SECONDARY_DMP_ENABLED == ENABLED
ahrs2.init(&timer_scheduler);
ahrs2.set_as_secondary(true);
ahrs2.set_fast_gains(true);
#endif
// reset the leds
// ---------------------------
clear_leds();