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

Merge branch 'master' of https://code.google.com/p/ardupilot-mega

This commit is contained in:
Chris Anderson 2012-03-19 12:34:56 -07:00
parent 794ae44edf 9f4becbacf
commit 356c248ad7
57 changed files with 1382 additions and 1010 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
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
@ -2210,6 +2206,9 @@ static void update_nav_wp()
nav_lon = g.pid_loiter_rate_lon.get_integrator();
nav_lat = g.pid_loiter_rate_lon.get_integrator();
nav_lon = constrain(nav_lon, -2000, 2000); // 20°
nav_lat = constrain(nav_lat, -2000, 2000); // 20°
// rotate pitch and roll to the copter frame of reference
calc_loiter_pitch_roll();
}

24
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
@ -255,7 +255,6 @@ static void reset_I_all(void)
{
reset_rate_I();
reset_stability_I();
reset_nav_I();
reset_wind_I();
reset_throttle_I();
reset_optflow_I();
@ -282,13 +281,14 @@ static void reset_optflow_I(void)
static void reset_wind_I(void)
{
// Wind Compensation
// this i is not currently being used, but we reset it anyway
// because someone may modify it and not realize it, causing a bug
g.pi_loiter_lat.reset_I();
g.pi_loiter_lon.reset_I();
}
static void reset_nav_I(void)
{
// Rate control for WP navigation
g.pid_loiter_rate_lat.reset_I();
g.pid_loiter_rate_lon.reset_I();
g.pid_nav_lat.reset_I();
g.pid_nav_lon.reset_I();
}
@ -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
// ------

50
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);
}
}
@ -1470,19 +1470,25 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
Log_Write_Data(1, ((AP_Int32 *)vp)->get());
#endif
if (packet.param_value < 0) rounding_addition = -rounding_addition;
((AP_Int32 *)vp)->set_and_save(packet.param_value+rounding_addition);
float v = packet.param_value+rounding_addition;
v = constrain(v, -2147483648, 2147483647);
((AP_Int32 *)vp)->set_and_save(v);
} else if (var_type == AP_PARAM_INT16) {
#if LOGGING_ENABLED == ENABLED
Log_Write_Data(3, (int32_t)((AP_Int16 *)vp)->get());
#endif
if (packet.param_value < 0) rounding_addition = -rounding_addition;
((AP_Int16 *)vp)->set_and_save(packet.param_value+rounding_addition);
float v = packet.param_value+rounding_addition;
v = constrain(v, -32768, 32767);
((AP_Int16 *)vp)->set_and_save(v);
} else if (var_type == AP_PARAM_INT8) {
#if LOGGING_ENABLED == ENABLED
Log_Write_Data(4, (int32_t)((AP_Int8 *)vp)->get());
#endif
if (packet.param_value < 0) rounding_addition = -rounding_addition;
((AP_Int8 *)vp)->set_and_save(packet.param_value+rounding_addition);
float v = packet.param_value+rounding_addition;
v = constrain(v, -128, 127);
((AP_Int8 *)vp)->set_and_save(v);
} else {
// we don't support mavlink set on this parameter
break;
@ -1559,8 +1565,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();
}

4
ArduCopter/system.pde
View File

@ -532,8 +532,6 @@ static void set_mode(byte mode)
reset_nav_params();
// remove the wind compenstaion
reset_wind_I();
// Clears the WP navigation speed compensation
reset_nav_I();
// Clears the alt hold compensation
reset_throttle_I();
}
@ -567,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,

38
ArduPlane/ArduPlane.pde
View File

