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

AHRS: adapt ArduCopter for new AHRS framework

This commit is contained in:
Andrew Tridgell 2012-03-11 19:36:12 +11:00
parent 8afd196907
commit 547b4ed55b
9 changed files with 57 additions and 64 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
ArduCopter/ArduCopter.pde
View File

@ -73,8 +73,7 @@ http://code.google.com/p/ardupilot-mega/downloads/list
#include <AP_PeriodicProcess.h> // Parent header of Timer
// (only included for makefile libpath to work)
#include <AP_TimerProcess.h> // TimerProcess is the scheduler for MPU6000 reads.
#include <AP_Quaternion.h> // Madgwick quaternion system
#include <AP_DCM.h> // ArduPilot Mega DCM Library
#include <AP_AHRS.h>
#include <APM_PI.h> // PI library
#include <AC_PID.h> // PID library
#include <RC_Channel.h> // RC Channel Library
@ -235,12 +234,9 @@ AP_IMU_INS imu(&ins);
static GPS *g_gps_null;
#if QUATERNION_ENABLE == ENABLED
// this shouldn't be called dcm of course, but until we
// decide to actually use something else, I don't want the patch
// size to be huge
AP_Quaternion dcm(&imu, g_gps_null);
AP_AHRS_Quaternion ahrs(&imu, g_gps_null);
#else
AP_DCM dcm(&imu, g_gps_null);
AP_AHRS_DCM ahrs(&imu, g_gps_null);
#endif
AP_TimerProcess timer_scheduler;
@ -251,7 +247,7 @@ AP_TimerProcess timer_scheduler;
AP_Compass_HIL compass;
AP_GPS_HIL g_gps_driver(NULL);
AP_IMU_Shim imu;
AP_DCM dcm(&imu, g_gps);
AP_AHRS_DCM ahrs(&imu, g_gps);
AP_PeriodicProcessStub timer_scheduler;
AP_InertialSensor_Stub ins;
@ -259,11 +255,11 @@ AP_TimerProcess timer_scheduler;
#elif HIL_MODE == HIL_MODE_ATTITUDE
AP_ADC_HIL adc;
AP_DCM_HIL dcm;
AP_IMU_Shim imu; // never used
AP_AHRS_HIL ahrs(&imu, g_gps);
AP_GPS_HIL g_gps_driver(NULL);
AP_Compass_HIL compass; // never used
AP_Baro_BMP085_HIL barometer;
AP_IMU_Shim imu; // never used
AP_InertialSensor_Stub ins;
AP_PeriodicProcessStub timer_scheduler;
#ifdef OPTFLOW_ENABLED
@ -984,7 +980,7 @@ static void medium_loop()
if(g.compass_enabled){
if (compass.read()) {
// Calculate heading
Matrix3f m = dcm.get_dcm_matrix();
Matrix3f m = ahrs.get_dcm_matrix();
compass.calculate(m);
compass.null_offsets(m);
}
@ -1297,7 +1293,7 @@ static void update_optical_flow(void)
static int log_counter = 0;
optflow.update();
optflow.update_position(dcm.roll, dcm.pitch, cos_yaw_x, sin_yaw_y, current_loc.alt); // updates internal lon and lat with estimation based on optical flow
optflow.update_position(ahrs.roll, ahrs.pitch, cos_yaw_x, sin_yaw_y, current_loc.alt); // updates internal lon and lat with estimation based on optical flow
// write to log
log_counter++;
@ -1555,7 +1551,7 @@ void update_simple_mode(void)
// which improves speed of function
simple_counter++;
int delta = wrap_360(dcm.yaw_sensor - initial_simple_bearing)/100;
int delta = wrap_360(ahrs.yaw_sensor - initial_simple_bearing)/100;
if (simple_counter == 1){
// roll
@ -1863,17 +1859,17 @@ static void read_AHRS(void)
// Perform IMU calculations and get attitude info
//-----------------------------------------------
#if HIL_MODE != HIL_MODE_DISABLED
// update hil before dcm update
// update hil before ahrs update
gcs_update();
#endif
dcm.update_DCM();
ahrs.update();
omega = imu.get_gyro();
}
static void update_trig(void){
Vector2f yawvector;
Matrix3f temp = dcm.get_dcm_matrix();
Matrix3f temp = ahrs.get_dcm_matrix();
yawvector.x = temp.a.x; // sin
yawvector.y = temp.b.x; // cos

14
ArduCopter/Attitude.pde
View File

@ -4,7 +4,7 @@ static int16_t
get_stabilize_roll(int32_t target_angle)
{
// angle error
target_angle = wrap_180(target_angle - dcm.roll_sensor);
target_angle = wrap_180(target_angle - ahrs.roll_sensor);
#if FRAME_CONFIG == HELI_FRAME
@ -33,7 +33,7 @@ static int16_t
get_stabilize_pitch(int32_t target_angle)
{
// angle error
target_angle = wrap_180(target_angle - dcm.pitch_sensor);
target_angle = wrap_180(target_angle - ahrs.pitch_sensor);
#if FRAME_CONFIG == HELI_FRAME
// limit the error we're feeding to the PID
@ -60,7 +60,7 @@ static int16_t
get_stabilize_yaw(int32_t target_angle)
{
// angle error
target_angle = wrap_180(target_angle - dcm.yaw_sensor);
target_angle = wrap_180(target_angle - ahrs.yaw_sensor);
#if FRAME_CONFIG == HELI_FRAME // cannot use rate control for helicopters
// limit the error we're feeding to the PID
@ -320,13 +320,13 @@ get_nav_yaw_offset(int yaw_input, int reset)
if(reset == 0){
// we are on the ground
return dcm.yaw_sensor;
return ahrs.yaw_sensor;
}else{
// re-define nav_yaw if we have stick input
if(yaw_input != 0){
// set nav_yaw + or - the current location
_yaw = yaw_input + dcm.yaw_sensor;
_yaw = yaw_input + ahrs.yaw_sensor;
// we need to wrap our value so we can be 0 to 360 (*100)
return wrap_360(_yaw);
@ -364,7 +364,7 @@ static int get_z_damping()
float get_world_Z_accel()
{
accels_rot = dcm.get_dcm_matrix() * imu.get_accel();
accels_rot = ahrs.get_dcm_matrix() * imu.get_accel();
//Serial.printf("z %1.4f\n", accels_rot.z);
return accels_rot.z;
}
@ -412,7 +412,7 @@ static float fullDampP = 0.100;
float get_world_Z_accel()
{
accels_rot = dcm.get_dcm_matrix() * imu.get_accel();
accels_rot = ahrs.get_dcm_matrix() * imu.get_accel();
return accels_rot.z;
}

10
ArduCopter/Camera.pde
View File

@ -23,10 +23,10 @@ camera_stabilization()
// Allow user to control camera pitch with channel 6 (mixed with pitch DCM)
if(g.radio_tuning == 0) {
g.rc_camera_pitch.set_pwm(APM_RC.InputCh(CH_6));
g.rc_camera_pitch.servo_out = g.rc_camera_pitch.control_mix(dcm.pitch_sensor);
g.rc_camera_pitch.servo_out = g.rc_camera_pitch.control_mix(ahrs.pitch_sensor);
}else{
// unless channel 6 is already being used for tuning
g.rc_camera_pitch.servo_out = dcm.pitch_sensor * -1;
g.rc_camera_pitch.servo_out = ahrs.pitch_sensor * -1;
}
g.rc_camera_pitch.servo_out = (float)g.rc_camera_pitch.servo_out * g.camera_pitch_gain;
@ -39,14 +39,14 @@ camera_stabilization()
// allow control mixing
/*
g.rc_camera_roll.set_pwm(APM_RC.InputCh(CH_6)); // I'm using CH 6 input here.
g.rc_camera_roll.servo_out = g.rc_camera_roll.control_mix(-dcm.roll_sensor);
g.rc_camera_roll.servo_out = g.rc_camera_roll.control_mix(-ahrs.roll_sensor);
*/
// dont allow control mixing
g.rc_camera_roll.servo_out = (float)-dcm.roll_sensor * g.camera_roll_gain;
g.rc_camera_roll.servo_out = (float)-ahrs.roll_sensor * g.camera_roll_gain;
// limit
//g.rc_camera_roll.servo_out = constrain(-dcm.roll_sensor, -4500, 4500);
//g.rc_camera_roll.servo_out = constrain(-ahrs.roll_sensor, -4500, 4500);
// Output
// ------

38
ArduCopter/GCS_Mavlink.pde
View File

@ -38,9 +38,9 @@ static NOINLINE void send_attitude(mavlink_channel_t chan)
mavlink_msg_attitude_send(
chan,
micros(),
dcm.roll,
dcm.pitch,
dcm.yaw,
ahrs.roll,
ahrs.pitch,
ahrs.yaw,
omega.x,
omega.y,
omega.z);
@ -99,7 +99,7 @@ static void NOINLINE send_meminfo(mavlink_channel_t chan)
static void NOINLINE send_location(mavlink_channel_t chan)
{
Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now
Matrix3f rot = ahrs.get_dcm_matrix(); // neglecting angle of attack for now
mavlink_msg_global_position_int_send(
chan,
current_loc.lat,
@ -125,18 +125,18 @@ static void NOINLINE send_nav_controller_output(mavlink_channel_t chan)
}
#if HIL_MODE != HIL_MODE_ATTITUDE
static void NOINLINE send_dcm(mavlink_channel_t chan)
static void NOINLINE send_ahrs(mavlink_channel_t chan)
{
Vector3f omega_I = dcm.get_gyro_drift();
mavlink_msg_dcm_send(
Vector3f omega_I = ahrs.get_gyro_drift();
mavlink_msg_ahrs_send(
chan,
omega_I.x,
omega_I.y,
omega_I.z,
dcm.get_accel_weight(),
dcm.get_renorm_val(),
dcm.get_error_rp(),
dcm.get_error_yaw());
1,
0,
ahrs.get_error_rp(),
ahrs.get_error_yaw());
}
#endif // HIL_MODE != HIL_MODE_ATTITUDE
@ -275,7 +275,7 @@ static void NOINLINE send_vfr_hud(mavlink_channel_t chan)
chan,
(float)g_gps->ground_speed / 100.0,
(float)g_gps->ground_speed / 100.0,
(dcm.yaw_sensor / 100) % 360,
(ahrs.yaw_sensor / 100) % 360,
g.rc_3.servo_out/10,
current_loc.alt / 100.0,
climb_rate);
@ -471,16 +471,16 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id,
send_statustext(chan);
break;
case MSG_DCM:
case MSG_AHRS:
#if HIL_MODE != HIL_MODE_ATTITUDE
CHECK_PAYLOAD_SIZE(DCM);
send_dcm(chan);
CHECK_PAYLOAD_SIZE(AHRS);
send_ahrs(chan);
#endif
break;
case MSG_SIMSTATE:
#ifdef DESKTOP_BUILD
CHECK_PAYLOAD_SIZE(DCM);
CHECK_PAYLOAD_SIZE(SIMSTATE);
send_simstate(chan);
#endif
break;
@ -736,7 +736,7 @@ GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax)
}
if (freqLoopMatch(streamRateExtra3, freqMin, freqMax)){
send_message(MSG_DCM);
send_message(MSG_AHRS);
send_message(MSG_HWSTATUS);
}
}
@ -1559,8 +1559,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
mavlink_attitude_t packet;
mavlink_msg_attitude_decode(msg, &packet);
// set dcm hil sensor
dcm.setHil(packet.roll,packet.pitch,packet.yaw,packet.rollspeed,
// set AHRS hil sensor
ahrs.setHil(packet.roll,packet.pitch,packet.yaw,packet.rollspeed,
packet.pitchspeed,packet.yawspeed);
// rad/sec

23
ArduCopter/Log.pde
View File

@ -617,11 +617,10 @@ static void Log_Write_Performance()
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_PERFORMANCE_MSG);
DataFlash.WriteByte( dcm.gyro_sat_count); //1
DataFlash.WriteByte( imu.adc_constraints); //2
DataFlash.WriteByte( dcm.renorm_range_count); //3
DataFlash.WriteByte( dcm.renorm_blowup_count); //4
DataFlash.WriteByte( gps_fix_count); //5
DataFlash.WriteByte( imu.adc_constraints); //1
DataFlash.WriteByte( ahrs.renorm_range_count); //2
DataFlash.WriteByte( ahrs.renorm_blowup_count); //3
DataFlash.WriteByte( gps_fix_count); //4
DataFlash.WriteByte(END_BYTE);
}
@ -632,15 +631,13 @@ static void Log_Read_Performance()
int8_t temp2 = DataFlash.ReadByte();
int8_t temp3 = DataFlash.ReadByte();
int8_t temp4 = DataFlash.ReadByte();
int8_t temp5 = DataFlash.ReadByte();
//1 2 3 4 5
Serial.printf_P(PSTR("PM, %d, %d, %d, %d, %d\n"),
//1 2 3 4
Serial.printf_P(PSTR("PM, %d, %d, %d, %d\n"),
(int)temp1,
(int)temp2,
(int)temp3,
(int)temp4,
(int)temp5);
(int)temp4);
}
// Write a command processing packet. Total length : 21 bytes
@ -696,11 +693,11 @@ static void Log_Write_Attitude()
DataFlash.WriteByte(LOG_ATTITUDE_MSG);
DataFlash.WriteInt(g.rc_1.control_in); // 1
DataFlash.WriteInt((int)dcm.roll_sensor); // 2
DataFlash.WriteInt((int)ahrs.roll_sensor); // 2
DataFlash.WriteInt(g.rc_2.control_in); // 3
DataFlash.WriteInt((int)dcm.pitch_sensor); // 4
DataFlash.WriteInt((int)ahrs.pitch_sensor); // 4
DataFlash.WriteInt(g.rc_4.control_in); // 5
DataFlash.WriteInt((uint16_t)dcm.yaw_sensor); // 6
DataFlash.WriteInt((uint16_t)ahrs.yaw_sensor); // 6
DataFlash.WriteInt((uint16_t)(wrap_360(ToDeg(compass.heading)*100))); // 7
DataFlash.WriteByte(END_BYTE);

2
ArduCopter/defines.h
View File

@ -236,7 +236,7 @@ enum ap_message {
MSG_NEXT_WAYPOINT,
MSG_NEXT_PARAM,
MSG_STATUSTEXT,
MSG_DCM,
MSG_AHRS,
MSG_SIMSTATE,
MSG_HWSTATUS,
MSG_RETRY_DEFERRED // this must be last

2
ArduCopter/sensors.pde
View File

@ -93,7 +93,7 @@ static void init_compass()
Serial.println_P(PSTR("COMPASS INIT ERROR"));
return;
}
dcm.set_compass(&compass);
ahrs.set_compass(&compass);
compass.null_offsets_enable();
}

2
ArduCopter/system.pde
View File

@ -565,7 +565,7 @@ static void set_failsafe(boolean mode)
static void
init_simple_bearing()
{
initial_simple_bearing = dcm.yaw_sensor;
initial_simple_bearing = ahrs.yaw_sensor;
}
static void update_throttle_cruise()

2
ArduCopter/test.pde
View File

@ -935,7 +935,7 @@ test_mag(uint8_t argc, const Menu::arg *argv)
while(1){
delay(100);
if (compass.read()) {
compass.calculate(dcm.get_dcm_matrix());
compass.calculate(ahrs.get_dcm_matrix());
Vector3f maggy = compass.get_offsets();
Serial.printf_P(PSTR("Heading: %ld, XYZ: %d, %d, %d\n"),
(wrap_360(ToDeg(compass.heading) * 100)) /100,