aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Merge remote-tracking branch 'upstream/master'

Browse Source
This commit is contained in:
Rustom Jehangir 2016-03-01 10:50:52 -08:00
commit 77a84beb3f
25 changed files with 199 additions and 188 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
APMrover2/test.cpp
View File

@ -136,7 +136,7 @@ int8_t Rover::test_radio(uint8_t argc, const Menu::arg *argv)
int8_t Rover::test_failsafe(uint8_t argc, const Menu::arg *argv)
{
uint8_t fail_test;
uint8_t fail_test = 0;
print_hit_enter();
for(int i = 0; i < 50; i++){
delay(20);

2
ArduCopter/APM_Config.h
View File

@ -35,10 +35,10 @@
//#define OPTFLOW DISABLED // disable optical flow sensor to save 5K of flash space
//#define FRSKY_TELEM_ENABLED DISABLED // disable FRSky telemetry
//#define ADSB_ENABLED DISABLED // disable ADSB support
//#define PRECISION_LANDING DISABLED // disable precision landing using companion computer or IRLock sensor
// features below are disabled by default on all boards
//#define SPRAYER ENABLED // enable the crop sprayer feature (two ESC controlled pumps the speed of which depends upon the vehicle's horizontal velocity)
//#define PRECISION_LANDING ENABLED // enable precision landing using companion computer or IRLock sensor
//#define GNDEFFECT_COMPENSATION ENABLED // enable ground effect compensation for barometer (if propwash interferes with the barometer on the ground)
//#define CAL_ALWAYS_REBOOT // flight controller will reboot after compass or accelerometer calibration completes
//#define DISALLOW_GCS_MODE_CHANGE_DURING_RC_FAILSAFE // disable mode changes from GCS during Radio failsafes. Avoids a race condition for vehicle like Solo in which the RC and telemetry travel along the same link

2
ArduCopter/config.h
View File

@ -307,7 +307,7 @@
//////////////////////////////////////////////////////////////////////////////
// Precision Landing with companion computer or IRLock sensor
#ifndef PRECISION_LANDING
# define PRECISION_LANDING DISABLED
# define PRECISION_LANDING ENABLED
#endif
//////////////////////////////////////////////////////////////////////////////

23
ArduPlane/ArduPlane.cpp
View File

@ -307,9 +307,6 @@ void Plane::update_aux(void)
void Plane::one_second_loop()
{
if (should_log(MASK_LOG_CURRENT))
Log_Write_Current();
// send a heartbeat
gcs_send_message(MSG_HEARTBEAT);
@ -765,25 +762,15 @@ void Plane::update_navigation()
// on every loop
auto_state.checked_for_autoland = true;
}
// fall through to LOITER
if (g.rtl_radius < 0) {
loiter.direction = -1;
} else {
loiter.direction = 1;
}
radius = abs(g.rtl_radius);
if (radius > 0) {
loiter.direction = (g.rtl_radius < 0) ? -1 : 1;
}
// fall through to LOITER
case LOITER:
case GUIDED:
// allow loiter direction to be changed in flight
if (radius == 0) {
if (g.loiter_radius < 0) {
loiter.direction = -1;
} else {
loiter.direction = 1;
}
}
update_loiter(abs(radius));
update_loiter(radius);
break;
case CRUISE:

61
ArduPlane/GCS_Mavlink.cpp
View File

@ -510,54 +510,67 @@ void NOINLINE Plane::send_rpm(mavlink_channel_t chan)
void Plane::send_pid_tuning(mavlink_channel_t chan)
{
const Vector3f &gyro = ahrs.get_gyro();
const DataFlash_Class::PID_Info *pid_info;
if (g.gcs_pid_mask & 1) {
const DataFlash_Class::PID_Info &pid_info = rollController.get_pid_info();
if (quadplane.in_vtol_mode()) {
pid_info = &quadplane.pid_rate_roll.get_pid_info();
} else {
pid_info = &rollController.get_pid_info();
}
mavlink_msg_pid_tuning_send(chan, PID_TUNING_ROLL,
pid_info.desired,
pid_info->desired,
degrees(gyro.x),
pid_info.FF,
pid_info.P,
pid_info.I,
pid_info.D);
pid_info->FF,
pid_info->P,
pid_info->I,
pid_info->D);
if (!HAVE_PAYLOAD_SPACE(chan, PID_TUNING)) {
return;
}
}
if (g.gcs_pid_mask & 2) {
const DataFlash_Class::PID_Info &pid_info = pitchController.get_pid_info();
if (quadplane.in_vtol_mode()) {
pid_info = &quadplane.pid_rate_pitch.get_pid_info();
} else {
pid_info = &pitchController.get_pid_info();
}
mavlink_msg_pid_tuning_send(chan, PID_TUNING_PITCH,
pid_info.desired,
pid_info->desired,
degrees(gyro.y),
pid_info.FF,
pid_info.P,
pid_info.I,
pid_info.D);
pid_info->FF,
pid_info->P,
pid_info->I,
pid_info->D);
if (!HAVE_PAYLOAD_SPACE(chan, PID_TUNING)) {
return;
}
}
if (g.gcs_pid_mask & 4) {
const DataFlash_Class::PID_Info &pid_info = yawController.get_pid_info();
if (quadplane.in_vtol_mode()) {
pid_info = &quadplane.pid_rate_yaw.get_pid_info();
} else {
pid_info = &yawController.get_pid_info();
}
mavlink_msg_pid_tuning_send(chan, PID_TUNING_YAW,
pid_info.desired,
pid_info->desired,
degrees(gyro.z),
pid_info.FF,
pid_info.P,
pid_info.I,
pid_info.D);
pid_info->FF,
pid_info->P,
pid_info->I,
pid_info->D);
if (!HAVE_PAYLOAD_SPACE(chan, PID_TUNING)) {
return;
}
}
if (g.gcs_pid_mask & 8) {
const DataFlash_Class::PID_Info &pid_info = steerController.get_pid_info();
pid_info = &steerController.get_pid_info();
mavlink_msg_pid_tuning_send(chan, PID_TUNING_STEER,
pid_info.desired,
pid_info->desired,
degrees(gyro.z),
pid_info.FF,
pid_info.P,
pid_info.I,
pid_info.D);
pid_info->FF,
pid_info->P,
pid_info->I,
pid_info->D);
if (!HAVE_PAYLOAD_SPACE(chan, PID_TUNING)) {
return;
}