@ -40,8 +40,7 @@ version 2.1 of the License, or (at your option) any later version.
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <AP_InertialSensor.h> // Inertial Sensor (uncalibated IMU) Library
#include <AP_IMU.h> // ArduPilot Mega IMU Library
#include <AP_DCM.h> // ArduPilot Mega DCM Library
#include <AP_Quaternion.h> // Madgwick quaternion system
#include <AP_AHRS.h> // ArduPilot Mega DCM Library
#include <PID.h> // PID library
#include <RC_Channel.h> // RC Channel Library
#include <AP_RangeFinder.h> // Range finder library
@ -194,12 +193,9 @@ AP_GPS_None g_gps_driver(NULL);
AP_IMU_INS imu( &ins );
#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);
AP_AHRS_Quaternion ahrs(&imu, g_gps);
#else
AP_DCM dcm(&imu, g_gps);
AP_AHRS_DCM ahrs(&imu, g_gps);
#endif
#elif HIL_MODE == HIL_MODE_SENSORS
@ -210,14 +206,14 @@ AP_Compass_HIL compass;
AP_GPS_HIL g_gps_driver(NULL);
AP_InertialSensor_Oilpan ins( &adc );
AP_IMU_Shim imu;
AP_DCM dcm(&imu, g_gps);
AP_AHRS_DCM ahrs(&imu, g_gps);
#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_IMU_Shim imu; // never used
AP_Baro_BMP085_HIL barometer;
#else
@ -261,7 +257,7 @@ AP_Relay relay;
// Camera/Antenna mount tracking and stabilisation stuff
// --------------------------------------
#if MOUNT == ENABLED
AP_Mount camera_mount(g_gps, &dcm);
AP_Mount camera_mount(g_gps, &ahrs);
#endif
@ -719,18 +715,18 @@ static void fast_loop()
}
#if HIL_MODE == HIL_MODE_SENSORS
// update hil before dcm update
// update hil before AHRS update
gcs_update();
#endif
dcm.update_DCM();
ahrs.update();
// 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((int)dcm.roll_sensor, (int)dcm.pitch_sensor, (uint16_t)dcm.yaw_sensor);
Log_Write_Attitude((int)ahrs.roll_sensor, (int)ahrs.pitch_sensor, (uint16_t)ahrs.yaw_sensor);
if (g.log_bitmask & MASK_LOG_RAW)
Log_Write_Raw();
@ -784,19 +780,19 @@ static void medium_loop()
#if HIL_MODE != HIL_MODE_ATTITUDE
if (g.compass_enabled && compass.read()) {
dcm.set_compass(&compass);
ahrs.set_compass(&compass);
// Calculate heading
Matrix3f m = dcm.get_dcm_matrix();
Matrix3f m = ahrs.get_dcm_matrix();
compass.calculate(m);
compass.null_offsets(m);
} else {
dcm.set_compass(NULL);
ahrs.set_compass(NULL);
}
#endif
/*{
Serial.print(dcm.roll_sensor, DEC); Serial.printf_P(PSTR("\t"));
Serial.print(dcm.pitch_sensor, DEC); Serial.printf_P(PSTR("\t"));
Serial.print(dcm.yaw_sensor, DEC); Serial.printf_P(PSTR("\t"));
Serial.print(ahrs.roll_sensor, DEC); Serial.printf_P(PSTR("\t"));
Serial.print(ahrs.pitch_sensor, DEC); Serial.printf_P(PSTR("\t"));
Serial.print(ahrs.yaw_sensor, DEC); Serial.printf_P(PSTR("\t"));
Vector3f tempaccel = imu.get_accel();
Serial.print(tempaccel.x, DEC); Serial.printf_P(PSTR("\t"));
Serial.print(tempaccel.y, DEC); Serial.printf_P(PSTR("\t"));
@ -850,7 +846,7 @@ Serial.println(tempaccel.z, DEC);
#if HIL_MODE != HIL_MODE_ATTITUDE
if ((g.log_bitmask & MASK_LOG_ATTITUDE_MED) && !(g.log_bitmask & MASK_LOG_ATTITUDE_FAST))
Log_Write_Attitude((int)dcm.roll_sensor, (int)dcm.pitch_sensor, (uint16_t)dcm.yaw_sensor);
Log_Write_Attitude((int)ahrs.roll_sensor, (int)ahrs.pitch_sensor, (uint16_t)ahrs.yaw_sensor);
if (g.log_bitmask & MASK_LOG_CTUN)
Log_Write_Control_Tuning();

12
ArduPlane/Attitude.pde
View File

@ -38,7 +38,7 @@ static void stabilize()
// handle this is to ensure both go in the same direction from
// zero
nav_roll += 18000;
if (dcm.roll_sensor < 0) nav_roll -= 36000;
if (ahrs.roll_sensor < 0) nav_roll -= 36000;
}
// For Testing Only
@ -48,11 +48,11 @@ static void stabilize()
// Calculate dersired servo output for the roll
// ---------------------------------------------
g.channel_roll.servo_out = g.pidServoRoll.get_pid((nav_roll - dcm.roll_sensor), delta_ms_fast_loop, speed_scaler);
g.channel_roll.servo_out = g.pidServoRoll.get_pid((nav_roll - ahrs.roll_sensor), delta_ms_fast_loop, speed_scaler);
long tempcalc = nav_pitch +
fabs(dcm.roll_sensor * g.kff_pitch_compensation) +
fabs(ahrs.roll_sensor * g.kff_pitch_compensation) +
(g.channel_throttle.servo_out * g.kff_throttle_to_pitch) -
(dcm.pitch_sensor - g.pitch_trim);
(ahrs.pitch_sensor - g.pitch_trim);
if (inverted_flight) {
// when flying upside down the elevator control is inverted
tempcalc = -tempcalc;
@ -120,7 +120,7 @@ static void stabilize()
static void crash_checker()
{
if(dcm.pitch_sensor < -4500){
if(ahrs.pitch_sensor < -4500){
crash_timer = 255;
}
if(crash_timer > 0)
@ -215,7 +215,7 @@ static void calc_nav_roll()
nav_roll = constrain(nav_roll, -g.roll_limit.get(), g.roll_limit.get());
Vector3f omega;
omega = dcm.get_gyro();
omega = ahrs.get_gyro();
// rate limiter
long rate = degrees(omega.z) * 100; // 3rad = 17188 , 6rad = 34377

56
ArduPlane/GCS_Mavlink.pde
View File

@ -108,13 +108,13 @@ static NOINLINE void send_heartbeat(mavlink_channel_t chan)
static NOINLINE void send_attitude(mavlink_channel_t chan)
{
Vector3f omega = dcm.get_gyro();
Vector3f omega = ahrs.get_gyro();
mavlink_msg_attitude_send(
chan,
micros(),
dcm.roll,
dcm.pitch - radians(g.pitch_trim*0.01),
dcm.yaw,
ahrs.roll,
ahrs.pitch - radians(g.pitch_trim*0.01),
ahrs.yaw,
omega.x,
omega.y,
omega.z);
@ -306,7 +306,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,
millis(),
@ -434,7 +434,7 @@ static void NOINLINE send_vfr_hud(mavlink_channel_t chan)
chan,
(float)airspeed / 100.0,
(float)g_gps->ground_speed / 100.0,
(dcm.yaw_sensor / 100) % 360,
(ahrs.yaw_sensor / 100) % 360,
(uint16_t)(100 * (g.channel_throttle.norm_output() / 2.0 + 0.5)), // scale -1,1 to 0-100
current_loc.alt / 100.0,
0);
@ -486,18 +486,18 @@ static void NOINLINE send_raw_imu3(mavlink_channel_t chan)
imu.ax(), imu.ay(), imu.az());
}
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());
0,
0,
ahrs.get_error_rp(),
ahrs.get_error_yaw());
}
#endif // HIL_MODE != HIL_MODE_ATTITUDE
@ -679,16 +679,16 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id,
break;
#endif
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;
@ -932,7 +932,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);
}
}
@ -1791,13 +1791,19 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
((AP_Float *)vp)->set_and_save(packet.param_value);
} else if (var_type == AP_PARAM_INT32) {
if (packet.param_value < 0) rounding_addition = -rounding_addition;
((AP_Int32 *)vp)->set_and_save(packet.param_value+rounding_addition);
float v = packet.param_value+rounding_addition;
v = constrain(v, -2147483648, 2147483647);
((AP_Int32 *)vp)->set_and_save(v);
} else if (var_type == AP_PARAM_INT16) {
if (packet.param_value < 0) rounding_addition = -rounding_addition;
((AP_Int16 *)vp)->set_and_save(packet.param_value+rounding_addition);
float v = packet.param_value+rounding_addition;
v = constrain(v, -32768, 32767);
((AP_Int16 *)vp)->set_and_save(v);
} else if (var_type == AP_PARAM_INT8) {
if (packet.param_value < 0) rounding_addition = -rounding_addition;
((AP_Int8 *)vp)->set_and_save(packet.param_value+rounding_addition);
float v = packet.param_value+rounding_addition;
v = constrain(v, -128, 127);
((AP_Int8 *)vp)->set_and_save(v);
} else {
// we don't support mavlink set on this parameter
break;
@ -1928,8 +1934,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
#else
// 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);
#endif
@ -1945,8 +1951,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);
break;
}

20
ArduPlane/Log.pde
View File

@ -239,15 +239,15 @@ static void Log_Write_Performance()
DataFlash.WriteLong(millis()- perf_mon_timer);
DataFlash.WriteInt((int16_t)mainLoop_count);
DataFlash.WriteInt(G_Dt_max);
DataFlash.WriteByte(dcm.gyro_sat_count);
DataFlash.WriteByte(0);
DataFlash.WriteByte(imu.adc_constraints);
DataFlash.WriteByte(dcm.renorm_range_count);
DataFlash.WriteByte(dcm.renorm_blowup_count);
DataFlash.WriteByte(ahrs.renorm_range_count);
DataFlash.WriteByte(ahrs.renorm_blowup_count);
DataFlash.WriteByte(gps_fix_count);
DataFlash.WriteInt((int)(dcm.get_health() * 1000));
DataFlash.WriteInt((int)(dcm.get_gyro_drift().x * 1000));
DataFlash.WriteInt((int)(dcm.get_gyro_drift().y * 1000));
DataFlash.WriteInt((int)(dcm.get_gyro_drift().z * 1000));
DataFlash.WriteInt(1); // AHRS health
DataFlash.WriteInt((int)(ahrs.get_gyro_drift().x * 1000));
DataFlash.WriteInt((int)(ahrs.get_gyro_drift().y * 1000));
DataFlash.WriteInt((int)(ahrs.get_gyro_drift().z * 1000));
DataFlash.WriteInt(pmTest1);
DataFlash.WriteByte(END_BYTE);
}
@ -300,10 +300,10 @@ static void Log_Write_Control_Tuning()
DataFlash.WriteByte(LOG_CONTROL_TUNING_MSG);
DataFlash.WriteInt((int)(g.channel_roll.servo_out));
DataFlash.WriteInt((int)nav_roll);
DataFlash.WriteInt((int)dcm.roll_sensor);
DataFlash.WriteInt((int)ahrs.roll_sensor);
DataFlash.WriteInt((int)(g.channel_pitch.servo_out));
DataFlash.WriteInt((int)nav_pitch);
DataFlash.WriteInt((int)dcm.pitch_sensor);
DataFlash.WriteInt((int)ahrs.pitch_sensor);
DataFlash.WriteInt((int)(g.channel_throttle.servo_out));
DataFlash.WriteInt((int)(g.channel_rudder.servo_out));
DataFlash.WriteInt((int)(accel.y * 10000));
@ -317,7 +317,7 @@ static void Log_Write_Nav_Tuning()
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_NAV_TUNING_MSG);
DataFlash.WriteInt((uint16_t)dcm.yaw_sensor);
DataFlash.WriteInt((uint16_t)ahrs.yaw_sensor);
DataFlash.WriteInt((int)wp_distance);
DataFlash.WriteInt((uint16_t)target_bearing);
DataFlash.WriteInt((uint16_t)nav_bearing);

4
ArduPlane/commands_logic.pde
View File

@ -279,7 +279,7 @@ static bool verify_takeoff()
if (hold_course == -1) {
// save our current course to take off
if(g.compass_enabled) {
hold_course = dcm.yaw_sensor;
hold_course = ahrs.yaw_sensor;
} else {
hold_course = g_gps->ground_course;
}
@ -326,7 +326,7 @@ static bool verify_land()
// be quite large at this point, and that could induce a
// sudden large roll correction which is very nasty at
// this point in the landing.
hold_course = dcm.yaw_sensor;
hold_course = ahrs.yaw_sensor;
}
}

2
ArduPlane/defines.h
View File

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

2
ArduPlane/navigation.pde
View File

@ -115,7 +115,7 @@ static void calc_bearing_error()
correction using the GPS typically takes 10 seconds or so
for a 180 degree correction.
*/
bearing_error = nav_bearing - dcm.yaw_sensor;
bearing_error = nav_bearing - ahrs.yaw_sensor;
} else {
// TODO: we need to use the Yaw gyro for in between GPS reads,

13
ArduPlane/system.pde
View File

@ -183,7 +183,7 @@ static void init_ardupilot()
Serial.println_P(PSTR("Compass initialisation failed!"));
g.compass_enabled = false;
} else {
dcm.set_compass(&compass);
ahrs.set_compass(&compass);
compass.null_offsets_enable();
}
}
@ -263,7 +263,7 @@ static void init_ardupilot()
//read_EEPROM_airstart_critical();
#if HIL_MODE != HIL_MODE_ATTITUDE
imu.init(IMU::WARM_START, mavlink_delay, flash_leds, &timer_scheduler);
dcm.set_centripetal(1);
ahrs.set_centripetal(1);
#endif
// This delay is important for the APM_RC library to work.
@ -455,8 +455,8 @@ static void startup_IMU_ground(void)
imu.init(IMU::COLD_START, mavlink_delay, flash_leds, &timer_scheduler);
imu.init_accel(mavlink_delay, flash_leds);
dcm.set_centripetal(1);
dcm.matrix_reset();
ahrs.set_centripetal(1);
ahrs.reset();
// read Baro pressure at ground
//-----------------------------
@ -510,10 +510,9 @@ static void update_GPS_light(void)
static void resetPerfData(void) {
mainLoop_count = 0;
G_Dt_max = 0;
dcm.gyro_sat_count = 0;
imu.adc_constraints = 0;
dcm.renorm_range_count = 0;
dcm.renorm_blowup_count = 0;
ahrs.renorm_range_count = 0;
ahrs.renorm_blowup_count = 0;
gps_fix_count = 0;
pmTest1 = 0;
perf_mon_timer = millis();

22
ArduPlane/test.pde
View File

@ -514,7 +514,7 @@ test_imu(uint8_t argc, const Menu::arg *argv)
{
//Serial.printf_P(PSTR("Calibrating."));
imu.init(IMU::COLD_START, delay, flash_leds, &timer_scheduler);
dcm.matrix_reset();
ahrs.reset();
print_hit_enter();
delay(1000);
@ -528,14 +528,14 @@ test_imu(uint8_t argc, const Menu::arg *argv)
// IMU
// ---
dcm.update_DCM();
ahrs.update();
if(g.compass_enabled) {
medium_loopCounter++;
if(medium_loopCounter == 5){
if (compass.read()) {
// Calculate heading
compass.calculate(dcm.get_dcm_matrix());
compass.calculate(ahrs.get_dcm_matrix());
}
medium_loopCounter = 0;
}
@ -546,9 +546,9 @@ test_imu(uint8_t argc, const Menu::arg *argv)
Vector3f gyros = imu.get_gyro();
Vector3f accels = imu.get_accel();
Serial.printf_P(PSTR("r:%4d p:%4d y:%3d g=(%5.1f %5.1f %5.1f) a=(%5.1f %5.1f %5.1f)\n"),
(int)dcm.roll_sensor / 100,
(int)dcm.pitch_sensor / 100,
(uint16_t)dcm.yaw_sensor / 100,
(int)ahrs.roll_sensor / 100,
(int)ahrs.pitch_sensor / 100,
(uint16_t)ahrs.yaw_sensor / 100,
gyros.x, gyros.y, gyros.z,
accels.x, accels.y, accels.z);
}
@ -574,12 +574,12 @@ test_mag(uint8_t argc, const Menu::arg *argv)
return 0;
}
compass.null_offsets_enable();
dcm.set_compass(&compass);
ahrs.set_compass(&compass);
report_compass();
// we need the DCM initialised for this test
// we need the AHRS initialised for this test
imu.init(IMU::COLD_START, delay, flash_leds, &timer_scheduler);
dcm.matrix_reset();
ahrs.reset();
int counter = 0;
//Serial.printf_P(PSTR("MAG_ORIENTATION: %d\n"), MAG_ORIENTATION);
@ -595,13 +595,13 @@ test_mag(uint8_t argc, const Menu::arg *argv)
// IMU
// ---
dcm.update_DCM();
ahrs.update();
medium_loopCounter++;
if(medium_loopCounter == 5){
if (compass.read()) {
// Calculate heading
Matrix3f m = dcm.get_dcm_matrix();
Matrix3f m = ahrs.get_dcm_matrix();
compass.calculate(m);
compass.null_offsets(m);
}

5
Tools/VARTest/VARTest.pde
View File

@ -15,7 +15,7 @@
#include <SPI.h> // Arduino SPI lib
#include <I2C.h>
#include <DataFlash.h>
#include <AP_DCM.h>
#include <AP_AHRS.h>
#include <AP_ADC.h>
#include <AP_Baro.h>
#include <Filter.h>
@ -25,6 +25,7 @@
#include <AP_IMU.h> // ArduPilot Mega IMU Library
#include <AP_GPS.h>
#include <AP_Math.h>
#include <GCS_MAVLink.h>
#include <config.h>
#include <Parameters.h>
@ -43,7 +44,7 @@ AP_GPS_Auto g_gps_driver(&Serial1, &g_gps);
AP_InertialSensor_Oilpan ins( &adc );
#endif // CONFIG_IMU_TYPE
AP_IMU_INS imu( &ins );
AP_DCM dcm(&imu, g_gps);
AP_AHRS_DCM ahrs(&imu, g_gps);
Arduino_Mega_ISR_Registry isr_registry;
#ifdef DESKTOP_BUILD

109
libraries/AP_AHRS/AP_AHRS.h Normal file
View File

@ -0,0 +1,109 @@
#ifndef AP_AHRS_H
#define AP_AHRS_H
/*
AHRS (Attitude Heading Reference System) interface for ArduPilot
This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.
*/
#include <AP_Math.h>
#include <inttypes.h>
#include <AP_Compass.h>
#include <AP_GPS.h>
#include <AP_IMU.h>
#if defined(ARDUINO) && ARDUINO >= 100
#include "Arduino.h"
#else
#include "WProgram.h"
#endif
class AP_AHRS
{
public:
// Constructor
AP_AHRS(IMU *imu, GPS *&gps):
_imu(imu),
_gps(gps)
{
// base the ki values by the sensors maximum drift
// rate. The APM2 has gyros which are much less drift
// prone than the APM1, so we should have a lower ki,
// which will make us less prone to increasing omegaI
// incorrectly due to sensor noise
_gyro_drift_limit = imu->get_gyro_drift_rate();
}
// Accessors
void set_centripetal(bool b) { _centripetal = b; }
bool get_centripetal(void) { return _centripetal; }
void set_compass(Compass *compass) { _compass = compass; }
// Methods
virtual void update(void) = 0;
// Euler angles (radians)
float roll;
float pitch;
float yaw;
// integer Euler angles (Degrees * 100)
int32_t roll_sensor;
int32_t pitch_sensor;
int32_t yaw_sensor;
// return a smoothed and corrected gyro vector
virtual Vector3f get_gyro(void) = 0;
// return the current estimate of the gyro drift
virtual Vector3f get_gyro_drift(void) = 0;
// reset the current attitude, used on new IMU calibration
virtual void reset(bool recover_eulers=false) = 0;
// how often our attitude representation has gone out of range
uint8_t renorm_range_count;
// how often our attitude representation has blown up completely
uint8_t renorm_blowup_count;
// return the average size of the roll/pitch error estimate
// since last call
virtual float get_error_rp(void) = 0;
// return the average size of the yaw error estimate
// since last call
virtual float get_error_yaw(void) = 0;
// return a DCM rotation matrix representing our current
// attitude
virtual Matrix3f get_dcm_matrix(void) = 0;
protected:
// pointer to compass object, if enabled
Compass * _compass;
// time in microseconds of last compass update
uint32_t _compass_last_update;
// note: we use ref-to-pointer here so that our caller can change the GPS without our noticing
// IMU under us without our noticing.
GPS *&_gps;
IMU *_imu;
// true if we are doing centripetal acceleration correction
bool _centripetal;
// the limit of the gyro drift claimed by the sensors, in
// radians/s/s
float _gyro_drift_limit;
};
#include <AP_AHRS_DCM.h>
#include <AP_AHRS_Quaternion.h>
#include <AP_AHRS_HIL.h>
#endif // AP_AHRS_H

137
libraries/AP_DCM/AP_DCM.cpp → libraries/AP_AHRS/AP_AHRS_DCM.cpp
View File

@ -1,22 +1,19 @@
/*
APM_DCM.cpp - DCM AHRS Library, fixed wing version, for Ardupilot Mega
Code by Doug Weibel, Jordi Muñoz and Jose Julio. DIYDrones.com
APM_AHRS_DCM.cpp
This library works with the ArduPilot Mega and "Oilpan"
AHRS system using DCM matrices
Based on DCM code by Doug Weibel, Jordi Muñoz and Jose Julio. DIYDrones.com
Adapted for the general ArduPilot AHRS interface by Andrew Tridgell
This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.
Methods:
update_DCM() : Updates the AHRS by integrating the rotation matrix over time using the IMU object data
get_gyro() : Returns gyro vector corrected for bias
get_accel() : Returns accelerometer vector
get_dcm_matrix() : Returns dcm matrix
modify it under the terms of the GNU Lesser General Public License
as published by the Free Software Foundation; either version 2.1
of the License, or (at your option) any later version.
*/
#include <AP_DCM.h>
#include <FastSerial.h>
#include <AP_AHRS.h>
// this is the speed in cm/s above which we first get a yaw lock with
// the GPS
@ -26,21 +23,11 @@
// from the GPS and wait for the ground speed to get above GPS_SPEED_MIN
#define GPS_SPEED_RESET 100
void
AP_DCM::set_compass(Compass *compass)
{
_compass = compass;
}
// run a full DCM update round
// the drift_correction_frequency is how many steps of the algorithm
// to run before doing an accelerometer and yaw drift correction step
void
AP_DCM::update_DCM(uint8_t drift_correction_frequency)
AP_AHRS_DCM::update(void)
{
float delta_t;
Vector3f accel;
// tell the IMU to grab some data
_imu->update();
@ -50,43 +37,16 @@ AP_DCM::update_DCM(uint8_t drift_correction_frequency)
// Get current values for gyros
_gyro_vector = _imu->get_gyro();
// accumulate some more accelerometer data
accel = _imu->get_accel();
_accel_sum += accel;
_drift_correction_time += delta_t;
_accel_vector = _imu->get_accel();
// Integrate the DCM matrix using gyro inputs
matrix_update(delta_t);
// add up the omega vector so we pass a value to the drift
// correction averaged over the same time period as the
// accelerometers
_omega_sum += _omega_integ_corr;
// Normalize the DCM matrix
normalize();
// see if we will perform drift correction on this call
_drift_correction_count++;
if (_drift_correction_count >= drift_correction_frequency) {
// calculate the average accelerometer vector over
// since the last drift correction call
float scale = 1.0 / _drift_correction_count;
_accel_vector = _accel_sum * scale;
_accel_sum.zero();
// calculate the average omega value over this time
_omega_smoothed = _omega_sum * scale;
_omega_sum.zero();
// Perform drift correction
drift_correction(_drift_correction_time);
_drift_correction_time = 0;
_drift_correction_count = 0;
}
drift_correction(delta_t);
// paranoid check for bad values in the DCM matrix
check_matrix();
@ -97,7 +57,7 @@ AP_DCM::update_DCM(uint8_t drift_correction_frequency)
// update the DCM matrix using only the gyros
void
AP_DCM::matrix_update(float _G_Dt)
AP_AHRS_DCM::matrix_update(float _G_Dt)
{
// _omega_integ_corr is used for _centripetal correction
// (theoretically better than _omega)
@ -106,33 +66,22 @@ AP_DCM::matrix_update(float _G_Dt)
// Equation 16, adding proportional and integral correction terms
_omega = _omega_integ_corr + _omega_P + _omega_yaw_P;
// this is an expansion of the DCM matrix multiply (equation
// this is a replacement of the DCM matrix multiply (equation
// 17), with known zero elements removed and the matrix
// operations inlined. This runs much faster than the original
// version of this code, as the compiler was doing a terrible
// job of realising that so many of the factors were in common
// or zero. It also uses much less stack, as we no longer need
// additional local matrices
// two additional local matrices
float tmpx = _G_Dt * _omega.x;
float tmpy = _G_Dt * _omega.y;
float tmpz = _G_Dt * _omega.z;
_dcm_matrix.a.x += _dcm_matrix.a.y * tmpz - _dcm_matrix.a.z * tmpy;
_dcm_matrix.a.y += _dcm_matrix.a.z * tmpx - _dcm_matrix.a.x * tmpz;
_dcm_matrix.a.z += _dcm_matrix.a.x * tmpy - _dcm_matrix.a.y * tmpx;
_dcm_matrix.b.x += _dcm_matrix.b.y * tmpz - _dcm_matrix.b.z * tmpy;
_dcm_matrix.b.y += _dcm_matrix.b.z * tmpx - _dcm_matrix.b.x * tmpz;
_dcm_matrix.b.z += _dcm_matrix.b.x * tmpy - _dcm_matrix.b.y * tmpx;
_dcm_matrix.c.x += _dcm_matrix.c.y * tmpz - _dcm_matrix.c.z * tmpy;
_dcm_matrix.c.y += _dcm_matrix.c.z * tmpx - _dcm_matrix.c.x * tmpz;
_dcm_matrix.c.z += _dcm_matrix.c.x * tmpy - _dcm_matrix.c.y * tmpx;
Vector3f r = _omega * _G_Dt;
_dcm_matrix.rotate(r);
}
// adjust an accelerometer vector for known acceleration forces
void
AP_DCM::accel_adjust(Vector3f &accel)
AP_AHRS_DCM::accel_adjust(Vector3f &accel)
{
float veloc;
// compensate for linear acceleration. This makes a
@ -149,8 +98,8 @@ AP_DCM::accel_adjust(Vector3f &accel)
// direction as positive
// Equation 26 broken up into separate pieces
accel.y -= _omega_smoothed.z * veloc;
accel.z += _omega_smoothed.y * veloc;
accel.y -= _omega_integ_corr.z * veloc;
accel.z += _omega_integ_corr.y * veloc;
}
/*
@ -158,7 +107,7 @@ AP_DCM::accel_adjust(Vector3f &accel)
extreme errors in the matrix
*/
void
AP_DCM::matrix_reset(bool recover_eulers)
AP_AHRS_DCM::reset(bool recover_eulers)
{
if (_compass != NULL) {
_compass->null_offsets_disable();
@ -169,7 +118,6 @@ AP_DCM::matrix_reset(bool recover_eulers)
_omega_P.zero();
_omega_yaw_P.zero();
_omega_integ_corr.zero();
_omega_smoothed.zero();
_omega.zero();
// if the caller wants us to try to recover to the current
@ -193,13 +141,13 @@ AP_DCM::matrix_reset(bool recover_eulers)
check the DCM matrix for pathological values
*/
void
AP_DCM::check_matrix(void)
AP_AHRS_DCM::check_matrix(void)
{
if (_dcm_matrix.is_nan()) {
//Serial.printf("ERROR: DCM matrix NAN\n");
SITL_debug("ERROR: DCM matrix NAN\n");
renorm_blowup_count++;
matrix_reset(true);
reset(true);
return;
}
// some DCM matrix values can lead to an out of range error in
@ -221,7 +169,7 @@ AP_DCM::check_matrix(void)
SITL_debug("ERROR: DCM matrix error. _dcm_matrix.c.x=%f\n",
_dcm_matrix.c.x);
renorm_blowup_count++;
matrix_reset(true);
reset(true);
}
}
}
@ -229,7 +177,7 @@ AP_DCM::check_matrix(void)
// renormalise one vector component of the DCM matrix
// this will return false if renormalization fails
bool
AP_DCM::renorm(Vector3f const &a, Vector3f &result)
AP_AHRS_DCM::renorm(Vector3f const &a, Vector3f &result)
{
float renorm_val;
@ -289,7 +237,7 @@ simple matter to stay ahead of it.
We call the process of enforcing the orthogonality conditions ÒrenormalizationÓ.
*/
void
AP_DCM::normalize(void)
AP_AHRS_DCM::normalize(void)
{
float error;
Vector3f t0, t1, t2;
@ -305,7 +253,7 @@ AP_DCM::normalize(void)
!renorm(t2, _dcm_matrix.c)) {
// Our solution is blowing up and we will force back
// to last euler angles
matrix_reset(true);
reset(true);
}
}
@ -319,7 +267,7 @@ AP_DCM::normalize(void)
// This function also updates _omega_yaw_P with a yaw correction term
// from our yaw reference vector
void
AP_DCM::drift_correction(float deltat)
AP_AHRS_DCM::drift_correction(float deltat)
{
float error_course = 0;
Vector3f accel;
@ -530,7 +478,7 @@ AP_DCM::drift_correction(float deltat)
// calculate the euler angles which will be used for high level
// navigation control
void
AP_DCM::euler_angles(void)
AP_AHRS_DCM::euler_angles(void)
{
_dcm_matrix.to_euler(&roll, &pitch, &yaw);
@ -544,27 +492,8 @@ AP_DCM::euler_angles(void)
/* reporting of DCM state for MAVLink */
// average accel_weight since last call
float AP_DCM::get_accel_weight(void)
{
return 1.0;
}
// average renorm_val since last call
float AP_DCM::get_renorm_val(void)
{
float ret;
if (_renorm_val_count == 0) {
return 0;
}
ret = _renorm_val_sum / _renorm_val_count;
_renorm_val_sum = 0;
_renorm_val_count = 0;
return ret;
}
// average error_roll_pitch since last call
float AP_DCM::get_error_rp(void)
float AP_AHRS_DCM::get_error_rp(void)
{
float ret;
if (_error_rp_count == 0) {
@ -577,7 +506,7 @@ float AP_DCM::get_error_rp(void)
}
// average error_yaw since last call
float AP_DCM::get_error_yaw(void)
float AP_AHRS_DCM::get_error_yaw(void)
{
float ret;
if (_error_yaw_count == 0) {

85
libraries/AP_AHRS/AP_AHRS_DCM.h Normal file
View File

@ -0,0 +1,85 @@
#ifndef AP_AHRS_DCM_H
#define AP_AHRS_DCM_H
/*
DCM based AHRS (Attitude Heading Reference System) interface for
ArduPilot
This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.
*/
class AP_AHRS_DCM : public AP_AHRS
{
public:
// Constructors
AP_AHRS_DCM(IMU *imu, GPS *&gps) : AP_AHRS(imu, gps)
{
_kp_roll_pitch = 0.13;
_kp_yaw = 0.4;
_dcm_matrix(Vector3f(1, 0, 0),
Vector3f(0, 1, 0),
Vector3f(0, 0, 1));
// base the ki values on the sensors drift rate
_ki_roll_pitch = _gyro_drift_limit * 5;
_ki_yaw = _gyro_drift_limit * 8;
}
// return the smoothed gyro vector corrected for drift
Vector3f get_gyro(void) {return _omega; }
Matrix3f get_dcm_matrix(void) {return _dcm_matrix; }
// return the current drift correction integrator value
Vector3f get_gyro_drift(void) {return _omega_I; }
// Methods
void update(void);
void reset(bool recover_eulers = false);
// status reporting
float get_error_rp(void);
float get_error_yaw(void);
private:
float _kp_roll_pitch;
float _ki_roll_pitch;
float _kp_yaw;
float _ki_yaw;
bool _have_initial_yaw;
// Methods
void accel_adjust(Vector3f &accel);
void matrix_update(float _G_Dt);
void normalize(void);
void check_matrix(void);
bool renorm(Vector3f const &a, Vector3f &result);
void drift_correction(float deltat);
void euler_angles(void);
// primary representation of attitude
Matrix3f _dcm_matrix;
Vector3f _gyro_vector; // Store the gyros turn rate in a vector
Vector3f _accel_vector; // current accel vector
Vector3f _omega_P; // accel Omega Proportional correction
Vector3f _omega_yaw_P; // yaw Omega Proportional correction
Vector3f _omega_I; // Omega Integrator correction
Vector3f _omega_integ_corr; // Partially corrected Gyro_Vector data - used for centrepetal correction
Vector3f _omega; // Corrected Gyro_Vector data
// state to support status reporting
float _renorm_val_sum;
uint16_t _renorm_val_count;
float _error_rp_sum;
uint16_t _error_rp_count;
float _error_yaw_sum;
uint16_t _error_yaw_count;
// time in millis when we last got a GPS heading
uint32_t _gps_last_update;
};
#endif // AP_AHRS_DCM_H

30
libraries/AP_AHRS/AP_AHRS_HIL.cpp Normal file
View File

@ -0,0 +1,30 @@
/*
APM_AHRS_HIL.cpp
Hardware in the loop AHRS object
This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later
version.
*/
#include <FastSerial.h>
#include <AP_AHRS.h>
/**************************************************/
void
AP_AHRS_HIL::setHil(float _roll, float _pitch, float _yaw,
float _rollRate, float _pitchRate, float _yawRate)
{
roll = _roll;
pitch = _pitch;
yaw = _yaw;
_omega(_rollRate, _pitchRate, _yawRate);
roll_sensor = ToDeg(roll)*100;
pitch_sensor = ToDeg(pitch)*100;
yaw_sensor = ToDeg(yaw)*100;
}

39
libraries/AP_AHRS/AP_AHRS_HIL.h Normal file
View File

@ -0,0 +1,39 @@
#ifndef AP_AHRS_HIL_H
#define AP_AHRS_HIL_H
class AP_AHRS_HIL : public AP_AHRS
{
public:
// Constructors
AP_AHRS_HIL(IMU *imu, GPS *&gps) : AP_AHRS(imu, gps)
{
}
// Accessors
Vector3f get_gyro(void) {return _omega; }
Matrix3f get_dcm_matrix(void) {
Matrix3f m;
m.from_euler(roll, pitch, yaw);
return m;
}
// Methods
void update(void) {}
void setHil(float roll, float pitch, float yaw,
float rollRate, float pitchRate, float yawRate);
// return the current estimate of the gyro drift
Vector3f get_gyro_drift(void) { return Vector3f(0,0,0); }
// reset the current attitude, used on new IMU calibration
void reset(bool recover_eulers=false) {}
float get_error_rp(void) { return 0; }
float get_error_yaw(void) { return 0; }
private:
Vector3f _omega;
};
#endif

254
libraries/AP_Quaternion/AP_Quaternion.cpp → libraries/AP_AHRS/AP_AHRS_Quaternion.cpp
View File

@ -1,5 +1,5 @@
/*
AP_Quaternion code, based on quaternion code from Jeb Madgwick
AP_AHRS_Quaternion code, based on quaternion code from Jeb Madgwick
See http://www.x-io.co.uk/res/doc/madgwick_internal_report.pdf
@ -13,24 +13,7 @@
version.
*/
#include <FastSerial.h>
#include <AP_Quaternion.h>
#define ToRad(x) (x*0.01745329252) // *pi/180
#define ToDeg(x) (x*57.2957795131) // *180/pi
// this is the speed in cm/s above which we first get a yaw lock with
// the GPS
#define GPS_SPEED_MIN 300
// this is the speed in cm/s at which we stop using drift correction
// from the GPS and wait for the ground speed to get above GPS_SPEED_MIN
#define GPS_SPEED_RESET 100
void
AP_Quaternion::set_compass(Compass *compass)
{
_compass = compass;
}
#include <AP_AHRS.h>
// to keep the code as close to the original as possible, we use these
// macros for quaternion access
@ -40,7 +23,7 @@ AP_Quaternion::set_compass(Compass *compass)
#define SEq_4 q.q4
// Function to compute one quaternion iteration without magnetometer
void AP_Quaternion::update_IMU(float deltat, Vector3f &gyro, Vector3f &accel)
void AP_AHRS_Quaternion::update_IMU(float deltat, Vector3f &gyro, Vector3f &accel)
{
// Local system variables
float norm; // vector norm
@ -134,7 +117,7 @@ void AP_Quaternion::update_IMU(float deltat, Vector3f &gyro, Vector3f &accel)
// Function to compute one quaternion iteration including magnetometer
void AP_Quaternion::update_MARG(float deltat, Vector3f &gyro, Vector3f &accel, Vector3f &mag)
void AP_AHRS_Quaternion::update_MARG(float deltat, Vector3f &gyro, Vector3f &accel, Vector3f &mag)
{
// local system variables
float norm; // vector norm
@ -253,7 +236,7 @@ void AP_Quaternion::update_MARG(float deltat, Vector3f &gyro, Vector3f &accel, V
// don't allow the drift rate to be exceeded. This prevents a
// sudden drift change coming from a outage in the compass
float max_change = gyroMeasDrift * deltat;
float max_change = _gyro_drift_limit * deltat;
drift_delta.x = constrain(drift_delta.x, -max_change, max_change);
drift_delta.y = constrain(drift_delta.y, -max_change, max_change);
drift_delta.z = constrain(drift_delta.z, -max_change, max_change);
@ -308,161 +291,8 @@ void AP_Quaternion::update_MARG(float deltat, Vector3f &gyro, Vector3f &accel, V
}
// Function to update our gyro_bias drift estimate using the accelerometer
// and magnetometer. This is a cut-down version of update_MARG(), but
// without the quaternion update.
void AP_Quaternion::update_drift(float deltat, Vector3f &mag)
{
// local system variables
float norm; // vector norm
float f_4, f_5, f_6; // objective function elements
float J_11or24, J_12or23, J_13or22, J_14or21, J_32, J_33, // objective function Jacobian elements
J_41, J_42, J_43, J_44, J_51, J_52, J_53, J_54, J_61, J_62, J_63, J_64; //
float SEqHatDot_1, SEqHatDot_2, SEqHatDot_3, SEqHatDot_4; // estimated direction of the gyroscope error
// computed flux in the earth frame
Vector3f flux;
// estimated direction of the gyroscope error (radians)
Vector3f w_err;
// normalise the magnetometer measurement
mag.normalize();
if (mag.is_inf()) {
// discard this data point
renorm_range_count++;
return;
}
// auxiliary variables to avoid repeated calculations
float twoSEq_1 = 2.0f * SEq_1;
float twoSEq_2 = 2.0f * SEq_2;
float twoSEq_3 = 2.0f * SEq_3;
float twoSEq_4 = 2.0f * SEq_4;
float twob_x = 2.0f * b_x;
float twob_z = 2.0f * b_z;
float twob_xSEq_1 = 2.0f * b_x * SEq_1;
float twob_xSEq_2 = 2.0f * b_x * SEq_2;
float twob_xSEq_3 = 2.0f * b_x * SEq_3;
float twob_xSEq_4 = 2.0f * b_x * SEq_4;
float twob_zSEq_1 = 2.0f * b_z * SEq_1;
float twob_zSEq_2 = 2.0f * b_z * SEq_2;
float twob_zSEq_3 = 2.0f * b_z * SEq_3;
float twob_zSEq_4 = 2.0f * b_z * SEq_4;
float SEq_1SEq_2;
float SEq_1SEq_3 = SEq_1 * SEq_3;
float SEq_1SEq_4;
float SEq_2SEq_3;
float SEq_2SEq_4 = SEq_2 * SEq_4;
float SEq_3SEq_4;
Vector3f twom = mag * 2.0;
// compute the objective function and Jacobian
f_4 = twob_x * (0.5f - SEq_3 * SEq_3 - SEq_4 * SEq_4) + twob_z * (SEq_2SEq_4 - SEq_1SEq_3) - mag.x;
f_5 = twob_x * (SEq_2 * SEq_3 - SEq_1 * SEq_4) + twob_z * (SEq_1 * SEq_2 + SEq_3 * SEq_4) - mag.y;
f_6 = twob_x * (SEq_1SEq_3 + SEq_2SEq_4) + twob_z * (0.5f - SEq_2 * SEq_2 - SEq_3 * SEq_3) - mag.z;
J_11or24 = twoSEq_3; // J_11 negated in matrix multiplication
J_12or23 = 2.0f * SEq_4;
J_13or22 = twoSEq_1; // J_12 negated in matrix multiplication
J_14or21 = twoSEq_2;
J_32 = 2.0f * J_14or21; // negated in matrix multiplication
J_33 = 2.0f * J_11or24; // negated in matrix multiplication
J_41 = twob_zSEq_3; // negated in matrix multiplication
J_42 = twob_zSEq_4;
J_43 = 2.0f * twob_xSEq_3 + twob_zSEq_1; // negated in matrix multiplication
J_44 = 2.0f * twob_xSEq_4 - twob_zSEq_2; // negated in matrix multiplication
J_51 = twob_xSEq_4 - twob_zSEq_2; // negated in matrix multiplication
J_52 = twob_xSEq_3 + twob_zSEq_1;
J_53 = twob_xSEq_2 + twob_zSEq_4;
J_54 = twob_xSEq_1 - twob_zSEq_3; // negated in matrix multiplication
J_61 = twob_xSEq_3;
J_62 = twob_xSEq_4 - 2.0f * twob_zSEq_2;
J_63 = twob_xSEq_1 - 2.0f * twob_zSEq_3;
J_64 = twob_xSEq_2;
// compute the gradient (matrix multiplication)
SEqHatDot_1 = - J_41 * f_4 - J_51 * f_5 + J_61 * f_6;
SEqHatDot_2 = + J_42 * f_4 + J_52 * f_5 + J_62 * f_6;
SEqHatDot_3 = - J_43 * f_4 + J_53 * f_5 + J_63 * f_6;
SEqHatDot_4 = - J_44 * f_4 - J_54 * f_5 + J_64 * f_6;
// normalise the gradient to estimate direction of the gyroscope error
norm = 1.0 / safe_sqrt(SEqHatDot_1 * SEqHatDot_1 + SEqHatDot_2 * SEqHatDot_2 + SEqHatDot_3 * SEqHatDot_3 + SEqHatDot_4 * SEqHatDot_4);
if (isinf(norm)) {
// discard this data point
renorm_range_count++;
return;
}
SEqHatDot_1 *= norm;
SEqHatDot_2 *= norm;
SEqHatDot_3 *= norm;
SEqHatDot_4 *= norm;
// compute angular estimated direction of the gyroscope error
w_err.x = twoSEq_1 * SEqHatDot_2 - twoSEq_2 * SEqHatDot_1 - twoSEq_3 * SEqHatDot_4 + twoSEq_4 * SEqHatDot_3;
w_err.y = twoSEq_1 * SEqHatDot_3 + twoSEq_2 * SEqHatDot_4 - twoSEq_3 * SEqHatDot_1 - twoSEq_4 * SEqHatDot_2;
w_err.z = twoSEq_1 * SEqHatDot_4 - twoSEq_2 * SEqHatDot_3 + twoSEq_3 * SEqHatDot_2 - twoSEq_4 * SEqHatDot_1;
// keep track of the error rates
_error_rp_sum += 0.5*(fabs(w_err.x) + fabs(w_err.y));
_error_yaw_sum += fabs(w_err.z);
_error_rp_count++;
_error_yaw_count++;
// compute the gyroscope bias delta
Vector3f drift_delta = w_err * (deltat * zeta);
// don't allow the drift rate to be exceeded. This prevents a
// sudden drift change coming from a outage in the compass
float max_change = gyroMeasDrift * deltat;
drift_delta.x = constrain(drift_delta.x, -max_change, max_change);
drift_delta.y = constrain(drift_delta.y, -max_change, max_change);
drift_delta.z = constrain(drift_delta.z, -max_change, max_change);
gyro_bias += drift_delta;
// compute then integrate the estimated quaternion rate
SEq_1 -= (beta * SEqHatDot_1) * deltat;
SEq_2 -= (beta * SEqHatDot_2) * deltat;
SEq_3 -= (beta * SEqHatDot_3) * deltat;
SEq_4 -= (beta * SEqHatDot_4) * deltat;
// normalise quaternion
norm = 1.0/safe_sqrt(SEq_1 * SEq_1 + SEq_2 * SEq_2 + SEq_3 * SEq_3 + SEq_4 * SEq_4);
if (isinf(norm)) {
// our quaternion is bad! Reset based on roll/pitch/yaw
// and hope for the best ...
renorm_blowup_count++;
_compass->null_offsets_disable();
q.from_euler(roll, pitch, yaw);
_compass->null_offsets_disable();
return;
}
SEq_1 *= norm;
SEq_2 *= norm;
SEq_3 *= norm;
SEq_4 *= norm;
// compute flux in the earth frame
// recompute axulirary variables
SEq_1SEq_2 = SEq_1 * SEq_2;
SEq_1SEq_3 = SEq_1 * SEq_3;
SEq_1SEq_4 = SEq_1 * SEq_4;
SEq_3SEq_4 = SEq_3 * SEq_4;
SEq_2SEq_3 = SEq_2 * SEq_3;
SEq_2SEq_4 = SEq_2 * SEq_4;
flux.x = twom.x * (0.5f - SEq_3 * SEq_3 - SEq_4 * SEq_4) + twom.y * (SEq_2SEq_3 - SEq_1SEq_4) + twom.z * (SEq_2SEq_4 + SEq_1SEq_3);
flux.y = twom.x * (SEq_2SEq_3 + SEq_1SEq_4) + twom.y * (0.5f - SEq_2 * SEq_2 - SEq_4 * SEq_4) + twom.z * (SEq_3SEq_4 - SEq_1SEq_2);
flux.z = twom.x * (SEq_2SEq_4 - SEq_1SEq_3) + twom.y * (SEq_3SEq_4 + SEq_1SEq_2) + twom.z * (0.5f - SEq_2 * SEq_2 - SEq_3 * SEq_3);
// normalise the flux vector to have only components in the x and z
b_x = sqrt((flux.x * flux.x) + (flux.y * flux.y));
b_z = flux.z;
}
// Function to compute one quaternion iteration
void AP_Quaternion::update(void)
void AP_AHRS_Quaternion::update(void)
{
Vector3f gyro, accel;
float deltat;
@ -491,22 +321,16 @@ void AP_Quaternion::update(void)
// get current IMU state
gyro = _imu->get_gyro();
accel = _imu->get_accel();
// Quaternion code uses opposite x and y gyro sense from the
// rest of APM
gyro.x = -gyro.x;
gyro.y = -gyro.y;
// Quaternion code uses opposite z accel as well
accel.z = -accel.z;
// the quaternion system uses opposite sign for accel
accel = - _imu->get_accel();
if (_centripetal && _gps && _gps->status() == GPS::GPS_OK) {
// compensate for linear acceleration. This makes a
// surprisingly large difference in the pitch estimate when
// turning, plus on takeoff and landing
float acceleration = _gps->acceleration();
accel.x -= acceleration;
accel.x += acceleration;
// compensate for centripetal acceleration
float veloc;
@ -514,45 +338,18 @@ void AP_Quaternion::update(void)
// be careful of the signs in this calculation. the
// quaternion system uses different signs than the
// rest of APM
accel.y -= (gyro.z - gyro_bias.z) * veloc;
accel.z += (gyro.y - gyro_bias.y) * veloc;
}
#define SEPARATE_DRIFT 0
#if SEPARATE_DRIFT
/*
The full Madgwick quaternion 'MARG' system assumes you get
gyro, accel and magnetometer updates at the same rate. On
APM we get them at very different rates, and re-calculating
our drift due to the magnetometer in the fast loop is very
wasteful of CPU.
Instead, we only update the gyro_bias vector when we get a
new magnetometer point, and use the much simpler
update_IMU() as the main quaternion update function.
*/
_gyro_sum += gyro;
_accel_sum += accel;
_sum_count++;
if (_compass != NULL && _compass->use_for_yaw() &&
_compass->last_update != _compass_last_update &&
_sum_count != 0) {
// use new compass sample for drift correction
float mag_deltat = 1.0e-6*(_compass->last_update - _compass_last_update);
Vector3f mag = Vector3f(_compass->mag_x, _compass->mag_y, - _compass->mag_z);
_sum_count = 0;
update_drift(mag_deltat, mag);
_compass_last_update = _compass->last_update;
accel.y += (gyro.z - gyro_bias.z) * veloc;
accel.z -= (gyro.y - gyro_bias.y) * veloc;
}
if (_compass != NULL && _compass->use_for_yaw()) {
Vector3f mag = Vector3f(_compass->mag_x, _compass->mag_y, _compass->mag_z);
update_MARG(deltat, gyro, accel, mag);
} else {
// step the quaternion solution using just gyros and accels
gyro -= gyro_bias;
update_IMU(deltat, gyro, accel);
#else
Vector3f mag = Vector3f(_compass->mag_x, _compass->mag_y, - _compass->mag_z);
update_MARG(deltat, gyro, accel, mag);
#endif
}
#ifdef DESKTOP_BUILD
if (q.is_nan()) {
@ -587,7 +384,7 @@ void AP_Quaternion::update(void)
}
// average error in roll/pitch since last call
float AP_Quaternion::get_error_rp(void)
float AP_AHRS_Quaternion::get_error_rp(void)
{
float ret;
if (_error_rp_count == 0) {
@ -600,7 +397,7 @@ float AP_Quaternion::get_error_rp(void)
}
// average error in yaw since last call
float AP_Quaternion::get_error_yaw(void)
float AP_AHRS_Quaternion::get_error_yaw(void)
{
float ret;
if (_error_yaw_count == 0) {
@ -611,3 +408,18 @@ float AP_Quaternion::get_error_yaw(void)
_error_yaw_count = 0;
return ret;
}
// reset attitude system
void AP_AHRS_Quaternion::reset(bool recover_eulers)
{
if (recover_eulers) {
q.from_euler(roll, pitch, yaw);
} else {
q(1, 0, 0, 0);
}
gyro_bias.zero();
// reference direction of flux in earth frame
b_x = 0;
b_z = -1;
}

94
libraries/AP_AHRS/AP_AHRS_Quaternion.h Normal file
View File

@ -0,0 +1,94 @@
#ifndef AP_Quaternion_h
#define AP_Quaternion_h
#include <AP_Math.h>
#include <inttypes.h>
#include <AP_Compass.h>
#include <AP_GPS.h>
#include <AP_IMU.h>
#if defined(ARDUINO) && ARDUINO >= 100
#include "Arduino.h"
#else
#include "WProgram.h"
#endif
class AP_AHRS_Quaternion : public AP_AHRS
{
public:
// Constructor
AP_AHRS_Quaternion(IMU *imu, GPS *&gps) : AP_AHRS(imu, gps)
{
// scaled gyro drift limits
beta = sqrt(3.0f / 4.0f) * gyroMeasError;
zeta = sqrt(3.0f / 4.0f) * _gyro_drift_limit;
// reset attitude
reset();
}
// Methods
void update(void);
void reset(bool recover_eulers=false);
// get corrected gyro vector
Vector3f get_gyro(void) {
// notice the sign reversals here
return Vector3f(_gyro_corrected.x, _gyro_corrected.y, _gyro_corrected.z);
}
Vector3f get_gyro_drift(void) {
// notice the sign reversals here. The quaternion
// system uses a -ve gyro bias, DCM uses a +ve
return Vector3f(-gyro_bias.x, -gyro_bias.y, -gyro_bias.z);
}
float get_error_rp(void);
float get_error_yaw(void);
// convert quaternion to a DCM matrix, used by compass
// null offsets code
Matrix3f get_dcm_matrix(void) {
Matrix3f ret;
q.rotation_matrix(ret);
return ret;
}
private:
void update_IMU(float deltat, Vector3f &gyro, Vector3f &accel);
void update_MARG(float deltat, Vector3f &gyro, Vector3f &accel, Vector3f &mag);
bool _have_initial_yaw;
// Methods
void accel_adjust(void);
// maximum gyroscope measurement error in rad/s (set to 7 degrees/second)
static const float gyroMeasError = 20.0 * (M_PI/180.0);
// scaled tuning constants
float beta;
float zeta;
// quaternion elements
Quaternion q;
// magnetic flux estimates. These are used for the automatic
// magnetometer calibration
float b_x;
float b_z;
// estimate gyroscope biases error
Vector3f gyro_bias;
// the current corrected gyro vector
Vector3f _gyro_corrected;
// estimate of error
float _error_rp_sum;
uint16_t _error_rp_count;
float _error_yaw_sum;
uint16_t _error_yaw_count;
};
#endif

16
libraries/AP_Quaternion/examples/Quaternion/Quaternion.pde → libraries/AP_AHRS/examples/AHRS_Test/AHRS_Test.pde
View File

@ -1,7 +1,7 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
//
// Simple test for the AP_Quaternion library
// Simple test for the AP_AHRS interface
//
#include <FastSerial.h>
@ -13,8 +13,7 @@
#include <AP_ADC.h>
#include <AP_IMU.h>
#include <AP_GPS.h>
#include <AP_Quaternion.h>
#include <AP_DCM.h>
#include <AP_AHRS.h>
#include <AP_Math.h>
#include <AP_Common.h>
#include <AP_Compass.h>
@ -24,7 +23,7 @@
#include <GCS_MAVLink.h>
// uncomment this for a APM2 board
//#define APM2_HARDWARE
#define APM2_HARDWARE
FastSerialPort(Serial, 0);
@ -42,13 +41,15 @@ AP_Compass_HMC5843 compass;
# else
AP_ADC_ADS7844 adc;
AP_InertialSensor_Oilpan ins( &adc );
#endif // CONFIG_IMU_TYPE
#endif
static GPS *g_gps;
AP_IMU_INS imu(&ins);
AP_Quaternion ahrs(&imu, g_gps);
//AP_DCM ahrs(&imu, g_gps);
// choose which AHRS system to use
//AP_AHRS_DCM ahrs(&imu, g_gps);
AP_AHRS_Quaternion ahrs(&imu, g_gps);
AP_Baro_BMP085_HIL barometer;
@ -122,6 +123,7 @@ void loop(void)
if (now - last_compass > 100*1000UL &&
compass.read()) {
compass.calculate(ahrs.get_dcm_matrix());
// read compass at 10Hz
last_compass = now;
}

0
libraries/AP_DCM/examples/DCM_Test/Makefile → libraries/AP_AHRS/examples/AHRS_Test/Makefile
View File

0
libraries/AP_DCM/examples/DCM_Test/DCM_Test.pde → libraries/AP_AHRS/examples/DCM_Test/DCM_Test.pde
View File

0
libraries/AP_Quaternion/examples/Quaternion/Makefile → libraries/AP_AHRS/examples/DCM_Test/Makefile
View File

157
libraries/AP_DCM/AP_DCM.h
View File

@ -1,157 +0,0 @@
#ifndef AP_DCM_h
#define AP_DCM_h
// temporarily include all other classes here
// since this naming is a bit off from the
// convention and the AP_DCM should be the top
// header file
#include "AP_DCM_HIL.h"
#include "../FastSerial/FastSerial.h"
#include "../AP_Math/AP_Math.h"
#include <inttypes.h>
#include "../AP_Compass/AP_Compass.h"
#include "../AP_ADC/AP_ADC.h"
#include "../AP_GPS/AP_GPS.h"
#include "../AP_IMU/AP_IMU.h"
#if defined(ARDUINO) && ARDUINO >= 100
#include "Arduino.h"
#else
#include "WProgram.h"
#endif
class AP_DCM
{
public:
// Constructors
AP_DCM(IMU *imu, GPS *&gps) :
_kp_roll_pitch(0.13),
_kp_yaw(0.4),
_gps(gps),
_imu(imu),
_dcm_matrix(1, 0, 0,
0, 1, 0,
0, 0, 1),
_health(1.),
_toggle(0)
{
// base the ki values by the sensors maximum drift
// rate. The APM2 has gyros which are much less drift
// prone than the APM1, so we should have a lower ki,
// which will make us less prone to increasing omegaI
// incorrectly due to sensor noise
_gyro_drift_limit = imu->get_gyro_drift_rate();
_ki_roll_pitch = _gyro_drift_limit * 5;
_ki_yaw = _gyro_drift_limit * 8;
}
// Accessors
// return the smoothed gyro vector corrected for drift
Vector3f get_gyro(void) {return _omega_smoothed; }
Matrix3f get_dcm_matrix(void) {return _dcm_matrix; }
Matrix3f get_dcm_transposed(void) {Matrix3f temp = _dcm_matrix; return temp.transpose();}
// return the current drift correction integrator value
Vector3f get_gyro_drift(void) {return _omega_I; }
float get_health(void) {return _health;}
void set_centripetal(bool b) {_centripetal = b;}
bool get_centripetal(void) {return _centripetal;}
void set_compass(Compass *compass);
// Methods
void update_DCM(uint8_t drift_correction_frequency=1);
void update(void) { update_DCM(); }
void matrix_reset(bool recover_eulers = false);
long roll_sensor; // Degrees * 100
long pitch_sensor; // Degrees * 100
long yaw_sensor; // Degrees * 100
float roll; // Radians
float pitch; // Radians
float yaw; // Radians
uint8_t gyro_sat_count;
uint8_t renorm_range_count;
uint8_t renorm_blowup_count;
// status reporting
float get_accel_weight(void);
float get_renorm_val(void);
float get_error_rp(void);
float get_error_yaw(void);
private:
float _kp_roll_pitch;
float _ki_roll_pitch;
float _kp_yaw;
float _ki_yaw;
float _gyro_drift_limit; // radians/s/s
bool _have_initial_yaw;
// Methods
void read_adc_raw(void);
void accel_adjust(Vector3f &accel);
float read_adc(int select);
void matrix_update(float _G_Dt);
void normalize(void);
void check_matrix(void);
bool renorm(Vector3f const &a, Vector3f &result);
void drift_correction(float deltat);
void euler_angles(void);
// members
Compass * _compass;
// note: we use ref-to-pointer here so that our caller can change the GPS without our noticing
// IMU under us without our noticing.
GPS *&_gps; // note: this is a reference to a pointer owned by the caller
IMU *_imu;
Matrix3f _dcm_matrix;
// sum of accel vectors between drift_correction() calls
// this allows the drift correction to run at a different rate
// to the main DCM update code
Vector3f _accel_vector;
Vector3f _accel_sum;
Vector3f _gyro_vector; // Store the gyros turn rate in a vector
Vector3f _omega_P; // accel Omega Proportional correction
Vector3f _omega_yaw_P; // yaw Omega Proportional correction
Vector3f _omega_I; // Omega Integrator correction
Vector3f _omega_integ_corr; // Partially corrected Gyro_Vector data - used for centrepetal correction
Vector3f _omega; // Corrected Gyro_Vector data
Vector3f _omega_sum;
Vector3f _omega_smoothed;
float _health;
bool _centripetal;
uint8_t _toggle;
// state to support status reporting
float _renorm_val_sum;
uint16_t _renorm_val_count;
float _error_rp_sum;
uint16_t _error_rp_count;
float _error_yaw_sum;
uint16_t _error_yaw_count;
// time in micros when we last got a compass fix
uint32_t _compass_last_update;
// time in millis when we last got a GPS heading
uint32_t _gps_last_update;
// counter of calls to update_DCM() without drift correction
uint8_t _drift_correction_count;
float _drift_correction_time;
};
#endif

53
libraries/AP_DCM/AP_DCM_HIL.cpp
View File

@ -1,53 +0,0 @@
/*
APM_DCM_HIL.cpp - DCM AHRS Library, for Ardupilot Mega, Hardware in the Loop Model
Code by James Goppert. DIYDrones.com
This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.
Methods:
update_DCM() : Updates the AHRS by integrating the rotation matrix over time using the IMU object data
get_gyro() : Returns gyro vector corrected for bias
get_accel() : Returns accelerometer vector
get_dcm_matrix() : Returns dcm matrix
*/
#include <AP_DCM_HIL.h>
#define ToRad(x) (x*0.01745329252) // *pi/180
#define ToDeg(x) (x*57.2957795131) // *180/pi
/**************************************************/
void
AP_DCM_HIL::setHil(float _roll, float _pitch, float _yaw,
float _rollRate, float _pitchRate, float _yawRate)
{
roll = _roll;
pitch = _pitch;
yaw = _yaw;
_omega_integ_corr.x = _rollRate;
_omega_integ_corr.y = _pitchRate;
_omega_integ_corr.z = _yawRate;
roll_sensor =ToDeg(roll)*100;
pitch_sensor =ToDeg(pitch)*100;
yaw_sensor =ToDeg(yaw)*100;
// Need the standard C_body<-nav dcm from navigation frame to body frame
// Strapdown Inertial Navigation Technology / Titterton/ pg. 41
float sRoll = sin(roll), cRoll = cos(roll);
float sPitch = sin(pitch), cPitch = cos(pitch);
float sYaw = sin(yaw), cYaw = cos(yaw);
_dcm_matrix.a.x = cPitch*cYaw;
_dcm_matrix.a.y = -cRoll*sYaw+sRoll*sPitch*cYaw;
_dcm_matrix.a.z = sRoll*sYaw+cRoll*sPitch*cYaw;
_dcm_matrix.b.x = cPitch*sYaw;
_dcm_matrix.b.y = cRoll*cYaw+sRoll*sPitch*sYaw;
_dcm_matrix.b.z = -sRoll*cYaw+cRoll*sPitch*sYaw;
_dcm_matrix.c.x = -sPitch;
_dcm_matrix.c.y = sRoll*cPitch;
_dcm_matrix.c.z = cRoll*cPitch;
}

81
libraries/AP_DCM/AP_DCM_HIL.h
View File

@ -1,81 +0,0 @@
#ifndef AP_DCM_HIL_H
#define AP_DCM_HIL_H
#include "../FastSerial/FastSerial.h"
#include "../AP_Math/AP_Math.h"
#include <inttypes.h>
#include "../AP_Compass/AP_Compass.h"
#include "../AP_ADC/AP_ADC.h"
#include "../AP_GPS/AP_GPS.h"
#include "../AP_IMU/AP_IMU.h"
#if defined(ARDUINO) && ARDUINO >= 100
#include "Arduino.h"
#else
#include "WProgram.h"
#endif
class AP_DCM_HIL
{
public:
// Constructors
AP_DCM_HIL() :
_dcm_matrix(1, 0, 0,
0, 1, 0,
0, 0, 1)
{}
// Accessors
Vector3f get_gyro(void) {return _omega_integ_corr; }
Vector3f get_accel(void) { return _accel_vector; }
Matrix3f get_dcm_matrix(void) {return _dcm_matrix; }
Matrix3f get_dcm_transposed(void) {Matrix3f temp = _dcm_matrix; return temp.transpose();}
void set_centripetal(bool b) {}
void set_compass(Compass *compass) {}
// Methods
void update_DCM(void) {}
void update_DCM_fast(void) {}
float get_health(void) { return 1.0; }
long roll_sensor; // Degrees * 100
long pitch_sensor; // Degrees * 100
long yaw_sensor; // Degrees * 100
float roll; // Radians
float pitch; // Radians
float yaw; // Radians
uint8_t gyro_sat_count;
uint8_t renorm_range_count;
uint8_t renorm_blowup_count;
float kp_roll_pitch() { return 0; }
void kp_roll_pitch(float v) { }
float ki_roll_pitch() { return 0; }
void ki_roll_pitch(float v) { }
float kp_yaw() { return 0; }
void kp_yaw(float v) { }
float ki_yaw() { return 0; }
void ki_yaw(float v) { }
void setHil(float roll, float pitch, float yaw,
float rollRate, float pitchRate, float yawRate);
private:
// Methods
Matrix3f _dcm_matrix;
Vector3f _omega_integ_corr;
Vector3f _accel_vector;
};
#endif

37
libraries/AP_Math/AP_Math.cpp
View File

@ -30,3 +30,40 @@ float safe_sqrt(float v)
}
return ret;
}
// find a rotation that is the combination of two other
// rotations. This is used to allow us to add an overall board
// rotation to an existing rotation of a sensor such as the compass
// Note that this relies the set of rotations being complete. The
// optional 'found' parameter is for the test suite to ensure that it is.
enum Rotation rotation_combination(enum Rotation r1, enum Rotation r2, bool *found)
{
Vector3f tv1, tv2;
enum Rotation r;
tv1(1,2,3);
tv1.rotate(r1);
tv1.rotate(r2);
for (r=ROTATION_NONE; r<ROTATION_MAX;
r = (enum Rotation)((uint8_t)r+1)) {
Vector3f diff;
tv2(1,2,3);
tv2.rotate(r);
diff = tv1 - tv2;
if (diff.length() < 1.0e-6) {
// we found a match
if (found) {
*found = true;
}
return r;
}
}
// we found no matching rotation. Someone has edited the
// rotations list and broken its completeness property ...
if (found) {
*found = false;
}
return ROTATION_NONE;
}

5
libraries/AP_Math/AP_Math.h
View File

@ -24,4 +24,9 @@ float safe_asin(float v);
// a varient of sqrt() that always gives a valid answer.
float safe_sqrt(float v);
// find a rotation that is the combination of two other
// rotations. This is used to allow us to add an overall board
// rotation to an existing rotation of a sensor such as the compass
enum Rotation rotation_combination(enum Rotation r1, enum Rotation r2, bool *found = NULL);
#endif

53
libraries/AP_Math/examples/eulers/eulers.pde
View File

@ -17,6 +17,8 @@ FastSerialPort(Serial, 0);
#include <Arduino_Mega_ISR_Registry.h>
#include <AP_PeriodicProcess.h>
#include <AP_ADC.h>
#include <SPI.h>
#include <I2C.h>
#include <AP_Baro.h>
#include <AP_Compass.h>
#include <AP_GPS.h>
@ -25,6 +27,10 @@ AP_Baro_BMP085_HIL barometer;
AP_Compass_HIL compass;
#endif
#if 0
#include <AP_Declination.h>
#endif
static float rad_diff(float rad1, float rad2)
{
float diff = rad1 - rad2;
@ -206,6 +212,52 @@ void test_frame_transforms(void)
printf("%f %f %f\n", v2.x, v2.y, v2.z);
}
// generate a random float between -1 and 1
static float rand_num(void)
{
float ret = ((unsigned)random()) % 2000000;
return (ret - 1.0e6) / 1.0e6;
}
void test_matrix_rotate(void)
{
Matrix3f m1, m2, diff;
Vector3f r;
m1.identity();
m2.identity();
r.x = rand_num();
r.y = rand_num();
r.z = rand_num();
for (uint16_t i = 0; i<1000; i++) {
// old method
Matrix3f temp_matrix;
temp_matrix.a.x = 0;
temp_matrix.a.y = -r.z;
temp_matrix.a.z = r.y;
temp_matrix.b.x = r.z;
temp_matrix.b.y = 0;
temp_matrix.b.z = -r.x;
temp_matrix.c.x = -r.y;
temp_matrix.c.y = r.x;
temp_matrix.c.z = 0;
temp_matrix = m1 * temp_matrix;
m1 += temp_matrix;
// new method
m2.rotate(r);
// check they behave in the same way
diff = m1 - m2;
float err = diff.a.length() + diff.b.length() + diff.c.length();
if (err > 0) {
Serial.printf("ERROR: i=%u err=%f\n", (unsigned)i, err);
}
}
}
/*
euler angle tests
*/
@ -220,6 +272,7 @@ void setup(void)
test_conversions();
test_quaternion_eulers();
test_matrix_eulers();
test_matrix_rotate();
}
void

28
libraries/AP_Math/examples/rotations/rotations.pde
View File

@ -25,6 +25,11 @@ AP_Baro_BMP085_HIL barometer;
AP_Compass_HIL compass;
#endif
#if AUTOMATIC_DECLINATION == ENABLED
// this is in an #if to avoid the static data
#include <AP_Declination.h> // ArduPilot Mega Declination Helper Library
#endif
// standard rotation matrices (these are the originals from the old code)
#define MATRIX_ROTATION_NONE Matrix3f(1, 0, 0, 0, 1, 0, 0 ,0, 1)
@ -162,6 +167,28 @@ static void test_vectors(void)
}
// test combinations of rotations
static void test_combinations(void)
{
enum Rotation r1, r2, r3;
bool found;
for (r1=ROTATION_NONE; r1<ROTATION_MAX;
r1 = (enum Rotation)((uint8_t)r1+1)) {
for (r2=ROTATION_NONE; r2<ROTATION_MAX;
r2 = (enum Rotation)((uint8_t)r2+1)) {
r3 = rotation_combination(r1, r2, &found);
if (found) {
Serial.printf("rotation: %u + %u -> %u\n",
(unsigned)r1, (unsigned)r2, (unsigned)r3);
} else {
Serial.printf("ERROR rotation: no combination for %u + %u\n",
(unsigned)r1, (unsigned)r2);
}
}
}
}
/*
rotation tests
*/
@ -171,6 +198,7 @@ void setup(void)
Serial.println("rotation unit tests\n");
test_matrices();
test_vectors();
test_combinations();
Serial.println("rotation unit tests done\n");
}

21
libraries/AP_Math/matrix3.cpp
View File

@ -44,6 +44,7 @@ void Matrix3<T>::rotation(enum Rotation r)
{
switch (r) {
case ROTATION_NONE:
case ROTATION_MAX:
*this = MATRIX_ROTATION_NONE;
break;
case ROTATION_YAW_45:
@ -133,7 +134,27 @@ void Matrix3<T>::to_euler(float *roll, float *pitch, float *yaw)
}
}
// apply an additional rotation from a body frame gyro vector
// to a rotation matrix.
template <typename T>
void Matrix3<T>::rotate(const Vector3<T> &g)
{
Matrix3f temp_matrix;
temp_matrix.a.x = a.y * g.z - a.z * g.y;
temp_matrix.a.y = a.z * g.x - a.x * g.z;
temp_matrix.a.z = a.x * g.y - a.y * g.x;
temp_matrix.b.x = b.y * g.z - b.z * g.y;
temp_matrix.b.y = b.z * g.x - b.x * g.z;
temp_matrix.b.z = b.x * g.y - b.y * g.x;
temp_matrix.c.x = c.y * g.z - c.z * g.y;
temp_matrix.c.y = c.z * g.x - c.x * g.z;
temp_matrix.c.z = c.x * g.y - c.y * g.x;
(*this) += temp_matrix;
}
// only define for float
template void Matrix3<float>::rotation(enum Rotation);
template void Matrix3<float>::rotate(const Vector3<float> &g);
template void Matrix3<float>::from_euler(float roll, float pitch, float yaw);
template void Matrix3<float>::to_euler(float *roll, float *pitch, float *yaw);

19
libraries/AP_Math/matrix3.h
View File

@ -124,6 +124,21 @@ public:
Matrix3<T> transpose(void)
{ return *this = transposed(); }
// zero the matrix
void zero(void) {
a.x = a.y = a.z = 0;
b.x = b.y = b.z = 0;
c.x = c.y = c.z = 0;
}
// setup the identity matrix
void identity(void) {
a.x = b.y = c.z = 1;
a.y = a.z = 0;
b.x = b.z = 0;
c.x = c.y = 0;
}
// check if any elements are NAN
bool is_nan(void)
{ return a.is_nan() || b.is_nan() || c.is_nan(); }
@ -136,6 +151,10 @@ public:
// create eulers from a rotation matrix
void to_euler(float *roll, float *pitch, float *yaw);
// apply an additional rotation from a body frame gyro vector
// to a rotation matrix.
void rotate(const Vector3<T> &g);
};
typedef Matrix3<int16_t> Matrix3i;

18
libraries/AP_Math/quaternion.cpp
View File

@ -34,12 +34,12 @@ void Quaternion::rotation_matrix(Matrix3f &m)
m.a.x = 1-2*(q3q3 + q4q4);
m.a.y = 2*(q2q3 - q1q4);
m.a.z = - 2*(q2q4 + q1q3);
m.a.z = 2*(q2q4 + q1q3);
m.b.x = 2*(q2q3 + q1q4);
m.b.y = 1-2*(q2q2 + q4q4);
m.b.z = -2*(q3q4 - q1q2);
m.c.x = -2*(q2q4 - q1q3);
m.c.y = -2*(q3q4 + q1q2);
m.b.z = 2*(q3q4 - q1q2);
m.c.x = 2*(q2q4 - q1q3);
m.c.y = 2*(q3q4 + q1q2);
m.c.z = 1-2*(q2q2 + q3q3);
}
@ -61,10 +61,8 @@ void Quaternion::from_euler(float roll, float pitch, float yaw)
float cr2 = cos(roll*0.5);
float cp2 = cos(pitch*0.5);
float cy2 = cos(yaw*0.5);
// the sign reversal here is due to the different conventions
// in the madgwick quaternion code and the rest of APM
float sr2 = -sin(roll*0.5);
float sp2 = -sin(pitch*0.5);
float sr2 = sin(roll*0.5);
float sp2 = sin(pitch*0.5);
float sy2 = sin(yaw*0.5);
q1 = cr2*cp2*cy2 + sr2*sp2*sy2;
@ -77,12 +75,12 @@ void Quaternion::from_euler(float roll, float pitch, float yaw)
void Quaternion::to_euler(float *roll, float *pitch, float *yaw)
{
if (roll) {
*roll = -(atan2(2.0*(q1*q2 + q3*q4),
*roll = (atan2(2.0*(q1*q2 + q3*q4),
1 - 2.0*(q2*q2 + q3*q3)));
}
if (pitch) {
// we let safe_asin() handle the singularities near 90/-90 in pitch
*pitch = -safe_asin(2.0*(q1*q3 - q4*q2));
*pitch = safe_asin(2.0*(q1*q3 - q4*q2));
}
if (yaw) {
*yaw = atan2(2.0*(q1*q4 + q2*q3),

9
libraries/AP_Math/rotations.h
View File

@ -17,6 +17,12 @@
* with this program. If not, see <http://www.gnu.org/licenses/>.
*/
// these rotations form a full set - every rotation in the following
// list when combined with another in the list forms an entry which is
// also in the list. This is an important property. Please run the
// rotations test suite if you add to the list.
// these rotation values are stored to EEPROM, so be careful not to
// change the numbering of any existing entry when adding a new entry.
enum Rotation {
@ -35,5 +41,6 @@ enum Rotation {
ROTATION_PITCH_180,
ROTATION_ROLL_180_YAW_225,
ROTATION_ROLL_180_YAW_270,
ROTATION_ROLL_180_YAW_315
ROTATION_ROLL_180_YAW_315,
ROTATION_MAX
};

1
libraries/AP_Math/vector3.cpp
View File

@ -29,6 +29,7 @@ void Vector3<T>::rotate(enum Rotation rotation)
T tmp;
switch (rotation) {
case ROTATION_NONE:
case ROTATION_MAX:
return;
case ROTATION_YAW_45: {
tmp = HALF_SQRT_2*(x - y);

39
libraries/AP_Mount/AP_Mount.cpp
View File

@ -4,10 +4,9 @@
extern RC_Channel_aux* g_rc_function[RC_Channel_aux::k_nr_aux_servo_functions]; // the aux. servo ch. assigned to each function
AP_Mount::AP_Mount(GPS *gps, AP_DCM *dcm)
AP_Mount::AP_Mount(GPS *gps, AP_AHRS *ahrs)
{
_dcm=dcm;
_dcm_hil=NULL;
_ahrs=ahrs;
_gps=gps;
//set_mode(MAV_MOUNT_MODE_RETRACT);
//set_mode(MAV_MOUNT_MODE_RC_TARGETING); // FIXME: This is just to test without mavlink
@ -18,20 +17,6 @@ AP_Mount::AP_Mount(GPS *gps, AP_DCM *dcm)
_retract_angles.z=0;
}
AP_Mount::AP_Mount(GPS *gps, AP_DCM_HIL *dcm)
{
_dcm=NULL;
_dcm_hil=dcm;
_gps=gps;
//set_mode(MAV_MOUNT_MODE_RETRACT);
//set_mode(MAV_MOUNT_MODE_RC_TARGETING); // FIXME: This is just to test without mavlink
set_mode(MAV_MOUNT_MODE_MAVLINK_TARGETING); // FIXME: this is to test ONLY targeting
_retract_angles.x=0;
_retract_angles.y=0;
_retract_angles.z=0;
}
//sets the servo angles for retraction, note angles are * 100
void AP_Mount::set_retract_angles(int roll, int pitch, int yaw)
{
@ -88,7 +73,8 @@ void AP_Mount::update_mount_position()
aux_vec.x = _mavlink_angles.x;
aux_vec.y = _mavlink_angles.y;
aux_vec.z = _mavlink_angles.z;
m = _dcm?_dcm->get_dcm_transposed():_dcm_hil->get_dcm_transposed();
m = _ahrs->get_dcm_matrix();
m.transpose();
//rotate vector
targ = m*aux_vec;
// TODO The next three lines are probably not correct yet
@ -101,17 +87,11 @@ void AP_Mount::update_mount_position()
case MAV_MOUNT_MODE_RC_TARGETING: // radio manual control
{
// TODO It does work, but maybe is a good idea to replace this simplified implementation with a proper one
if (_dcm)
if (_ahrs)
{
roll_angle = -_dcm->roll_sensor;
pitch_angle = -_dcm->pitch_sensor;
yaw_angle = -_dcm->yaw_sensor;
}
if (_dcm_hil)
{
roll_angle = -_dcm_hil->roll_sensor;
pitch_angle = -_dcm_hil->pitch_sensor;
yaw_angle = -_dcm_hil->yaw_sensor;
roll_angle = -_ahrs->roll_sensor;
pitch_angle = -_ahrs->pitch_sensor;
yaw_angle = -_ahrs->yaw_sensor;
}
if (g_rc_function[RC_Channel_aux::k_mount_roll])
roll_angle = rc_map(g_rc_function[RC_Channel_aux::k_mount_roll]);
@ -128,7 +108,8 @@ void AP_Mount::update_mount_position()
{
calc_GPS_target_vector(&_target_GPS_location);
}
m = (_dcm)?_dcm->get_dcm_transposed():_dcm_hil->get_dcm_transposed();
m = _ahrs->get_dcm_matrix();
m.transpose();
targ = m*_GPS_vector;
/* disable stabilization for now, this will help debug */
_stab_roll = 0;_stab_pitch=0;_stab_yaw=0;

8
libraries/AP_Mount/AP_Mount.h
View File

@ -24,7 +24,7 @@
#include <AP_Math.h>
#include <AP_Common.h>
#include <AP_GPS.h>
#include <AP_DCM.h>
#include <AP_AHRS.h>
#include <GCS_MAVLink.h>
#include <../RC_Channel/RC_Channel_aux.h>
@ -32,8 +32,7 @@ class AP_Mount
{
public:
//Constructors
AP_Mount(GPS *gps, AP_DCM *dcm);
AP_Mount(GPS *gps, AP_DCM_HIL *dcm); // constructor for HIL usage
AP_Mount(GPS *gps, AP_AHRS *ahrs);
//enums
enum MountType{
@ -72,8 +71,7 @@ private:
long rc_map(RC_Channel_aux* rc_ch);
//members
AP_DCM *_dcm;
AP_DCM_HIL *_dcm_hil;
AP_AHRS *_ahrs;
GPS *_gps;
int roll_angle; ///< degrees*100

139
libraries/AP_Quaternion/AP_Quaternion.h
View File

@ -1,139 +0,0 @@
#ifndef AP_Quaternion_h
#define AP_Quaternion_h
#include <AP_Math.h>
#include <inttypes.h>
#include <AP_Compass.h>
#include <AP_GPS.h>
#include <AP_IMU.h>
#if defined(ARDUINO) && ARDUINO >= 100
#include "Arduino.h"
#else
#include "WProgram.h"
#endif
class AP_Quaternion
{
public:
// Constructor
AP_Quaternion(IMU *imu, GPS *&gps) :
_imu(imu),
_gps(gps)
{
// reference direction of flux in earth frame
b_x = 0;
b_z = -1;
// limit the drift to the drift rate reported by the
// sensor driver
gyroMeasDrift = imu->get_gyro_drift_rate();
// scaled gyro drift limits
beta = sqrt(3.0f / 4.0f) * gyroMeasError;
zeta = sqrt(3.0f / 4.0f) * gyroMeasDrift;
}
// Accessors
void set_centripetal(bool b) {_centripetal = b;}
bool get_centripetal(void) {return _centripetal;}
void set_compass(Compass *compass);
// Methods
void update(void);
// Euler angles (radians)
float roll;
float pitch;
float yaw;
// integer Euler angles (Degrees * 100)
int32_t roll_sensor;
int32_t pitch_sensor;
int32_t yaw_sensor;
// compatibility methods with DCM
void update_DCM(void) { update(); }
void update_DCM_fast(void) { update(); }
Vector3f get_gyro(void) {
// notice the sign reversals here
return Vector3f(-_gyro_corrected.x, -_gyro_corrected.y, _gyro_corrected.z);
}
Vector3f get_gyro_drift(void) {
// notice the sign reversals here
return Vector3f(-gyro_bias.x, -gyro_bias.y, gyro_bias.z);
}
float get_accel_weight(void) { return 0; }
float get_renorm_val(void) { return 0; }
float get_health(void) { return 0; }
void matrix_reset(void) { }
uint8_t gyro_sat_count;
uint8_t renorm_range_count;
uint8_t renorm_blowup_count;
float get_error_rp(void);
float get_error_yaw(void);
Matrix3f get_dcm_matrix(void) {
Matrix3f ret;
q.rotation_matrix(ret);
return ret;
}
private:
void update_IMU(float deltat, Vector3f &gyro, Vector3f &accel);
void update_MARG(float deltat, Vector3f &gyro, Vector3f &accel, Vector3f &mag);
void update_drift(float deltat, Vector3f &mag);
bool _have_initial_yaw;
// Methods
void accel_adjust(void);
// members
Compass * _compass;
// time in microseconds of last compass update
uint32_t _compass_last_update;
// note: we use ref-to-pointer here so that our caller can change the GPS without our noticing
// IMU under us without our noticing.
GPS *&_gps;
IMU *_imu;
// true if we are doing centripetal acceleration correction
bool _centripetal;
// maximum gyroscope measurement error in rad/s (set to 7 degrees/second)
static const float gyroMeasError = 20.0 * (M_PI/180.0);
float gyroMeasDrift;
float beta;
float zeta;
// quaternion elements
Quaternion q;
// magnetic flux estimates. These are used for the automatic
// magnetometer calibration
float b_x;
float b_z;
// estimate gyroscope biases error
Vector3f gyro_bias;
// the current corrected gyro vector
Vector3f _gyro_corrected;
// accel and gyro accumulators for drift correction
Vector3f _gyro_sum;
Vector3f _accel_sum;
uint32_t _sum_count;
// estimate of error
float _error_rp_sum;
uint16_t _error_rp_count;
float _error_yaw_sum;
uint16_t _error_yaw_count;
};
#endif

6
libraries/GCS_MAVLink/include/ardupilotmega/ardupilotmega.h
View File

@ -16,11 +16,11 @@ extern "C" {
#endif
#ifndef MAVLINK_MESSAGE_CRCS
#define MAVLINK_MESSAGE_CRCS {72, 39, 190, 92, 191, 217, 104, 119, 0, 219, 60, 186, 10, 0, 0, 0, 0, 0, 0, 0, 89, 159, 162, 121, 0, 149, 222, 110, 179, 136, 66, 126, 185, 147, 112, 252, 162, 215, 229, 128, 9, 106, 101, 213, 4, 229, 21, 214, 215, 14, 206, 50, 157, 126, 108, 213, 95, 5, 127, 0, 0, 0, 57, 126, 130, 119, 193, 191, 236, 158, 143, 0, 0, 104, 123, 131, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 174, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 155, 0, 0, 0, 0, 0, 0, 0, 0, 0, 143, 29, 208, 188, 118, 242, 19, 97, 233, 0, 18, 68, 136, 205, 42, 21, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 178, 224, 60, 106, 7}
#define MAVLINK_MESSAGE_CRCS {72, 39, 190, 92, 191, 217, 104, 119, 0, 219, 60, 186, 10, 0, 0, 0, 0, 0, 0, 0, 89, 159, 162, 121, 0, 149, 222, 110, 179, 136, 66, 126, 185, 147, 112, 252, 162, 215, 229, 128, 9, 106, 101, 213, 4, 229, 21, 214, 215, 14, 206, 50, 157, 126, 108, 213, 95, 5, 127, 0, 0, 0, 57, 126, 130, 119, 193, 191, 236, 158, 143, 0, 0, 104, 123, 131, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 174, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 155, 0, 0, 0, 0, 0, 0, 0, 0, 0, 143, 29, 208, 188, 118, 242, 19, 97, 233, 0, 18, 68, 136, 127, 42, 21, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 178, 224, 60, 106, 7}
#endif
#ifndef MAVLINK_MESSAGE_INFO
#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_BOOT, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_SYSTEM_TIME_UTC, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {NULL}, MAVLINK_MESSAGE_INFO_ACTION_ACK, MAVLINK_MESSAGE_INFO_ACTION, MAVLINK_MESSAGE_INFO_SET_MODE, MAVLINK_MESSAGE_INFO_SET_NAV_MODE, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, {NULL}, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_LOCAL_POSITION, MAVLINK_MESSAGE_INFO_GPS_RAW, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_WAYPOINT, MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST, MAVLINK_MESSAGE_INFO_WAYPOINT_SET_CURRENT, MAVLINK_MESSAGE_INFO_WAYPOINT_CURRENT, MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST_LIST, MAVLINK_MESSAGE_INFO_WAYPOINT_COUNT, MAVLINK_MESSAGE_INFO_WAYPOINT_CLEAR_ALL, MAVLINK_MESSAGE_INFO_WAYPOINT_REACHED, MAVLINK_MESSAGE_INFO_WAYPOINT_ACK, MAVLINK_MESSAGE_INFO_GPS_SET_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_LOCAL_ORIGIN_SET, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT_SET, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_CONTROL_STATUS, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_POSITION_TARGET, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, MAVLINK_MESSAGE_INFO_SET_ALTITUDE, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_VFR_HUD, MAVLINK_MESSAGE_INFO_COMMAND, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_OBJECT_DETECTION_EVENT, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_SENSOR_OFFSETS, MAVLINK_MESSAGE_INFO_SET_MAG_OFFSETS, MAVLINK_MESSAGE_INFO_MEMINFO, MAVLINK_MESSAGE_INFO_AP_ADC, MAVLINK_MESSAGE_INFO_DIGICAM_CONFIGURE, MAVLINK_MESSAGE_INFO_DIGICAM_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_CONFIGURE, MAVLINK_MESSAGE_INFO_MOUNT_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_STATUS, {NULL}, MAVLINK_MESSAGE_INFO_FENCE_POINT, MAVLINK_MESSAGE_INFO_FENCE_FETCH_POINT, MAVLINK_MESSAGE_INFO_FENCE_STATUS, MAVLINK_MESSAGE_INFO_DCM, MAVLINK_MESSAGE_INFO_SIMSTATE, MAVLINK_MESSAGE_INFO_HWSTATUS, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG}
#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_BOOT, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_SYSTEM_TIME_UTC, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {NULL}, MAVLINK_MESSAGE_INFO_ACTION_ACK, MAVLINK_MESSAGE_INFO_ACTION, MAVLINK_MESSAGE_INFO_SET_MODE, MAVLINK_MESSAGE_INFO_SET_NAV_MODE, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, {NULL}, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_LOCAL_POSITION, MAVLINK_MESSAGE_INFO_GPS_RAW, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_WAYPOINT, MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST, MAVLINK_MESSAGE_INFO_WAYPOINT_SET_CURRENT, MAVLINK_MESSAGE_INFO_WAYPOINT_CURRENT, MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST_LIST, MAVLINK_MESSAGE_INFO_WAYPOINT_COUNT, MAVLINK_MESSAGE_INFO_WAYPOINT_CLEAR_ALL, MAVLINK_MESSAGE_INFO_WAYPOINT_REACHED, MAVLINK_MESSAGE_INFO_WAYPOINT_ACK, MAVLINK_MESSAGE_INFO_GPS_SET_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_LOCAL_ORIGIN_SET, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT_SET, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_CONTROL_STATUS, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_POSITION_TARGET, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, MAVLINK_MESSAGE_INFO_SET_ALTITUDE, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_VFR_HUD, MAVLINK_MESSAGE_INFO_COMMAND, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_OBJECT_DETECTION_EVENT, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_SENSOR_OFFSETS, MAVLINK_MESSAGE_INFO_SET_MAG_OFFSETS, MAVLINK_MESSAGE_INFO_MEMINFO, MAVLINK_MESSAGE_INFO_AP_ADC, MAVLINK_MESSAGE_INFO_DIGICAM_CONFIGURE, MAVLINK_MESSAGE_INFO_DIGICAM_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_CONFIGURE, MAVLINK_MESSAGE_INFO_MOUNT_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_STATUS, {NULL}, MAVLINK_MESSAGE_INFO_FENCE_POINT, MAVLINK_MESSAGE_INFO_FENCE_FETCH_POINT, MAVLINK_MESSAGE_INFO_FENCE_STATUS, MAVLINK_MESSAGE_INFO_AHRS, MAVLINK_MESSAGE_INFO_SIMSTATE, MAVLINK_MESSAGE_INFO_HWSTATUS, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG}
#endif
#include "../protocol.h"
@ -143,7 +143,7 @@ enum FENCE_BREACH
#include "./mavlink_msg_fence_point.h"
#include "./mavlink_msg_fence_fetch_point.h"
#include "./mavlink_msg_fence_status.h"
#include "./mavlink_msg_dcm.h"
#include "./mavlink_msg_ahrs.h"
#include "./mavlink_msg_simstate.h"
#include "./mavlink_msg_hwstatus.h"

276
libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_ahrs.h Normal file
View File

@ -0,0 +1,276 @@
// MESSAGE AHRS PACKING
#define MAVLINK_MSG_ID_AHRS 163
typedef struct __mavlink_ahrs_t
{
float omegaIx; ///< X gyro drift estimate rad/s
float omegaIy; ///< Y gyro drift estimate rad/s
float omegaIz; ///< Z gyro drift estimate rad/s
float accel_weight; ///< average accel_weight
float renorm_val; ///< average renormalisation value
float error_rp; ///< average error_roll_pitch value
float error_yaw; ///< average error_yaw value
} mavlink_ahrs_t;
#define MAVLINK_MSG_ID_AHRS_LEN 28
#define MAVLINK_MSG_ID_163_LEN 28
#define MAVLINK_MESSAGE_INFO_AHRS { \
"AHRS", \
7, \
{ { "omegaIx", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_ahrs_t, omegaIx) }, \
{ "omegaIy", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_ahrs_t, omegaIy) }, \
{ "omegaIz", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_ahrs_t, omegaIz) }, \
{ "accel_weight", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_ahrs_t, accel_weight) }, \
{ "renorm_val", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_ahrs_t, renorm_val) }, \
{ "error_rp", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_ahrs_t, error_rp) }, \
{ "error_yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_ahrs_t, error_yaw) }, \
} \
}
/**
* @brief Pack a ahrs message
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param msg The MAVLink message to compress the data into
*
* @param omegaIx X gyro drift estimate rad/s
* @param omegaIy Y gyro drift estimate rad/s
* @param omegaIz Z gyro drift estimate rad/s
* @param accel_weight average accel_weight
* @param renorm_val average renormalisation value
* @param error_rp average error_roll_pitch value
* @param error_yaw average error_yaw value
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_ahrs_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
float omegaIx, float omegaIy, float omegaIz, float accel_weight, float renorm_val, float error_rp, float error_yaw)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
char buf[28];
_mav_put_float(buf, 0, omegaIx);
_mav_put_float(buf, 4, omegaIy);
_mav_put_float(buf, 8, omegaIz);
_mav_put_float(buf, 12, accel_weight);
_mav_put_float(buf, 16, renorm_val);
_mav_put_float(buf, 20, error_rp);
_mav_put_float(buf, 24, error_yaw);
memcpy(_MAV_PAYLOAD(msg), buf, 28);
#else
mavlink_ahrs_t packet;
packet.omegaIx = omegaIx;
packet.omegaIy = omegaIy;
packet.omegaIz = omegaIz;
packet.accel_weight = accel_weight;
packet.renorm_val = renorm_val;
packet.error_rp = error_rp;
packet.error_yaw = error_yaw;
memcpy(_MAV_PAYLOAD(msg), &packet, 28);
#endif
msg->msgid = MAVLINK_MSG_ID_AHRS;
return mavlink_finalize_message(msg, system_id, component_id, 28);
}
/**
* @brief Pack a ahrs message on a channel
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param chan The MAVLink channel this message was sent over
* @param msg The MAVLink message to compress the data into
* @param omegaIx X gyro drift estimate rad/s
* @param omegaIy Y gyro drift estimate rad/s
* @param omegaIz Z gyro drift estimate rad/s
* @param accel_weight average accel_weight
* @param renorm_val average renormalisation value
* @param error_rp average error_roll_pitch value
* @param error_yaw average error_yaw value
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_ahrs_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
mavlink_message_t* msg,
float omegaIx,float omegaIy,float omegaIz,float accel_weight,float renorm_val,float error_rp,float error_yaw)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
char buf[28];
_mav_put_float(buf, 0, omegaIx);
_mav_put_float(buf, 4, omegaIy);
_mav_put_float(buf, 8, omegaIz);
_mav_put_float(buf, 12, accel_weight);
_mav_put_float(buf, 16, renorm_val);
_mav_put_float(buf, 20, error_rp);
_mav_put_float(buf, 24, error_yaw);
memcpy(_MAV_PAYLOAD(msg), buf, 28);
#else
mavlink_ahrs_t packet;
packet.omegaIx = omegaIx;
packet.omegaIy = omegaIy;
packet.omegaIz = omegaIz;
packet.accel_weight = accel_weight;
packet.renorm_val = renorm_val;
packet.error_rp = error_rp;
packet.error_yaw = error_yaw;
memcpy(_MAV_PAYLOAD(msg), &packet, 28);
#endif
msg->msgid = MAVLINK_MSG_ID_AHRS;
return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 28);
}
/**
* @brief Encode a ahrs struct into a message
*
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param msg The MAVLink message to compress the data into
* @param ahrs C-struct to read the message contents from
*/
static inline uint16_t mavlink_msg_ahrs_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_ahrs_t* ahrs)
{
return mavlink_msg_ahrs_pack(system_id, component_id, msg, ahrs->omegaIx, ahrs->omegaIy, ahrs->omegaIz, ahrs->accel_weight, ahrs->renorm_val, ahrs->error_rp, ahrs->error_yaw);
}
/**
* @brief Send a ahrs message
* @param chan MAVLink channel to send the message
*
* @param omegaIx X gyro drift estimate rad/s
* @param omegaIy Y gyro drift estimate rad/s
* @param omegaIz Z gyro drift estimate rad/s
* @param accel_weight average accel_weight
* @param renorm_val average renormalisation value
* @param error_rp average error_roll_pitch value
* @param error_yaw average error_yaw value
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_ahrs_send(mavlink_channel_t chan, float omegaIx, float omegaIy, float omegaIz, float accel_weight, float renorm_val, float error_rp, float error_yaw)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
char buf[28];
_mav_put_float(buf, 0, omegaIx);
_mav_put_float(buf, 4, omegaIy);
_mav_put_float(buf, 8, omegaIz);
_mav_put_float(buf, 12, accel_weight);
_mav_put_float(buf, 16, renorm_val);
_mav_put_float(buf, 20, error_rp);
_mav_put_float(buf, 24, error_yaw);
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS, buf, 28);
#else
mavlink_ahrs_t packet;
packet.omegaIx = omegaIx;
packet.omegaIy = omegaIy;
packet.omegaIz = omegaIz;
packet.accel_weight = accel_weight;
packet.renorm_val = renorm_val;
packet.error_rp = error_rp;
packet.error_yaw = error_yaw;
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS, (const char *)&packet, 28);
#endif
}
#endif
// MESSAGE AHRS UNPACKING
/**
* @brief Get field omegaIx from ahrs message
*
* @return X gyro drift estimate rad/s
*/
static inline float mavlink_msg_ahrs_get_omegaIx(const mavlink_message_t* msg)
{
return _MAV_RETURN_float(msg, 0);
}
/**
* @brief Get field omegaIy from ahrs message
*
* @return Y gyro drift estimate rad/s
*/
static inline float mavlink_msg_ahrs_get_omegaIy(const mavlink_message_t* msg)
{
return _MAV_RETURN_float(msg, 4);
}
/**
* @brief Get field omegaIz from ahrs message
*
* @return Z gyro drift estimate rad/s
*/
static inline float mavlink_msg_ahrs_get_omegaIz(const mavlink_message_t* msg)
{
return _MAV_RETURN_float(msg, 8);
}
/**
* @brief Get field accel_weight from ahrs message
*
* @return average accel_weight
*/
static inline float mavlink_msg_ahrs_get_accel_weight(const mavlink_message_t* msg)
{
return _MAV_RETURN_float(msg, 12);
}
/**
* @brief Get field renorm_val from ahrs message
*
* @return average renormalisation value
*/
static inline float mavlink_msg_ahrs_get_renorm_val(const mavlink_message_t* msg)
{
return _MAV_RETURN_float(msg, 16);
}
/**
* @brief Get field error_rp from ahrs message
*
* @return average error_roll_pitch value
*/
static inline float mavlink_msg_ahrs_get_error_rp(const mavlink_message_t* msg)
{
return _MAV_RETURN_float(msg, 20);
}
/**
* @brief Get field error_yaw from ahrs message
*
* @return average error_yaw value
*/
static inline float mavlink_msg_ahrs_get_error_yaw(const mavlink_message_t* msg)
{
return _MAV_RETURN_float(msg, 24);
}
/**
* @brief Decode a ahrs message into a struct
*
* @param msg The message to decode
* @param ahrs C-struct to decode the message contents into
*/
static inline void mavlink_msg_ahrs_decode(const mavlink_message_t* msg, mavlink_ahrs_t* ahrs)
{
#if MAVLINK_NEED_BYTE_SWAP
ahrs->omegaIx = mavlink_msg_ahrs_get_omegaIx(msg);
ahrs->omegaIy = mavlink_msg_ahrs_get_omegaIy(msg);
ahrs->omegaIz = mavlink_msg_ahrs_get_omegaIz(msg);
ahrs->accel_weight = mavlink_msg_ahrs_get_accel_weight(msg);
ahrs->renorm_val = mavlink_msg_ahrs_get_renorm_val(msg);
ahrs->error_rp = mavlink_msg_ahrs_get_error_rp(msg);
ahrs->error_yaw = mavlink_msg_ahrs_get_error_yaw(msg);
#else
memcpy(ahrs, _MAV_PAYLOAD(msg), 28);
#endif
}

26
libraries/GCS_MAVLink/include/ardupilotmega/testsuite.h
View File

@ -668,12 +668,12 @@ static void mavlink_test_fence_status(uint8_t system_id, uint8_t component_id, m
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
}
static void mavlink_test_dcm(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
static void mavlink_test_ahrs(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
{
mavlink_message_t msg;
uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
uint16_t i;
mavlink_dcm_t packet_in = {
mavlink_ahrs_t packet_in = {
17.0,
45.0,
73.0,
@ -682,7 +682,7 @@ static void mavlink_test_dcm(uint8_t system_id, uint8_t component_id, mavlink_me
157.0,
185.0,
};
mavlink_dcm_t packet1, packet2;
mavlink_ahrs_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
packet1.omegaIx = packet_in.omegaIx;
packet1.omegaIy = packet_in.omegaIy;
@ -695,18 +695,18 @@ static void mavlink_test_dcm(uint8_t system_id, uint8_t component_id, mavlink_me
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_dcm_encode(system_id, component_id, &msg, &packet1);
mavlink_msg_dcm_decode(&msg, &packet2);
mavlink_msg_ahrs_encode(system_id, component_id, &msg, &packet1);
mavlink_msg_ahrs_decode(&msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_dcm_pack(system_id, component_id, &msg , packet1.omegaIx , packet1.omegaIy , packet1.omegaIz , packet1.accel_weight , packet1.renorm_val , packet1.error_rp , packet1.error_yaw );
mavlink_msg_dcm_decode(&msg, &packet2);
mavlink_msg_ahrs_pack(system_id, component_id, &msg , packet1.omegaIx , packet1.omegaIy , packet1.omegaIz , packet1.accel_weight , packet1.renorm_val , packet1.error_rp , packet1.error_yaw );
mavlink_msg_ahrs_decode(&msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_dcm_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.omegaIx , packet1.omegaIy , packet1.omegaIz , packet1.accel_weight , packet1.renorm_val , packet1.error_rp , packet1.error_yaw );
mavlink_msg_dcm_decode(&msg, &packet2);
mavlink_msg_ahrs_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.omegaIx , packet1.omegaIy , packet1.omegaIz , packet1.accel_weight , packet1.renorm_val , packet1.error_rp , packet1.error_yaw );
mavlink_msg_ahrs_decode(&msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
@ -714,12 +714,12 @@ static void mavlink_test_dcm(uint8_t system_id, uint8_t component_id, mavlink_me
for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) {
comm_send_ch(MAVLINK_COMM_0, buffer[i]);
}
mavlink_msg_dcm_decode(last_msg, &packet2);
mavlink_msg_ahrs_decode(last_msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_dcm_send(MAVLINK_COMM_1 , packet1.omegaIx , packet1.omegaIy , packet1.omegaIz , packet1.accel_weight , packet1.renorm_val , packet1.error_rp , packet1.error_yaw );
mavlink_msg_dcm_decode(last_msg, &packet2);
mavlink_msg_ahrs_send(MAVLINK_COMM_1 , packet1.omegaIx , packet1.omegaIy , packet1.omegaIz , packet1.accel_weight , packet1.renorm_val , packet1.error_rp , packet1.error_yaw );
mavlink_msg_ahrs_decode(last_msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
}
@ -841,7 +841,7 @@ static void mavlink_test_ardupilotmega(uint8_t system_id, uint8_t component_id,
mavlink_test_fence_point(system_id, component_id, last_msg);
mavlink_test_fence_fetch_point(system_id, component_id, last_msg);
mavlink_test_fence_status(system_id, component_id, last_msg);
mavlink_test_dcm(system_id, component_id, last_msg);
mavlink_test_ahrs(system_id, component_id, last_msg);
mavlink_test_simstate(system_id, component_id, last_msg);
mavlink_test_hwstatus(system_id, component_id, last_msg);
}

2
libraries/GCS_MAVLink/include/ardupilotmega/version.h
View File

@ -5,7 +5,7 @@
#ifndef MAVLINK_VERSION_H
#define MAVLINK_VERSION_H
#define MAVLINK_BUILD_DATE "Sat Mar 10 09:38:12 2012"
#define MAVLINK_BUILD_DATE "Sun Mar 11 19:38:21 2012"
#define MAVLINK_WIRE_PROTOCOL_VERSION "0.9"
#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 101

2
libraries/GCS_MAVLink/include/common/version.h
View File

@ -5,7 +5,7 @@
#ifndef MAVLINK_VERSION_H
#define MAVLINK_VERSION_H
#define MAVLINK_BUILD_DATE "Sat Mar 10 09:38:12 2012"
#define MAVLINK_BUILD_DATE "Sun Mar 11 19:38:21 2012"
#define MAVLINK_WIRE_PROTOCOL_VERSION "0.9"
#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 101

6
libraries/GCS_MAVLink/include_v1.0/ardupilotmega/ardupilotmega.h
View File

@ -16,11 +16,11 @@ extern "C" {
#endif
#ifndef MAVLINK_MESSAGE_CRCS
#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 0, 0, 183, 0, 130, 0, 148, 21, 0, 52, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 19, 102, 158, 208, 56, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 134, 219, 208, 188, 84, 22, 19, 21, 134, 0, 78, 68, 189, 205, 42, 21, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 247}
#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 0, 0, 183, 0, 130, 0, 148, 21, 0, 52, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 19, 102, 158, 208, 56, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 134, 219, 208, 188, 84, 22, 19, 21, 134, 0, 78, 68, 189, 127, 42, 21, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 247}
#endif
#ifndef MAVLINK_MESSAGE_INFO
#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {NULL}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_SET_MODE, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, {NULL}, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, {NULL}, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {NULL}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_VFR_HUD, {NULL}, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_SENSOR_OFFSETS, MAVLINK_MESSAGE_INFO_SET_MAG_OFFSETS, MAVLINK_MESSAGE_INFO_MEMINFO, MAVLINK_MESSAGE_INFO_AP_ADC, MAVLINK_MESSAGE_INFO_DIGICAM_CONFIGURE, MAVLINK_MESSAGE_INFO_DIGICAM_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_CONFIGURE, MAVLINK_MESSAGE_INFO_MOUNT_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_STATUS, {NULL}, MAVLINK_MESSAGE_INFO_FENCE_POINT, MAVLINK_MESSAGE_INFO_FENCE_FETCH_POINT, MAVLINK_MESSAGE_INFO_FENCE_STATUS, MAVLINK_MESSAGE_INFO_DCM, MAVLINK_MESSAGE_INFO_SIMSTATE, MAVLINK_MESSAGE_INFO_HWSTATUS, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, MAVLINK_MESSAGE_INFO_EXTENDED_MESSAGE}
#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {NULL}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_SET_MODE, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, {NULL}, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, {NULL}, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {NULL}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_VFR_HUD, {NULL}, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_SENSOR_OFFSETS, MAVLINK_MESSAGE_INFO_SET_MAG_OFFSETS, MAVLINK_MESSAGE_INFO_MEMINFO, MAVLINK_MESSAGE_INFO_AP_ADC, MAVLINK_MESSAGE_INFO_DIGICAM_CONFIGURE, MAVLINK_MESSAGE_INFO_DIGICAM_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_CONFIGURE, MAVLINK_MESSAGE_INFO_MOUNT_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_STATUS, {NULL}, MAVLINK_MESSAGE_INFO_FENCE_POINT, MAVLINK_MESSAGE_INFO_FENCE_FETCH_POINT, MAVLINK_MESSAGE_INFO_FENCE_STATUS, MAVLINK_MESSAGE_INFO_AHRS, MAVLINK_MESSAGE_INFO_SIMSTATE, MAVLINK_MESSAGE_INFO_HWSTATUS, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, MAVLINK_MESSAGE_INFO_EXTENDED_MESSAGE}
#endif
#include "../protocol.h"
@ -142,7 +142,7 @@ enum FENCE_BREACH
#include "./mavlink_msg_fence_point.h"
#include "./mavlink_msg_fence_fetch_point.h"
#include "./mavlink_msg_fence_status.h"
#include "./mavlink_msg_dcm.h"
#include "./mavlink_msg_ahrs.h"
#include "./mavlink_msg_simstate.h"
#include "./mavlink_msg_hwstatus.h"

276
libraries/GCS_MAVLink/include_v1.0/ardupilotmega/mavlink_msg_ahrs.h Normal file
View File

@ -0,0 +1,276 @@
// MESSAGE AHRS PACKING
#define MAVLINK_MSG_ID_AHRS 163
typedef struct __mavlink_ahrs_t
{
float omegaIx; ///< X gyro drift estimate rad/s
float omegaIy; ///< Y gyro drift estimate rad/s
float omegaIz; ///< Z gyro drift estimate rad/s
float accel_weight; ///< average accel_weight
float renorm_val; ///< average renormalisation value
float error_rp; ///< average error_roll_pitch value
float error_yaw; ///< average error_yaw value
} mavlink_ahrs_t;
#define MAVLINK_MSG_ID_AHRS_LEN 28
#define MAVLINK_MSG_ID_163_LEN 28
#define MAVLINK_MESSAGE_INFO_AHRS { \
"AHRS", \
7, \
{ { "omegaIx", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_ahrs_t, omegaIx) }, \
{ "omegaIy", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_ahrs_t, omegaIy) }, \
{ "omegaIz", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_ahrs_t, omegaIz) }, \
{ "accel_weight", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_ahrs_t, accel_weight) }, \
{ "renorm_val", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_ahrs_t, renorm_val) }, \
{ "error_rp", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_ahrs_t, error_rp) }, \
{ "error_yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_ahrs_t, error_yaw) }, \
} \
}
/**
* @brief Pack a ahrs message
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param msg The MAVLink message to compress the data into
*
* @param omegaIx X gyro drift estimate rad/s
* @param omegaIy Y gyro drift estimate rad/s
* @param omegaIz Z gyro drift estimate rad/s
* @param accel_weight average accel_weight
* @param renorm_val average renormalisation value
* @param error_rp average error_roll_pitch value
* @param error_yaw average error_yaw value
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_ahrs_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
float omegaIx, float omegaIy, float omegaIz, float accel_weight, float renorm_val, float error_rp, float error_yaw)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
char buf[28];
_mav_put_float(buf, 0, omegaIx);
_mav_put_float(buf, 4, omegaIy);
_mav_put_float(buf, 8, omegaIz);
_mav_put_float(buf, 12, accel_weight);
_mav_put_float(buf, 16, renorm_val);
_mav_put_float(buf, 20, error_rp);
_mav_put_float(buf, 24, error_yaw);
memcpy(_MAV_PAYLOAD(msg), buf, 28);
#else
mavlink_ahrs_t packet;
packet.omegaIx = omegaIx;
packet.omegaIy = omegaIy;
packet.omegaIz = omegaIz;
packet.accel_weight = accel_weight;
packet.renorm_val = renorm_val;
packet.error_rp = error_rp;
packet.error_yaw = error_yaw;
memcpy(_MAV_PAYLOAD(msg), &packet, 28);
#endif
msg->msgid = MAVLINK_MSG_ID_AHRS;
return mavlink_finalize_message(msg, system_id, component_id, 28, 127);
}
/**
* @brief Pack a ahrs message on a channel
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param chan The MAVLink channel this message was sent over
* @param msg The MAVLink message to compress the data into
* @param omegaIx X gyro drift estimate rad/s
* @param omegaIy Y gyro drift estimate rad/s
* @param omegaIz Z gyro drift estimate rad/s
* @param accel_weight average accel_weight
* @param renorm_val average renormalisation value
* @param error_rp average error_roll_pitch value
* @param error_yaw average error_yaw value
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_ahrs_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
mavlink_message_t* msg,
float omegaIx,float omegaIy,float omegaIz,float accel_weight,float renorm_val,float error_rp,float error_yaw)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
char buf[28];
_mav_put_float(buf, 0, omegaIx);
_mav_put_float(buf, 4, omegaIy);
_mav_put_float(buf, 8, omegaIz);
_mav_put_float(buf, 12, accel_weight);
_mav_put_float(buf, 16, renorm_val);
_mav_put_float(buf, 20, error_rp);
_mav_put_float(buf, 24, error_yaw);
memcpy(_MAV_PAYLOAD(msg), buf, 28);
#else
mavlink_ahrs_t packet;
packet.omegaIx = omegaIx;
packet.omegaIy = omegaIy;
packet.omegaIz = omegaIz;
packet.accel_weight = accel_weight;
packet.renorm_val = renorm_val;
packet.error_rp = error_rp;
packet.error_yaw = error_yaw;
memcpy(_MAV_PAYLOAD(msg), &packet, 28);
#endif
msg->msgid = MAVLINK_MSG_ID_AHRS;
return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 28, 127);
}
/**
* @brief Encode a ahrs struct into a message
*
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param msg The MAVLink message to compress the data into
* @param ahrs C-struct to read the message contents from
*/
static inline uint16_t mavlink_msg_ahrs_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_ahrs_t* ahrs)
{
return mavlink_msg_ahrs_pack(system_id, component_id, msg, ahrs->omegaIx, ahrs->omegaIy, ahrs->omegaIz, ahrs->accel_weight, ahrs->renorm_val, ahrs->error_rp, ahrs->error_yaw);
}
/**
* @brief Send a ahrs message
* @param chan MAVLink channel to send the message
*
* @param omegaIx X gyro drift estimate rad/s
* @param omegaIy Y gyro drift estimate rad/s
* @param omegaIz Z gyro drift estimate rad/s
* @param accel_weight average accel_weight
* @param renorm_val average renormalisation value
* @param error_rp average error_roll_pitch value
* @param error_yaw average error_yaw value
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_ahrs_send(mavlink_channel_t chan, float omegaIx, float omegaIy, float omegaIz, float accel_weight, float renorm_val, float error_rp, float error_yaw)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
char buf[28];
_mav_put_float(buf, 0, omegaIx);
_mav_put_float(buf, 4, omegaIy);
_mav_put_float(buf, 8, omegaIz);
_mav_put_float(buf, 12, accel_weight);
_mav_put_float(buf, 16, renorm_val);
_mav_put_float(buf, 20, error_rp);
_mav_put_float(buf, 24, error_yaw);
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS, buf, 28, 127);
#else
mavlink_ahrs_t packet;
packet.omegaIx = omegaIx;
packet.omegaIy = omegaIy;
packet.omegaIz = omegaIz;
packet.accel_weight = accel_weight;
packet.renorm_val = renorm_val;
packet.error_rp = error_rp;
packet.error_yaw = error_yaw;
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS, (const char *)&packet, 28, 127);
#endif
}
#endif
// MESSAGE AHRS UNPACKING
/**
* @brief Get field omegaIx from ahrs message
*
* @return X gyro drift estimate rad/s
*/
static inline float mavlink_msg_ahrs_get_omegaIx(const mavlink_message_t* msg)
{
return _MAV_RETURN_float(msg, 0);
}
/**
* @brief Get field omegaIy from ahrs message
*
* @return Y gyro drift estimate rad/s
*/
static inline float mavlink_msg_ahrs_get_omegaIy(const mavlink_message_t* msg)
{
return _MAV_RETURN_float(msg, 4);
}
/**
* @brief Get field omegaIz from ahrs message
*
* @return Z gyro drift estimate rad/s
*/
static inline float mavlink_msg_ahrs_get_omegaIz(const mavlink_message_t* msg)
{
return _MAV_RETURN_float(msg, 8);
}
/**
* @brief Get field accel_weight from ahrs message
*
* @return average accel_weight
*/
static inline float mavlink_msg_ahrs_get_accel_weight(const mavlink_message_t* msg)
{
return _MAV_RETURN_float(msg, 12);
}
/**
* @brief Get field renorm_val from ahrs message
*
* @return average renormalisation value
*/
static inline float mavlink_msg_ahrs_get_renorm_val(const mavlink_message_t* msg)
{
return _MAV_RETURN_float(msg, 16);
}
/**
* @brief Get field error_rp from ahrs message
*
* @return average error_roll_pitch value
*/
static inline float mavlink_msg_ahrs_get_error_rp(const mavlink_message_t* msg)
{
return _MAV_RETURN_float(msg, 20);
}
/**
* @brief Get field error_yaw from ahrs message
*
* @return average error_yaw value
*/
static inline float mavlink_msg_ahrs_get_error_yaw(const mavlink_message_t* msg)
{
return _MAV_RETURN_float(msg, 24);
}
/**
* @brief Decode a ahrs message into a struct
*
* @param msg The message to decode
* @param ahrs C-struct to decode the message contents into
*/
static inline void mavlink_msg_ahrs_decode(const mavlink_message_t* msg, mavlink_ahrs_t* ahrs)
{
#if MAVLINK_NEED_BYTE_SWAP
ahrs->omegaIx = mavlink_msg_ahrs_get_omegaIx(msg);
ahrs->omegaIy = mavlink_msg_ahrs_get_omegaIy(msg);
ahrs->omegaIz = mavlink_msg_ahrs_get_omegaIz(msg);
ahrs->accel_weight = mavlink_msg_ahrs_get_accel_weight(msg);
ahrs->renorm_val = mavlink_msg_ahrs_get_renorm_val(msg);
ahrs->error_rp = mavlink_msg_ahrs_get_error_rp(msg);
ahrs->error_yaw = mavlink_msg_ahrs_get_error_yaw(msg);
#else
memcpy(ahrs, _MAV_PAYLOAD(msg), 28);
#endif
}

26
libraries/GCS_MAVLink/include_v1.0/ardupilotmega/testsuite.h
View File

@ -668,12 +668,12 @@ static void mavlink_test_fence_status(uint8_t system_id, uint8_t component_id, m
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
}
static void mavlink_test_dcm(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
static void mavlink_test_ahrs(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
{
mavlink_message_t msg;
uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
uint16_t i;
mavlink_dcm_t packet_in = {
mavlink_ahrs_t packet_in = {
17.0,
45.0,
73.0,
@ -682,7 +682,7 @@ static void mavlink_test_dcm(uint8_t system_id, uint8_t component_id, mavlink_me
157.0,
185.0,
};
mavlink_dcm_t packet1, packet2;
mavlink_ahrs_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
packet1.omegaIx = packet_in.omegaIx;
packet1.omegaIy = packet_in.omegaIy;
@ -695,18 +695,18 @@ static void mavlink_test_dcm(uint8_t system_id, uint8_t component_id, mavlink_me
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_dcm_encode(system_id, component_id, &msg, &packet1);
mavlink_msg_dcm_decode(&msg, &packet2);
mavlink_msg_ahrs_encode(system_id, component_id, &msg, &packet1);
mavlink_msg_ahrs_decode(&msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_dcm_pack(system_id, component_id, &msg , packet1.omegaIx , packet1.omegaIy , packet1.omegaIz , packet1.accel_weight , packet1.renorm_val , packet1.error_rp , packet1.error_yaw );
mavlink_msg_dcm_decode(&msg, &packet2);
mavlink_msg_ahrs_pack(system_id, component_id, &msg , packet1.omegaIx , packet1.omegaIy , packet1.omegaIz , packet1.accel_weight , packet1.renorm_val , packet1.error_rp , packet1.error_yaw );
mavlink_msg_ahrs_decode(&msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_dcm_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.omegaIx , packet1.omegaIy , packet1.omegaIz , packet1.accel_weight , packet1.renorm_val , packet1.error_rp , packet1.error_yaw );
mavlink_msg_dcm_decode(&msg, &packet2);
mavlink_msg_ahrs_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.omegaIx , packet1.omegaIy , packet1.omegaIz , packet1.accel_weight , packet1.renorm_val , packet1.error_rp , packet1.error_yaw );
mavlink_msg_ahrs_decode(&msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
@ -714,12 +714,12 @@ static void mavlink_test_dcm(uint8_t system_id, uint8_t component_id, mavlink_me
for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) {
comm_send_ch(MAVLINK_COMM_0, buffer[i]);
}
mavlink_msg_dcm_decode(last_msg, &packet2);
mavlink_msg_ahrs_decode(last_msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_dcm_send(MAVLINK_COMM_1 , packet1.omegaIx , packet1.omegaIy , packet1.omegaIz , packet1.accel_weight , packet1.renorm_val , packet1.error_rp , packet1.error_yaw );
mavlink_msg_dcm_decode(last_msg, &packet2);
mavlink_msg_ahrs_send(MAVLINK_COMM_1 , packet1.omegaIx , packet1.omegaIy , packet1.omegaIz , packet1.accel_weight , packet1.renorm_val , packet1.error_rp , packet1.error_yaw );
mavlink_msg_ahrs_decode(last_msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
}
@ -841,7 +841,7 @@ static void mavlink_test_ardupilotmega(uint8_t system_id, uint8_t component_id,
mavlink_test_fence_point(system_id, component_id, last_msg);
mavlink_test_fence_fetch_point(system_id, component_id, last_msg);
mavlink_test_fence_status(system_id, component_id, last_msg);
mavlink_test_dcm(system_id, component_id, last_msg);
mavlink_test_ahrs(system_id, component_id, last_msg);
mavlink_test_simstate(system_id, component_id, last_msg);
mavlink_test_hwstatus(system_id, component_id, last_msg);
}

2
libraries/GCS_MAVLink/include_v1.0/ardupilotmega/version.h
View File

@ -5,7 +5,7 @@
#ifndef MAVLINK_VERSION_H
#define MAVLINK_VERSION_H
#define MAVLINK_BUILD_DATE "Sat Mar 10 09:38:13 2012"
#define MAVLINK_BUILD_DATE "Sun Mar 11 19:38:21 2012"
#define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 101

2
libraries/GCS_MAVLink/include_v1.0/common/version.h
View File

@ -5,7 +5,7 @@
#ifndef MAVLINK_VERSION_H
#define MAVLINK_VERSION_H
#define MAVLINK_BUILD_DATE "Sat Mar 10 09:38:13 2012"
#define MAVLINK_BUILD_DATE "Sun Mar 11 19:38:21 2012"
#define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 101

2
libraries/GCS_MAVLink/message_definitions/ardupilotmega.xml
View File

@ -226,7 +226,7 @@
<field name="breach_time" type="uint32_t">time of last breach in milliseconds since boot</field>
</message>
<message name="DCM" id="163">
<message name="AHRS" id="163">
<description>Status of DCM attitude estimator</description>
<field type="float" name="omegaIx">X gyro drift estimate rad/s</field>
<field type="float" name="omegaIy">Y gyro drift estimate rad/s</field>

2
libraries/GCS_MAVLink/message_definitions_v1.0/ardupilotmega.xml
View File

@ -226,7 +226,7 @@
<field name="breach_time" type="uint32_t">time of last breach in milliseconds since boot</field>
</message>
<message name="DCM" id="163">
<message name="AHRS" id="163">
<description>Status of DCM attitude estimator</description>
<field type="float" name="omegaIx">X gyro drift estimate rad/s</field>
<field type="float" name="omegaIy">Y gyro drift estimate rad/s</field>