mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-10 09:58:28 -04:00
AHRS: adapt ArduCopter for new AHRS framework
This commit is contained in:
parent
c1e4f63907
commit
690ad58a64
@ -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
|
||||
|
@ -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;
|
||||
}
|
||||
|
||||
|
@ -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,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
|
||||
|
@ -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);
|
||||
|
@ -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
|
||||
|
@ -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();
|
||||
}
|
||||
|
||||
|
@ -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()
|
||||
|
@ -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,
|
||||
|
Loading…
Reference in New Issue
Block a user