4
ArduPlane/Log.cpp
View File

@ -329,10 +329,10 @@ void Plane::Log_Write_Status()
,is_flying : is_flying()
,is_flying_probability : isFlyingProbability
,armed : hal.util->get_soft_armed()
,safety : hal.util->safety_switch_state()
,safety : static_cast<uint8_t>(hal.util->safety_switch_state())
,is_crashed : crash_state.is_crashed
,is_still : plane.ins.is_still()
,stage : flight_stage
,stage : static_cast<uint8_t>(flight_stage)
,impact : crash_state.impact_detected
};

20
ArduPlane/commands_logic.cpp
View File

@ -601,17 +601,20 @@ bool Plane::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
bool Plane::verify_loiter_unlim()
{
if (g.rtl_radius < 0) {
loiter.direction = -1;
} else {
loiter.direction = 1;
}
if (mission.get_current_nav_cmd().p1 <= 1 && abs(g.rtl_radius) > 1) {
// if mission radius is 0,1, and rtl_radius is valid, use rtl_radius.
loiter.direction = (g.rtl_radius < 0) ? -1 : 1;
update_loiter(abs(g.rtl_radius));
} else {
// else use mission radius
update_loiter(mission.get_current_nav_cmd().p1);
}
return false;
}
bool Plane::verify_loiter_time()
{
// mission radius is always g.loiter_radius
update_loiter(0);
if (loiter.start_time_ms == 0) {
if (nav_controller->reached_loiter_target()) {
@ -627,7 +630,9 @@ bool Plane::verify_loiter_time()
bool Plane::verify_loiter_turns()
{
update_loiter(0);
uint16_t radius = HIGHBYTE(mission.get_current_nav_cmd().p1);
update_loiter(radius);
if (loiter.sum_cd > loiter.total_cd) {
loiter.total_cd = 0;
gcs_send_text(MAV_SEVERITY_WARNING,"Verify nav: LOITER orbits complete");
@ -644,7 +649,8 @@ bool Plane::verify_loiter_turns()
*/
bool Plane::verify_loiter_to_alt()
{
update_loiter(0);
uint16_t radius = HIGHBYTE(mission.get_current_nav_cmd().p1);
update_loiter(radius);
//has target altitude been reached?
if (condition_value != 0) {

6
ArduPlane/navigation.cpp
View File

@ -129,8 +129,10 @@ void Plane::calc_gndspeed_undershoot()
void Plane::update_loiter(uint16_t radius)
{
if (radius == 0) {
radius = abs(g.loiter_radius);
if (radius <= 1) {
// if radius is <=1 then use the general loiter radius. if it's small, use default
radius = (abs(g.loiter_radius <= 1)) ? LOITER_RADIUS_DEFAULT : abs(g.loiter_radius);
loiter.direction = (g.loiter_radius < 0) ? -1 : 1;
}
if (loiter.start_time_ms == 0 &&

6
ArduPlane/quadplane.h
View File

@ -60,8 +60,8 @@ public:
private:
AP_AHRS_NavEKF &ahrs;
AP_Vehicle::MultiCopter aparm;
AC_PID pid_rate_roll {0.15, 0.1, 0.004, 2000, 20, 0.02};
AC_PID pid_rate_pitch{0.15, 0.1, 0.004, 2000, 20, 0.02};
AC_PID pid_rate_roll {0.25, 0.1, 0.004, 2000, 20, 0.02};
AC_PID pid_rate_pitch{0.25, 0.1, 0.004, 2000, 20, 0.02};
AC_PID pid_rate_yaw {0.15, 0.1, 0.004, 2000, 20, 0.02};
AC_P p_stabilize_roll{4.5};
AC_P p_stabilize_pitch{4.5};
@ -72,7 +72,7 @@ private:
AC_P p_pos_xy{1};
AC_P p_alt_hold{1};
AC_P p_vel_z{5};
AC_PID pid_accel_z{0.5, 1, 0, 800, 20, 0.02};
AC_PID pid_accel_z{0.3, 1, 0, 800, 20, 0.02};
AC_PI_2D pi_vel_xy{1.0, 0.5, 1000, 5, 0.02};
AP_Int8 frame_class;

4
ArduPlane/sensors.cpp
View File

@ -128,6 +128,10 @@ void Plane::read_battery(void)
battery.exhausted(g.fs_batt_voltage, g.fs_batt_mah)) {
low_battery_event();
}
if (should_log(MASK_LOG_CURRENT)) {
Log_Write_Current();
}
}
// read the receiver RSSI as an 8 bit number for MAVLink

2
ArduPlane/test.cpp
View File

@ -141,7 +141,7 @@ int8_t Plane::test_radio(uint8_t argc, const Menu::arg *argv)
int8_t Plane::test_failsafe(uint8_t argc, const Menu::arg *argv)
{
uint8_t fail_test;
uint8_t fail_test = 0;
print_hit_enter();
for(int16_t i = 0; i < 50; i++) {
hal.scheduler->delay(20);

2
Tools/scripts/install-prereqs-ubuntu.sh
View File

@ -11,7 +11,7 @@ PX4_PKGS="python-serial python-argparse openocd flex bison libncurses5-dev \
autoconf texinfo build-essential libftdi-dev libtool zlib1g-dev \
zip genromfs python-empy"
BEBOP_PKGS="g++-arm-linux-gnueabihf"
UBUNTU64_PKGS="libc6:i386 libgcc1:i386 gcc-4.6-base:i386 libstdc++5:i386 libstdc++6:i386"
UBUNTU64_PKGS="libc6:i386 libgcc1:i386 gcc-4.9-base:i386 libstdc++5:i386 libstdc++6:i386"
ASSUME_YES=false
# GNU Tools for ARM Embedded Processors

2
Tools/vagrant/initvagrant.sh
View File

@ -10,7 +10,7 @@ PYTHON_PKGS="pymavlink MAVProxy droneapi"
PX4_PKGS="python-serial python-argparse openocd flex bison libncurses5-dev \
autoconf texinfo build-essential libftdi-dev libtool zlib1g-dev \
zip genromfs"
UBUNTU64_PKGS="libc6:i386 libgcc1:i386 gcc-4.6-base:i386 libstdc++5:i386 libstdc++6:i386"
UBUNTU64_PKGS="libc6:i386 libgcc1:i386 gcc-4.9-base:i386 libstdc++5:i386 libstdc++6:i386"
# GNU Tools for ARM Embedded Processors
# (see https://launchpad.net/gcc-arm-embedded/)

2
libraries/APM_Control/AP_AutoTune.cpp
View File

@ -337,7 +337,7 @@ void AP_AutoTune::write_log(float servo, float demanded, float achieved)
struct log_ATRP pkt = {
LOG_PACKET_HEADER_INIT(LOG_ATRP_MSG),
time_us : AP_HAL::micros64(),
type : type,
type : static_cast<uint8_t>(type),
state : (uint8_t)state,
servo : (int16_t)(servo*100),
demanded : demanded,

9
libraries/AP_HAL_SITL/SITL_State.cpp
View File

@ -24,7 +24,6 @@
extern const AP_HAL::HAL& hal;
using namespace HALSITL;
using namespace SITL;
void SITL_State::_set_param_default(const char *parm)
{
@ -93,10 +92,10 @@ void SITL_State::_sitl_setup(const char *home_str)
_update_gps(0, 0, 0, 0, 0, 0, false);
#endif
if (enable_gimbal) {
gimbal = new Gimbal(_sitl->state);
gimbal = new SITL::Gimbal(_sitl->state);
}
if (enable_ADSB) {
adsb = new ADSB(_sitl->state, home_str);
adsb = new SITL::ADSB(_sitl->state, home_str);
}
fg_socket.connect("127.0.0.1", 5503);
@ -254,7 +253,7 @@ void SITL_State::_output_to_flightgear(void)
*/
void SITL_State::_fdm_input_local(void)
{
Aircraft::sitl_input input;
SITL::Aircraft::sitl_input input;
// check for direct RC input
_fdm_input();
@ -315,7 +314,7 @@ void SITL_State::_apply_servo_filter(float deltat)
/*
create sitl_input structure for sending to FDM
*/
void SITL_State::_simulator_servos(Aircraft::sitl_input &input)
void SITL_State::_simulator_servos(SITL::Aircraft::sitl_input &input)
{
static uint32_t last_update_usec;

15
libraries/AP_Mission/AP_Mission.cpp
View File

@ -505,6 +505,7 @@ MAV_MISSION_RESULT AP_Mission::mavlink_to_mission_cmd(const mavlink_mission_item
case MAV_CMD_NAV_LOITER_UNLIM: // MAV ID: 17
copy_location = true;
cmd.p1 = fabsf(packet.param3); // store radius as 16bit since no other params are competing for space
cmd.content.location.flags.loiter_ccw = (packet.param3 < 0); // -1 = counter clockwise, +1 = clockwise
break;
@ -518,7 +519,7 @@ MAV_MISSION_RESULT AP_Mission::mavlink_to_mission_cmd(const mavlink_mission_item
case MAV_CMD_NAV_LOITER_TIME: // MAV ID: 19
copy_location = true;
cmd.p1 = packet.param1; // loiter time in seconds
cmd.p1 = packet.param1; // loiter time in seconds uses all 16 bits, 8bit seconds is too small. No room for radius.
cmd.content.location.flags.loiter_ccw = (packet.param3 < 0);
break;
@ -546,18 +547,10 @@ MAV_MISSION_RESULT AP_Mission::mavlink_to_mission_cmd(const mavlink_mission_item
case MAV_CMD_NAV_LOITER_TO_ALT: // MAV ID: 31
copy_location = true;
heading_req = packet.param1; //heading towards next waypoint required (0 = False)
cmd.content.location.flags.loiter_ccw = (packet.param2 < 0);
//Don't give users the impression I can set the radius size.
//I can only set the direction at this time and so can every
//other command, despite what is implied (I'm looking at YOU
//NAV_LOITER_TURNS):
radius_m = 1;
radius_m = fabsf(packet.param2); // radius in meters is held in high in param2
cmd.p1 = (((uint16_t)radius_m)<<8) | (uint16_t)heading_req; // store "radius" in high byte of p1, heading_req in low byte of p1
cmd.content.location.flags.loiter_ccw = (packet.param2 < 0);
break;
case MAV_CMD_NAV_SPLINE_WAYPOINT: // MAV ID: 82

10
libraries/AP_Mount/SoloGimbal.cpp
View File

@ -29,7 +29,7 @@ bool SoloGimbal::aligned()
gimbal_mode_t SoloGimbal::get_mode()
{
if ((_gimbalParams.initialized() && _gimbalParams.get_K_rate()==0.0f) || (_ahrs.get_rotation_body_to_ned().c.z < 0 && !(_lockedToBody || _calibrator.running()))) {
if ((_gimbalParams.initialized() && is_zero(_gimbalParams.get_K_rate())) || (_ahrs.get_rotation_body_to_ned().c.z < 0 && !(_lockedToBody || _calibrator.running()))) {
return GIMBAL_MODE_IDLE;
} else if (!_ekf.getStatus()) {
return GIMBAL_MODE_POS_HOLD;
@ -145,7 +145,7 @@ void SoloGimbal::send_controls(mavlink_channel_t chan)
// set GMB_MAX_TORQUE
float max_torque;
_gimbalParams.get_param(GMB_PARAM_GMB_MAX_TORQUE, max_torque, 0);
if (max_torque != _max_torque && max_torque != 0) {
if (!is_equal(max_torque,_max_torque) && !is_zero(max_torque)) {
_max_torque = max_torque;
}
@ -177,9 +177,9 @@ void SoloGimbal::extract_feedback(const mavlink_gimbal_report_t& report_msg)
_measurement.joint_angles -= _gimbalParams.get_joint_bias();
_measurement.delta_velocity -= _gimbalParams.get_accel_bias() * _measurement.delta_time;
Vector3f accel_gain = _gimbalParams.get_accel_gain();
_measurement.delta_velocity.x *= accel_gain.x == 0.0f ? 1.0f : accel_gain.x;
_measurement.delta_velocity.y *= accel_gain.y == 0.0f ? 1.0f : accel_gain.y;
_measurement.delta_velocity.z *= accel_gain.z == 0.0f ? 1.0f : accel_gain.z;
_measurement.delta_velocity.x *= (is_zero(accel_gain.x) ? 1.0f : accel_gain.x);
_measurement.delta_velocity.y *= (is_zero(accel_gain.y) ? 1.0f : accel_gain.y);
_measurement.delta_velocity.z *= (is_zero(accel_gain.z) ? 1.0f : accel_gain.z);
// update _ang_vel_mag_filt, used for accel sample readiness
Vector3f ang_vel = _measurement.delta_angles / _measurement.delta_time;

10
libraries/AP_Mount/SoloGimbal.h
View File

@ -48,12 +48,13 @@ public:
_state(GIMBAL_STATE_NOT_PRESENT),
_yaw_rate_ff_ef_filt(0.0f),
_vehicle_yaw_rate_ef_filt(0.0f),
_lockedToBody(false),
_vehicle_delta_angles(),
_vehicle_to_gimbal_quat(),
_vehicle_to_gimbal_quat_filt(),
_filtered_joint_angles(),
_last_report_msg_ms(0),
_max_torque(5000.0f),
_ang_vel_mag_filt(0),
_lockedToBody(false),
_log_dt(0),
_log_del_ang(),
_log_del_vel()
@ -110,6 +111,9 @@ private:
bool joints_near_limits();
// private member variables
SoloGimbalEKF _ekf; // state of small EKF for gimbal
const AP_AHRS_NavEKF &_ahrs; // Main EKF
gimbal_state_t _state;
struct {
@ -144,8 +148,6 @@ private:
bool _lockedToBody;
SoloGimbalEKF _ekf; // state of small EKF for gimbal
const AP_AHRS_NavEKF &_ahrs; // Main EKF
SoloGimbal_Parameters _gimbalParams;
AccelCalibrator _calibrator;

176
libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp
View File

@ -152,7 +152,7 @@ void NavEKF2_core::SelectMagFusion()
if (dataReady) {
// If we haven't performed the first airborne magnetic field update or have inhibited magnetic field learning, then we use the simple method of declination to maintain heading
if(inhibitMagStates) {
fuseCompass();
fuseEulerYaw();
// zero the test ratio output from the inactive 3-axis magneteometer fusion
magTestRatio.zero();
} else {
@ -603,79 +603,121 @@ void NavEKF2_core::FuseMagnetometer()
* It is not as robust to magneometer failures.
* It is not suitable for operation where the horizontal magnetic field strength is weak (within 30 degreees latitude of the the magnetic poles)
*/
void NavEKF2_core::fuseCompass()
void NavEKF2_core::fuseEulerYaw()
{
float q0 = stateStruct.quat[0];
float q1 = stateStruct.quat[1];
float q2 = stateStruct.quat[2];
float q3 = stateStruct.quat[3];
float magX = magDataDelayed.mag.x;
float magY = magDataDelayed.mag.y;
float magZ = magDataDelayed.mag.z;
// compass measurement error variance (rad^2)
const float R_MAG = 3e-2f;
const float R_YAW = 0.25f;
// calculate intermediate variables for observation jacobian
// calculate observation jacobian, predicted yaw and zero yaw body to earth rotation matrix
// determine if a 321 or 312 Euler sequence is best
float predicted_yaw;
float H_YAW[3];
Matrix3f Tbn_zeroYaw;
if (fabsf(prevTnb[0][2]) < fabsf(prevTnb[1][2])) {
// calculate observation jacobian when we are observing the first rotation in a 321 sequence
float t2 = q0*q0;
float t3 = q1*q1;
float t4 = q2*q2;
float t5 = q3*q3;
float t6 = q0*q3*2.0f;
float t8 = t2-t3+t4-t5;
float t9 = q0*q1*2.0f;
float t10 = q2*q3*2.0f;
float t11 = t9-t10;
float t14 = q1*q2*2.0f;
float t21 = magY*t8;
float t22 = t6+t14;
float t23 = magX*t22;
float t24 = magZ*t11;
float t7 = t21+t23-t24;
float t12 = t2+t3-t4-t5;
float t13 = magX*t12;
float t15 = q0*q2*2.0f;
float t16 = q1*q3*2.0f;
float t17 = t15+t16;
float t18 = magZ*t17;
float t19 = t6-t14;
float t25 = magY*t19;
float t20 = t13+t18-t25;
if (fabsf(t20) < 1e-6f) {
float t6 = t2+t3-t4-t5;
float t7 = q0*q3*2.0f;
float t8 = q1*q2*2.0f;
float t9 = t7+t8;
float t10 = sq(t6);
if (t10 > 1e-6f) {
t10 = 1.0f / t10;
} else {
return;
}
float t26 = 1.0f/(t20*t20);
float t27 = t7*t7;
float t28 = t26*t27;
float t29 = t28+1.0;
if (fabsf(t29) < 1e-12f) {
float t11 = t9*t9;
float t12 = t10*t11;
float t13 = t12+1.0f;
float t14;
if (fabsf(t13) > 1e-3f) {
t14 = 1.0f/t13;
} else {
return;
}
float t30 = 1.0f/t29;
if (fabsf(t20) < 1e-12f) {
return;
}
float t31 = 1.0f/t20;
float t15 = 1.0f/t6;
H_YAW[0] = 0.0f;
H_YAW[1] = t14*(t15*(q0*q1*2.0f-q2*q3*2.0f)+t9*t10*(q0*q2*2.0f+q1*q3*2.0f));
H_YAW[2] = t14*(t15*(t2-t3+t4-t5)+t9*t10*(t7-t8));
// calculate observation jacobian
float H_MAG[3];
H_MAG[0] = -t30*(t31*(magZ*t8+magY*t11)+t7*t26*(magY*t17+magZ*t19));
H_MAG[1] = t30*(t31*(magX*t11+magZ*t22)-t7*t26*(magZ*t12-magX*t17));
H_MAG[2] = t30*(t31*(magX*t8-magY*t22)+t7*t26*(magY*t12+magX*t19));
// Get the 321 euler angles
Vector3f euler321;
stateStruct.quat.to_euler(euler321.x, euler321.y, euler321.z);
predicted_yaw = euler321.z;
// set the yaw to zero and calculate the zero yaw rotation from body to earth frame
Tbn_zeroYaw.from_euler(euler321.x, euler321.y, 0.0f);
} else {
// calculate observaton jacobian when we are observing a rotation in a 312 sequence
float t2 = q0*q0;
float t3 = q1*q1;
float t4 = q2*q2;
float t5 = q3*q3;
float t6 = t2-t3+t4-t5;
float t7 = q0*q3*2.0f;
float t10 = q1*q2*2.0f;
float t8 = t7-t10;
float t9 = sq(t6);
if (t9 > 1e-6f) {
t9 = 1.0f/t9;
} else {
return;
}
float t11 = t8*t8;
float t12 = t9*t11;
float t13 = t12+1.0f;
float t14;
if (fabsf(t13) > 1e-3f) {
t14 = 1.0f/t13;
} else {
return;
}
float t15 = 1.0f/t6;
H_YAW[0] = -t14*(t15*(q0*q2*2.0+q1*q3*2.0)-t8*t9*(q0*q1*2.0-q2*q3*2.0));
H_YAW[1] = 0.0f;
H_YAW[2] = t14*(t15*(t2+t3-t4-t5)+t8*t9*(t7+t10));
// Get the 321 euler angles
Vector3f euler312 = stateStruct.quat.to_vector312();
predicted_yaw = euler312.z;
// set the yaw to zero and calculate the zero yaw rotation from body to earth frame
Tbn_zeroYaw.from_euler312(euler312.x, euler312.y, 0.0f);
}
// rotate measured mag components into earth frame
Vector3f magMeasNED = Tbn_zeroYaw*magDataDelayed.mag;
// Use the difference between the horizontal projection and declination to give the measured yaw
float measured_yaw = wrap_PI(-atan2f(magMeasNED.y, magMeasNED.x) + _ahrs->get_compass()->get_declination());
// Calculate the innovation
float innovation = wrap_PI(predicted_yaw - measured_yaw);
// Copy raw value to output variable used for data logging
innovYaw = innovation;
// Calculate innovation variance and Kalman gains, taking advantage of the fact that only the first 3 elements in H are non zero
float PH[3];
float varInnov = R_MAG;
float varInnov = R_YAW;
for (uint8_t rowIndex=0; rowIndex<=2; rowIndex++) {
PH[rowIndex] = 0.0f;
for (uint8_t colIndex=0; colIndex<=2; colIndex++) {
PH[rowIndex] += P[rowIndex][colIndex]*H_MAG[colIndex];
PH[rowIndex] += P[rowIndex][colIndex]*H_YAW[colIndex];
}
varInnov += H_MAG[rowIndex]*PH[rowIndex];
varInnov += H_YAW[rowIndex]*PH[rowIndex];
}
float varInnovInv;
if (varInnov >= R_MAG) {
if (varInnov >= R_YAW) {
varInnovInv = 1.0f / varInnov;
// All three magnetometer components are used in this measurement, so we output health status on three axes
faultStatus.bad_xmag = false;
@ -694,17 +736,11 @@ void NavEKF2_core::fuseCompass()
for (uint8_t rowIndex=0; rowIndex<=stateIndexLim; rowIndex++) {
Kfusion[rowIndex] = 0.0f;
for (uint8_t colIndex=0; colIndex<=2; colIndex++) {
Kfusion[rowIndex] += P[rowIndex][colIndex]*H_MAG[colIndex];
Kfusion[rowIndex] += P[rowIndex][colIndex]*H_YAW[colIndex];
}
Kfusion[rowIndex] *= varInnovInv;
}
// Calculate the innovation
float innovation = calcMagHeadingInnov();
// Copy raw value to output variable used for data logging
innovYaw = innovation;
// calculate the innovation test ratio
yawTestRatio = sq(innovation) / (sq(MAX(0.01f * (float)frontend->_magInnovGate, 1.0f)) * varInnov);
@ -742,7 +778,7 @@ void NavEKF2_core::fuseCompass()
for (uint8_t colIndex=0; colIndex<=stateIndexLim; colIndex++) {
HP[colIndex] = 0.0f;
for (uint8_t rowIndex=0; rowIndex<=2; rowIndex++) {
HP[colIndex] += H_MAG[rowIndex]*P[rowIndex][colIndex];
HP[colIndex] += H_YAW[rowIndex]*P[rowIndex][colIndex];
}
}
for (uint8_t rowIndex=0; rowIndex<=stateIndexLim; rowIndex++) {
@ -865,34 +901,6 @@ void NavEKF2_core::FuseDeclination()
}
// Calculate magnetic heading declination innovation
float NavEKF2_core::calcMagHeadingInnov()
{
// rotate measured body components into earth axis
Matrix3f Tbn_temp;
stateStruct.quat.rotation_matrix(Tbn_temp);
Vector3f magMeasNED = Tbn_temp*magDataDelayed.mag;
// the observation is the declination angle of the earth field from the compass library
// the prediction is the azimuth angle of the projection of the measured field onto the horizontal
float innovation = atan2f(magMeasNED.y,magMeasNED.x) - _ahrs->get_compass()->get_declination();
// wrap the innovation so it sits on the range from +-pi
innovation = wrap_PI(innovation);
// Unwrap so that a large yaw gyro bias offset that causes the heading to wrap does not lead to continual uncontrolled heading drift
if (innovation - lastInnovation > M_PI) {
// Angle has wrapped in the positive direction to subtract an additional 2*Pi
innovationIncrement -= M_2PI;
} else if (innovation -innovationIncrement < -M_PI) {
// Angle has wrapped in the negative direction so add an additional 2*Pi
innovationIncrement += M_2PI;
}
lastInnovation = innovation;
return innovation + innovationIncrement;
}
/********************************************************
* MISC FUNCTIONS *
********************************************************/

2
libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp
View File

@ -552,7 +552,7 @@ void NavEKF2_core::selectHeightForFusion()
// determine if we should be using a height source other than baro
bool usingRangeForHgt = (frontend->_altSource == 1 && imuSampleTime_ms - rngValidMeaTime_ms < 500 && frontend->_fusionModeGPS == 3);
bool usingGpsForHgt = (frontend->_altSource == 2 && imuSampleTime_ms - lastTimeGpsReceived_ms < 500 && validOrigin);
bool usingGpsForHgt = (frontend->_altSource == 2 && imuSampleTime_ms - lastTimeGpsReceived_ms < 500 && validOrigin && gpsAccuracyGood);
// if there is new baro data to fuse, calculate filterred baro data required by other processes
if (baroDataToFuse) {

5
libraries/AP_NavEKF2/AP_NavEKF2_core.h
View File

@ -595,14 +595,11 @@ private:
void alignMagStateDeclination();
// Fuse compass measurements using a simple declination observation (doesn't require magnetic field states)
void fuseCompass();
void fuseEulerYaw();
// Fuse declination angle to keep earth field declination from changing when we don't have earth relative observations.
void FuseDeclination();
// Calculate compass heading innovation
float calcMagHeadingInnov();
// Propagate PVA solution forward from the fusion time horizon to the current time horizon
// using a simple observer
void calcOutputStatesFast();

2
libraries/AP_RPM/RPM_SITL.cpp
View File

@ -25,7 +25,7 @@ extern const AP_HAL::HAL& hal;
open the sensor in constructor
*/
AP_RPM_SITL::AP_RPM_SITL(AP_RPM &_ap_rpm, uint8_t _instance, AP_RPM::RPM_State &_state) :
AP_RPM_Backend(_ap_rpm, instance, _state)
AP_RPM_Backend(_ap_rpm, _instance, _state)
{
sitl = (SITL::SITL *)AP_Param::find_object("SIM_");
instance = _instance;

2
libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.cpp
View File

@ -62,7 +62,7 @@ bool AP_RangeFinder_LightWareSerial::get_reading(uint16_t &reading_cm)
char c = uart->read();
if (c == '\r') {
linebuf[linebuf_len] = 0;
sum += atof(linebuf);
sum += (float)atof(linebuf);
count++;
linebuf_len = 0;
} else if (isdigit(c) || c == '.') {

2
libraries/DataFlash/LogFile.cpp
View File

@ -1555,7 +1555,7 @@ void DataFlash_Class::Log_Write_CameraInfo(enum LogMessages msg, const AP_AHRS &
}
struct log_Camera pkt = {
LOG_PACKET_HEADER_INIT(msg),
LOG_PACKET_HEADER_INIT(static_cast<uint8_t>(msg)),
time_us : AP_HAL::micros64(),
gps_time : gps.time_week_ms(),
gps_week : gps.time_week(),

2
libraries/SITL/SIM_Tracker.cpp
View File

@ -72,7 +72,7 @@ void Tracker::update(const struct sitl_input &input)
// how much time has passed?
float delta_time = frame_time_us * 1.0e-6f;
float yaw_rate, pitch_rate;
float yaw_rate = 0.0f, pitch_rate = 0.0f;
yaw_input = (input.servos[0]-1500)/500.0f;
pitch_input = (input.servos[1]-1500)/500.0f;