diff --git a/.gitmodules b/.gitmodules index 7adbe483cee63..8183378d7e65f 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,6 +1,6 @@ [submodule "modules/PX4Firmware"] path = modules/PX4Firmware - url = git://github.com/ArduPilot/PX4Firmware.git + url = git://github.com/jschall/PX4Firmware.git [submodule "modules/PX4NuttX"] path = modules/PX4NuttX url = git://github.com/ArduPilot/PX4NuttX.git diff --git a/ArduCopter/ArduCopter.cpp b/ArduCopter/ArduCopter.cpp index 02dc3566126f1..f25f79adfb22e 100644 --- a/ArduCopter/ArduCopter.cpp +++ b/ArduCopter/ArduCopter.cpp @@ -102,7 +102,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = { SCHED_TASK(compass_accumulate, 100, 100), SCHED_TASK(barometer_accumulate, 50, 90), #if PRECISION_LANDING == ENABLED - SCHED_TASK(update_precland, 50, 50), + SCHED_TASK(update_precland, 400, 50), #endif #if FRAME_CONFIG == HELI_FRAME SCHED_TASK(check_dynamic_flight, 50, 75), @@ -428,6 +428,11 @@ void Copter::twentyfive_hz_logging() DataFlash.Log_Write_IMU(ins); } #endif + +#if PRECISION_LANDING == ENABLED + // log output + Log_Write_Precland(); +#endif } void Copter::dataflash_periodic(void) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 74faecad0a01d..1546633096646 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -813,7 +813,8 @@ class Copter : public AP_HAL::HAL::Callbacks { void land_run(); void land_gps_run(); void land_nogps_run(); - float get_land_descent_speed(); + void land_run_vertical_control(bool pause_descent = false); + void land_run_horizontal_control(); void land_do_not_use_GPS(); void set_mode_land_with_pause(mode_reason_t reason); bool landing_with_GPS(); diff --git a/ArduCopter/control_auto.cpp b/ArduCopter/control_auto.cpp index 9a1c9e4cc60dc..f096073f06e10 100644 --- a/ArduCopter/control_auto.cpp +++ b/ArduCopter/control_auto.cpp @@ -360,8 +360,9 @@ void Copter::auto_land_start(const Vector3f& destination) // initialise loiter target destination wp_nav.init_loiter_target(destination); - // initialise altitude target to stopping point - pos_control.set_target_to_stopping_point_z(); + // initialise position and desired velocity + pos_control.set_alt_target(inertial_nav.get_altitude()); + pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z()); // initialise yaw set_auto_yaw_mode(AUTO_YAW_HOLD); @@ -371,11 +372,8 @@ void Copter::auto_land_start(const Vector3f& destination) // called by auto_run at 100hz or more void Copter::auto_land_run() { - int16_t roll_control = 0, pitch_control = 0; - float target_yaw_rate = 0; - - // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately - if (!motors.armed() || !ap.auto_armed || ap.land_complete) { + // if not auto armed or landed or motor interlock not enabled set throttle to zero and exit immediately + if (!motors.armed() || !ap.auto_armed || ap.land_complete || !motors.get_interlock()) { #if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw // call attitude controller attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain()); @@ -390,64 +388,11 @@ void Copter::auto_land_run() return; } - // relax loiter targets if we might be landed - if (ap.land_complete_maybe) { - wp_nav.loiter_soften_for_landing(); - } - - // process pilot's input - if (!failsafe.radio) { - if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){ - Log_Write_Event(DATA_LAND_CANCELLED_BY_PILOT); - // exit land if throttle is high - if (!set_mode(LOITER, MODE_REASON_THROTTLE_LAND_ESCAPE)) { - set_mode(ALT_HOLD, MODE_REASON_THROTTLE_LAND_ESCAPE); - } - } - - if (g.land_repositioning) { - // apply SIMPLE mode transform to pilot inputs - update_simple_mode(); - - // process pilot's roll and pitch input - roll_control = channel_roll->get_control_in(); - pitch_control = channel_pitch->get_control_in(); - } - - // get pilot's desired yaw rate - target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); - } - // set motors to full range motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); - - // process roll, pitch inputs - wp_nav.set_pilot_desired_acceleration(roll_control, pitch_control); - -#if PRECISION_LANDING == ENABLED - // run precision landing - if (!ap.land_repo_active && precland.target_acquired() && precland_last_update_ms != precland.last_update_ms()) { - Vector3f target_pos; - precland.get_target_position(target_pos); - pos_control.set_xy_target(target_pos.x, target_pos.y); - pos_control.freeze_ff_xy(); - precland_last_update_ms = precland.last_update_ms(); - } -#endif - - // run loiter controller - wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler); - - // call z-axis position controller - float cmb_rate = get_land_descent_speed(); - pos_control.set_alt_target_from_climb_rate(cmb_rate, G_Dt, true); - pos_control.update_z_controller(); - - // record desired climb rate for logging - desired_climb_rate = cmb_rate; - - // roll & pitch from waypoint controller, yaw rate from pilot - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain()); + + land_run_horizontal_control(); + land_run_vertical_control(); } // auto_rtl_start - initialises RTL in AUTO flight mode diff --git a/ArduCopter/control_land.cpp b/ArduCopter/control_land.cpp index 6d6bde5372ad2..dd4ef644db9a0 100644 --- a/ArduCopter/control_land.cpp +++ b/ArduCopter/control_land.cpp @@ -23,9 +23,10 @@ bool Copter::land_init(bool ignore_checks) pos_control.set_speed_z(wp_nav.get_speed_down(), wp_nav.get_speed_up()); pos_control.set_accel_z(wp_nav.get_accel_z()); - // initialise altitude target to stopping point - pos_control.set_target_to_stopping_point_z(); - + // initialise position and desired velocity + pos_control.set_alt_target(inertial_nav.get_altitude()); + pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z()); + land_start_time = millis(); land_pause = false; @@ -52,9 +53,6 @@ void Copter::land_run() // should be called at 100hz or more void Copter::land_gps_run() { - int16_t roll_control = 0, pitch_control = 0; - float target_yaw_rate = 0; - // if not auto armed or landed or motor interlock not enabled set throttle to zero and exit immediately if (!motors.armed() || !ap.auto_armed || ap.land_complete || !motors.get_interlock()) { #if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw @@ -81,78 +79,17 @@ void Copter::land_gps_run() #endif return; } - - // relax loiter target if we might be landed - if (ap.land_complete_maybe) { - wp_nav.loiter_soften_for_landing(); - } - - // process pilot inputs - if (!failsafe.radio) { - if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){ - Log_Write_Event(DATA_LAND_CANCELLED_BY_PILOT); - // exit land if throttle is high - if (!set_mode(LOITER, MODE_REASON_THROTTLE_LAND_ESCAPE)) { - set_mode(ALT_HOLD, MODE_REASON_THROTTLE_LAND_ESCAPE); - } - } - - if (g.land_repositioning) { - // apply SIMPLE mode transform to pilot inputs - update_simple_mode(); - - // process pilot's roll and pitch input - roll_control = channel_roll->get_control_in(); - pitch_control = channel_pitch->get_control_in(); - - // record if pilot has overriden roll or pitch - if (roll_control != 0 || pitch_control != 0) { - ap.land_repo_active = true; - } - } - - // get pilot's desired yaw rate - target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); - } - + // set motors to full range motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); - - // process roll, pitch inputs - wp_nav.set_pilot_desired_acceleration(roll_control, pitch_control); - -#if PRECISION_LANDING == ENABLED - // run precision landing - if (!ap.land_repo_active && precland.target_acquired() && precland_last_update_ms != precland.last_update_ms()) { - Vector3f target_pos; - precland.get_target_position(target_pos); - pos_control.set_xy_target(target_pos.x, target_pos.y); - pos_control.freeze_ff_xy(); - precland_last_update_ms = precland.last_update_ms(); - } -#endif - - // run loiter controller - wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler); - - // call attitude controller - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain()); - - // pause 4 seconds before beginning land descent - float cmb_rate; - if(land_pause && millis()-land_start_time < 4000) { - cmb_rate = 0; - } else { + + // pause before beginning land descent + if(land_pause && millis()-land_start_time >= LAND_WITH_DELAY_MS) { land_pause = false; - cmb_rate = get_land_descent_speed(); } - - // record desired climb rate for logging - desired_climb_rate = cmb_rate; - - // update altitude target and call position controller - pos_control.set_alt_target_from_climb_rate(cmb_rate, G_Dt, true); - pos_control.update_z_controller(); + + land_run_horizontal_control(); + land_run_vertical_control(land_pause); } // land_nogps_run - runs the land controller @@ -215,56 +152,115 @@ void Copter::land_nogps_run() // call attitude controller attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); - // pause 4 seconds before beginning land descent - float cmb_rate; - if(land_pause && millis()-land_start_time < LAND_WITH_DELAY_MS) { - cmb_rate = 0; - } else { + // pause before beginning land descent + if(land_pause && millis()-land_start_time >= LAND_WITH_DELAY_MS) { land_pause = false; - cmb_rate = get_land_descent_speed(); + } + + land_run_vertical_control(land_pause); +} + +void Copter::land_run_vertical_control(bool pause_descent) +{ + bool navigating = pos_control.is_active_xy(); + +#if PRECISION_LANDING == ENABLED + bool doing_precision_landing = !ap.land_repo_active && precland.target_acquired() && navigating; +#else + bool doing_precision_landing = false; +#endif + + // compute desired velocity + const float precland_acceptable_error = 25.0f; + const float precland_min_descent_speed = 10.0f; + int32_t alt_above_ground; + if (rangefinder_alt_ok()) { + alt_above_ground = rangefinder_state.alt_cm_filt.get(); + } else { + if (!navigating || !current_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, alt_above_ground)) { + current_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_HOME, alt_above_ground); + } + } + + float cmb_rate = 0; + if (!pause_descent) { + cmb_rate = AC_AttitudeControl::sqrt_controller(LAND_START_ALT-alt_above_ground, g.p_alt_hold.kP(), pos_control.get_accel_z()); + cmb_rate = constrain_float(cmb_rate, pos_control.get_speed_down(), -abs(g.land_speed)); + + if (doing_precision_landing && alt_above_ground < 300.0f) { + float land_slowdown = MAX(0.0f, pos_control.get_horizontal_error()*(abs(g.land_speed)/precland_acceptable_error)); + cmb_rate = MIN(-precland_min_descent_speed, cmb_rate+land_slowdown); + } } // record desired climb rate for logging desired_climb_rate = cmb_rate; - // call position controller - pos_control.set_alt_target_from_climb_rate(cmb_rate, G_Dt, true); + // update altitude target and call position controller + pos_control.set_alt_target_from_climb_rate_ff(cmb_rate, G_Dt, true); pos_control.update_z_controller(); } -// get_land_descent_speed - high level landing logic -// returns climb rate (in cm/s) which should be passed to the position controller -// should be called at 100hz or higher -float Copter::get_land_descent_speed() +void Copter::land_run_horizontal_control() { -#if RANGEFINDER_ENABLED == ENABLED - bool rangefinder_ok = rangefinder_state.enabled && rangefinder_state.alt_healthy; -#else - bool rangefinder_ok = false; -#endif + int16_t roll_control = 0, pitch_control = 0; + float target_yaw_rate = 0; + + // relax loiter target if we might be landed + if (ap.land_complete_maybe) { + wp_nav.loiter_soften_for_landing(); + } + + // process pilot inputs + if (!failsafe.radio) { + if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){ + Log_Write_Event(DATA_LAND_CANCELLED_BY_PILOT); + // exit land if throttle is high + if (!set_mode(LOITER, MODE_REASON_THROTTLE_LAND_ESCAPE)) { + set_mode(ALT_HOLD, MODE_REASON_THROTTLE_LAND_ESCAPE); + } + } - // get position controller's target altitude above terrain - Location_Class target_loc = pos_control.get_pos_target(); - int32_t target_alt_cm; + if (g.land_repositioning) { + // apply SIMPLE mode transform to pilot inputs + update_simple_mode(); - // get altitude target above home by default - target_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_HOME, target_alt_cm); + // process pilot's roll and pitch input + roll_control = channel_roll->get_control_in(); + pitch_control = channel_pitch->get_control_in(); - // try to use terrain if enabled - if (terrain_use() && !target_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, target_alt_cm)) { - Log_Write_Error(ERROR_SUBSYSTEM_TERRAIN, ERROR_CODE_MISSING_TERRAIN_DATA); + // record if pilot has overriden roll or pitch + if (roll_control != 0 || pitch_control != 0) { + ap.land_repo_active = true; + } + } + + // get pilot's desired yaw rate + target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); } - // if we are above 10m and the rangefinder does not sense anything perform regular alt hold descent - if ((target_alt_cm >= LAND_START_ALT) && !rangefinder_ok) { - if (g.land_speed_high > 0) { - // user has asked for a different landing speed than normal descent rate - return -abs(g.land_speed_high); - } - return pos_control.get_speed_down(); - }else{ - return -abs(g.land_speed); +#if PRECISION_LANDING == ENABLED + bool doing_precision_landing = !ap.land_repo_active && precland.target_acquired(); + // run precision landing + if (doing_precision_landing && precland_last_update_ms != precland.last_update_ms()) { + Vector3f target_pos; + precland.get_target_position(target_pos); + pos_control.set_xy_target(target_pos.x, target_pos.y); + pos_control.freeze_ff_xy(); + precland_last_update_ms = precland.last_update_ms(); } +#else + bool doing_precision_landing = false; +#endif + + // process roll, pitch inputs + wp_nav.set_pilot_desired_acceleration(roll_control, pitch_control); + + // run loiter controller + wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler); + + // call attitude controller + attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain()); } // land_do_not_use_GPS - forces land-mode to not use the GPS but instead rely on pilot input for roll and pitch diff --git a/ArduCopter/control_rtl.cpp b/ArduCopter/control_rtl.cpp index 962770ad43ed5..4713f471c96f8 100644 --- a/ArduCopter/control_rtl.cpp +++ b/ArduCopter/control_rtl.cpp @@ -349,8 +349,9 @@ void Copter::rtl_land_start() // Set wp navigation target to above home wp_nav.init_loiter_target(wp_nav.get_wp_destination()); - // initialise altitude target to stopping point - pos_control.set_target_to_stopping_point_z(); + // initialise position and desired velocity + pos_control.set_alt_target(inertial_nav.get_altitude()); + pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z()); // initialise yaw set_auto_yaw_mode(AUTO_YAW_HOLD); @@ -360,8 +361,6 @@ void Copter::rtl_land_start() // called by rtl_run at 100hz or more void Copter::rtl_land_run() { - int16_t roll_control = 0, pitch_control = 0; - float target_yaw_rate = 0; // if not auto armed or landing completed or motor interlock not enabled set throttle to zero and exit immediately if (!motors.armed() || !ap.auto_armed || ap.land_complete || !motors.get_interlock()) { #if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw @@ -393,53 +392,11 @@ void Copter::rtl_land_run() return; } - // relax loiter target if we might be landed - if (ap.land_complete_maybe) { - wp_nav.loiter_soften_for_landing(); - } - - // process pilot's input - if (!failsafe.radio) { - if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){ - Log_Write_Event(DATA_LAND_CANCELLED_BY_PILOT); - // exit land if throttle is high - if (!set_mode(LOITER, MODE_REASON_THROTTLE_LAND_ESCAPE)) { - set_mode(ALT_HOLD, MODE_REASON_THROTTLE_LAND_ESCAPE); - } - } - - if (g.land_repositioning) { - // apply SIMPLE mode transform to pilot inputs - update_simple_mode(); - - // process pilot's roll and pitch input - roll_control = channel_roll->get_control_in(); - pitch_control = channel_pitch->get_control_in(); - } - - // get pilot's desired yaw rate - target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); - } - // set motors to full range motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); - // process pilot's roll and pitch input - wp_nav.set_pilot_desired_acceleration(roll_control, pitch_control); - - // run loiter controller - wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler); - - // call z-axis position controller - float cmb_rate = get_land_descent_speed(); - pos_control.set_alt_target_from_climb_rate(cmb_rate, G_Dt, true); - pos_control.update_z_controller(); - - // record desired climb rate for logging - desired_climb_rate = cmb_rate; - - // roll & pitch from waypoint controller, yaw rate from pilot - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain()); + land_run_horizontal_control(); + land_run_vertical_control(); // check if we've completed this stage of RTL rtl_state_complete = ap.land_complete; diff --git a/ArduCopter/precision_landing.cpp b/ArduCopter/precision_landing.cpp index 4b4592ad05232..769bf69bc0396 100644 --- a/ArduCopter/precision_landing.cpp +++ b/ArduCopter/precision_landing.cpp @@ -15,16 +15,15 @@ void Copter::init_precland() void Copter::update_precland() { - float final_alt = current_loc.alt; + int32_t height_above_ground_cm = current_loc.alt; - // use range finder altitude if it is valid + // use range finder altitude if it is valid, else try to get terrain alt if (rangefinder_alt_ok()) { - final_alt = rangefinder_state.alt_cm; + height_above_ground_cm = rangefinder_state.alt_cm; + } else if (terrain_use()) { + current_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, height_above_ground_cm); } - copter.precland.update(final_alt); - - // log output - Log_Write_Precland(); + copter.precland.update(height_above_ground_cm); } #endif diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index 0010d7f24722e..89abb8ee34703 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -744,6 +744,11 @@ void AC_PosControl::update_vel_controller_xyz(float ekfNavVelGainScaler) update_z_controller(); } +float AC_PosControl::get_horizontal_error() const +{ + return norm(_pos_error.x, _pos_error.y); +} + /// /// private methods /// diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index b459e836dcd2c..abbaa8988d737 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/libraries/AC_AttitudeControl/AC_PosControl.h @@ -147,6 +147,9 @@ class AC_PosControl /// get_alt_error - returns altitude error in cm float get_alt_error() const; + + // returns horizontal error in cm + float get_horizontal_error() const; /// set_target_to_stopping_point_z - sets altitude target to reasonable stopping altitude in cm above home void set_target_to_stopping_point_z(); diff --git a/libraries/AC_PrecLand/AC_PrecLand.cpp b/libraries/AC_PrecLand/AC_PrecLand.cpp index 1ba2a8ec18f91..c2cbf0f00716a 100644 --- a/libraries/AC_PrecLand/AC_PrecLand.cpp +++ b/libraries/AC_PrecLand/AC_PrecLand.cpp @@ -83,12 +83,19 @@ void AC_PrecLand::init() void AC_PrecLand::update(float alt_above_terrain_cm) { // run backend update - if (_backend != NULL) { + if (_backend != NULL && _enabled) { // read from sensor _backend->update(); - - // calculate angles to target and position estimate - calc_angles_and_pos(alt_above_terrain_cm); + + if (_backend->have_los_meas() && _backend->los_meas_time_ms() != _last_backend_los_meas_ms) { + // we have a new, unique los measurement + _last_backend_los_meas_ms = _backend->los_meas_time_ms(); + + Vector3f target_vec_unit_body; + _backend->get_los_body(target_vec_unit_body); + + calc_angles_and_pos(target_vec_unit_body, alt_above_terrain_cm); + } } } @@ -126,55 +133,23 @@ bool AC_PrecLand::get_target_velocity_relative(Vector3f& ret) // raw sensor angles stored in _angle_to_target (might be in earth frame, or maybe body frame) // earth-frame angles stored in _ef_angle_to_target // position estimate is stored in _target_pos -void AC_PrecLand::calc_angles_and_pos(float alt_above_terrain_cm) +void AC_PrecLand::calc_angles_and_pos(const Vector3f& target_vec_unit_body, float alt_above_terrain_cm) { - // exit immediately if not enabled - if (_backend == NULL) { - return; - } - - // get angles to target from backend - if (!_backend->get_angle_to_target(_angle_to_target.x, _angle_to_target.y)) { - return; - } - - _angle_to_target.x = -_angle_to_target.x; - _angle_to_target.y = -_angle_to_target.y; - - // compensate for delay - _angle_to_target.x -= _ahrs.get_gyro().x*4.0e-2f; - _angle_to_target.y -= _ahrs.get_gyro().y*4.0e-2f; - - Vector3f target_vec_ned; - - if (_angle_to_target.is_zero()) { - target_vec_ned = Vector3f(0.0f,0.0f,1.0f); - } else { - float theta = _angle_to_target.length(); - Vector3f axis = Vector3f(_angle_to_target.x, _angle_to_target.y, 0.0f)/theta; - float sin_theta = sinf(theta); - float cos_theta = cosf(theta); - - target_vec_ned.x = axis.y*sin_theta; - target_vec_ned.y = -axis.x*sin_theta; - target_vec_ned.z = cos_theta; - } - // rotate into NED frame - target_vec_ned = _ahrs.get_rotation_body_to_ned()*target_vec_ned; + Vector3f target_vec_unit_ned = _ahrs.get_rotation_body_to_ned()*target_vec_unit_body; // extract the angles to target (logging only) - _ef_angle_to_target.x = atan2f(target_vec_ned.z,target_vec_ned.x); - _ef_angle_to_target.y = atan2f(target_vec_ned.z,target_vec_ned.y); + _angle_to_target.x = atan2f(-target_vec_unit_body.y, target_vec_unit_body.z); + _angle_to_target.y = atan2f( target_vec_unit_body.x, target_vec_unit_body.z); + _ef_angle_to_target.x = atan2f(-target_vec_unit_ned.y, target_vec_unit_ned.z); + _ef_angle_to_target.y = atan2f( target_vec_unit_ned.x, target_vec_unit_ned.z); - // ensure that the angle to target is no more than 70 degrees - if (target_vec_ned.z > 0.26f) { + if (target_vec_unit_ned.z > 0.0f) { // get current altitude (constrained to be positive) float alt = MAX(alt_above_terrain_cm, 0.0f); - float dist = alt/target_vec_ned.z; - // - _target_pos_rel.x = target_vec_ned.x*dist; - _target_pos_rel.y = target_vec_ned.y*dist; + float dist = alt/target_vec_unit_ned.z; + _target_pos_rel.x = target_vec_unit_ned.x*dist; + _target_pos_rel.y = target_vec_unit_ned.y*dist; _target_pos_rel.z = alt; // not used _target_pos = _inav.get_position()+_target_pos_rel; diff --git a/libraries/AC_PrecLand/AC_PrecLand.h b/libraries/AC_PrecLand/AC_PrecLand.h index 12ab889167f50..5be6d4390c5d6 100644 --- a/libraries/AC_PrecLand/AC_PrecLand.h +++ b/libraries/AC_PrecLand/AC_PrecLand.h @@ -83,7 +83,7 @@ class AC_PrecLand // angles stored in _angle_to_target // earth-frame angles stored in _ef_angle_to_target // position estimate is stored in _target_pos - void calc_angles_and_pos(float alt_above_terrain_cm); + void calc_angles_and_pos(const Vector3f& target_vec_unit_body, float alt_above_terrain_cm); // returns enabled parameter as an behaviour enum PrecLandBehaviour get_behaviour() const { return (enum PrecLandBehaviour)(_enabled.get()); } @@ -97,6 +97,7 @@ class AC_PrecLand AP_Int8 _type; // precision landing controller type uint32_t _last_update_ms; // epoch time in millisecond when update is called + uint32_t _last_backend_los_meas_ms; // output from sensor (stored for logging) Vector2f _angle_to_target; // last raw sensor angle to target diff --git a/libraries/AC_PrecLand/AC_PrecLand_Backend.h b/libraries/AC_PrecLand/AC_PrecLand_Backend.h index d8b7aeefd1836..6cdc422417820 100644 --- a/libraries/AC_PrecLand/AC_PrecLand_Backend.h +++ b/libraries/AC_PrecLand/AC_PrecLand_Backend.h @@ -10,7 +10,6 @@ class AC_PrecLand_Backend { public: - // Constructor AC_PrecLand_Backend(const AC_PrecLand& frontend, AC_PrecLand::precland_state& state) : _frontend(frontend), @@ -19,26 +18,26 @@ class AC_PrecLand_Backend // destructor virtual ~AC_PrecLand_Backend() {} - // init - perform any required initialisation of backend controller + // perform any required initialisation of backend virtual void init() = 0; - // update - give chance to driver to get updates from sensor - // returns true if new data available - virtual bool update() = 0; - // what frame of reference is our sensor reporting in? - virtual MAV_FRAME get_frame_of_reference() = 0; - - // get_angle_to_target - returns angles (in radians) to target - // returns true if angles are available, false if not (i.e. no target) - // x_angle_rad : roll direction, positive = target is to right (looking down) - // y_angle_rad : pitch direction, postiive = target is forward (looking down) - virtual bool get_angle_to_target(float &x_angle_rad, float &y_angle_rad) = 0; - - // handle_msg - parses a mavlink message from the companion computer + // retrieve updates from sensor + virtual void update() = 0; + + // provides a unit vector towards the target in body frame + // returns same as have_los_meas() + virtual bool get_los_body(Vector3f& dir_body) = 0; + + // returns system time in milliseconds of last los measurement + virtual uint32_t los_meas_time_ms() = 0; + + // return true if there is a valid los measurement available + virtual bool have_los_meas() = 0; + + // parses a mavlink message from the companion computer virtual void handle_msg(mavlink_message_t* msg) = 0; protected: - const AC_PrecLand& _frontend; // reference to precision landing front end AC_PrecLand::precland_state &_state; // reference to this instances state }; diff --git a/libraries/AC_PrecLand/AC_PrecLand_Companion.cpp b/libraries/AC_PrecLand/AC_PrecLand_Companion.cpp index 1498dd7fd6c42..61f3accec442c 100644 --- a/libraries/AC_PrecLand/AC_PrecLand_Companion.cpp +++ b/libraries/AC_PrecLand/AC_PrecLand_Companion.cpp @@ -7,52 +7,47 @@ extern const AP_HAL::HAL& hal; // Constructor AC_PrecLand_Companion::AC_PrecLand_Companion(const AC_PrecLand& frontend, AC_PrecLand::precland_state& state) : AC_PrecLand_Backend(frontend, state), - _frame(MAV_FRAME_BODY_NED), _distance_to_target(0.0f), _timestamp_us(0), - _new_estimate(false) + _have_los_meas(false) { } -// init - perform initialisation of this backend +// perform any required initialisation of backend void AC_PrecLand_Companion::init() { // set healthy _state.healthy = true; - _new_estimate = false; + _have_los_meas = false; } -// update - give chance to driver to get updates from sensor -// returns true if new data available -bool AC_PrecLand_Companion::update() +// retrieve updates from sensor +void AC_PrecLand_Companion::update() { - // Mavlink commands are received asynchronous so all new data is processed by handle_msg() - return _new_estimate; + _have_los_meas = _have_los_meas && AP_HAL::millis()-_los_meas_time_ms <= 1000; } -MAV_FRAME AC_PrecLand_Companion::get_frame_of_reference() -{ - return _frame; -} - -// get_angle_to_target - returns angles (in radians) to target -// returns true if angles are available, false if not (i.e. no target) -// x_angle_rad : roll direction, positive = target is to right (looking down) -// y_angle_rad : pitch direction, postiive = target is forward (looking down) -bool AC_PrecLand_Companion::get_angle_to_target(float &x_angle_rad, float &y_angle_rad) -{ - if (_new_estimate){ - x_angle_rad = _angle_to_target.x; - y_angle_rad = _angle_to_target.y; - - // reset and wait for new data - _new_estimate = false; +// provides a unit vector towards the target in body frame +// returns same as have_los_meas() +bool AC_PrecLand_Companion::get_los_body(Vector3f& ret) { + if (have_los_meas()) { + ret = _los_meas_body; return true; } - return false; } +// returns system time in milliseconds of last los measurement +uint32_t AC_PrecLand_Companion::los_meas_time_ms() { + return _los_meas_time_ms; +} + +// return true if there is a valid los measurement available +bool AC_PrecLand_Companion::have_los_meas() +{ + return _have_los_meas; +} + void AC_PrecLand_Companion::handle_msg(mavlink_message_t* msg) { // parse mavlink message @@ -60,10 +55,12 @@ void AC_PrecLand_Companion::handle_msg(mavlink_message_t* msg) mavlink_msg_landing_target_decode(msg, &packet); _timestamp_us = packet.time_usec; - _frame = (MAV_FRAME) packet.frame; - _angle_to_target.x = packet.angle_x; - _angle_to_target.y = packet.angle_y; _distance_to_target = packet.distance; - _state.healthy = true; - _new_estimate = true; + + // compute unit vector towards target + _los_meas_body = Vector3f(-tanf(packet.angle_y), tanf(packet.angle_x), 1.0f); + _los_meas_body /= _los_meas_body.length(); + + _los_meas_time_ms = AP_HAL::millis(); + _have_los_meas = true; } diff --git a/libraries/AC_PrecLand/AC_PrecLand_Companion.h b/libraries/AC_PrecLand/AC_PrecLand_Companion.h index 7048d5605b7a9..9c108f90e19eb 100644 --- a/libraries/AC_PrecLand/AC_PrecLand_Companion.h +++ b/libraries/AC_PrecLand/AC_PrecLand_Companion.h @@ -13,34 +13,33 @@ class AC_PrecLand_Companion : public AC_PrecLand_Backend { public: - // Constructor AC_PrecLand_Companion(const AC_PrecLand& frontend, AC_PrecLand::precland_state& state); - - // init - perform any required initialisation of backend controller + + // perform any required initialisation of backend void init(); - // update - give chance to driver to get updates from sensor - // returns true if new data available - bool update(); - // what frame of reference is our sensor reporting in? - MAV_FRAME get_frame_of_reference(); - - // get_angle_to_target - returns angles (in radians) to target - // returns true if angles are available, false if not (i.e. no target) - // x_angle_rad : roll direction, positive = target is to right (looking down) - // y_angle_rad : pitch direction, postiive = target is forward (looking down) - bool get_angle_to_target(float &x_angle_rad, float &y_angle_rad); - - // handle_msg - parses a mavlink message from the companion computer + // retrieve updates from sensor + void update(); + + // provides a unit vector towards the target in body frame + // returns same as have_los_meas() + bool get_los_body(Vector3f& ret); + + // returns system time in milliseconds of last los measurement + uint32_t los_meas_time_ms(); + + // return true if there is a valid los measurement available + bool have_los_meas(); + + // parses a mavlink message from the companion computer void handle_msg(mavlink_message_t* msg); private: - - // output from camera - MAV_FRAME _frame; // what frame of reference is our sensor reporting in? - Vector2f _angle_to_target; // last angle to target + uint64_t _timestamp_us; // timestamp from message float _distance_to_target; // distance from the camera to target in meters - uint64_t _timestamp_us; // timestamp when the image was captured(synced via UAVCAN) - bool _new_estimate; // true if new data from the camera has been received + + Vector3f _los_meas_body; // unit vector in body frame pointing towards target + bool _have_los_meas; // true if there is a valid measurement from the camera + uint32_t _los_meas_time_ms; // system time in milliseconds when los was measured }; diff --git a/libraries/AC_PrecLand/AC_PrecLand_IRLock.cpp b/libraries/AC_PrecLand/AC_PrecLand_IRLock.cpp index 9666aa44cfc57..47124e10be89c 100644 --- a/libraries/AC_PrecLand/AC_PrecLand_IRLock.cpp +++ b/libraries/AC_PrecLand/AC_PrecLand_IRLock.cpp @@ -10,7 +10,9 @@ extern const AP_HAL::HAL& hal; // Constructor AC_PrecLand_IRLock::AC_PrecLand_IRLock(const AC_PrecLand& frontend, AC_PrecLand::precland_state& state) : AC_PrecLand_Backend(frontend, state), - irlock() + irlock(), + _have_los_meas(false), + _los_meas_time_ms(0) { } @@ -18,25 +20,43 @@ AC_PrecLand_IRLock::AC_PrecLand_IRLock(const AC_PrecLand& frontend, AC_PrecLand: void AC_PrecLand_IRLock::init() { irlock.init(); - - // set healthy - _state.healthy = irlock.healthy(); } // update - give chance to driver to get updates from sensor -bool AC_PrecLand_IRLock::update() +void AC_PrecLand_IRLock::update() { + // update health + _state.healthy = irlock.healthy(); + // get new sensor data - return (irlock.update()); + irlock.update(); + + if (irlock.num_targets() > 0 && irlock.last_update_ms() != _los_meas_time_ms) { + irlock.get_unit_vector_body(_los_meas_body); + _have_los_meas = true; + _los_meas_time_ms = irlock.last_update_ms(); + } + _have_los_meas = _have_los_meas && AP_HAL::millis()-_los_meas_time_ms <= 1000; } -// get_angle_to_target - returns body frame angles (in radians) to target -// returns true if angles are available, false if not (i.e. no target) -// x_angle_rad : body-frame roll direction, positive = target is to right (looking down) -// y_angle_rad : body-frame pitch direction, postiive = target is forward (looking down) -bool AC_PrecLand_IRLock::get_angle_to_target(float &x_angle_rad, float &y_angle_rad) -{ - return irlock.get_angle_to_target(x_angle_rad, y_angle_rad); +// provides a unit vector towards the target in body frame +// returns same as have_los_meas() +bool AC_PrecLand_IRLock::get_los_body(Vector3f& ret) { + if (have_los_meas()) { + ret = _los_meas_body; + return true; + } + return false; +} + +// returns system time in milliseconds of last los measurement +uint32_t AC_PrecLand_IRLock::los_meas_time_ms() { + return _los_meas_time_ms; +} + +// return true if there is a valid los measurement available +bool AC_PrecLand_IRLock::have_los_meas() { + return _have_los_meas; } #endif // PX4 diff --git a/libraries/AC_PrecLand/AC_PrecLand_IRLock.h b/libraries/AC_PrecLand/AC_PrecLand_IRLock.h index 62e1fbddc1ecd..2d792bf86a0eb 100644 --- a/libraries/AC_PrecLand/AC_PrecLand_IRLock.h +++ b/libraries/AC_PrecLand/AC_PrecLand_IRLock.h @@ -20,27 +20,31 @@ class AC_PrecLand_IRLock : public AC_PrecLand_Backend // Constructor AC_PrecLand_IRLock(const AC_PrecLand& frontend, AC_PrecLand::precland_state& state); - - // init - perform any required initialisation of backend controller + + // perform any required initialisation of backend void init(); - // update - give chance to driver to get updates from sensor - // returns true if new data available - bool update(); - // IRLock is hard-mounted to the frame of the vehicle, so it will always be in body-frame - MAV_FRAME get_frame_of_reference() { return MAV_FRAME_BODY_NED; } - - // get_angle_to_target - returns body frame angles (in radians) to target - // returns true if angles are available, false if not (i.e. no target) - // x_angle_rad : body-frame roll direction, positive = target is to right (looking down) - // y_angle_rad : body-frame pitch direction, postiive = target is forward (looking down) - bool get_angle_to_target(float &x_angle_rad, float &y_angle_rad); - - // handle_msg - parses a mavlink message from the companion computer - void handle_msg(mavlink_message_t* msg) { /* do nothing */ } + // retrieve updates from sensor + void update(); + + // provides a unit vector towards the target in body frame + // returns same as have_los_meas() + bool get_los_body(Vector3f& ret); + + // returns system time in milliseconds of last los measurement + uint32_t los_meas_time_ms(); + + // return true if there is a valid los measurement available + bool have_los_meas(); + + // parses a mavlink message from the companion computer + void handle_msg(mavlink_message_t* msg) {}; private: AP_IRLock_PX4 irlock; - + + Vector3f _los_meas_body; // unit vector in body frame pointing towards target + bool _have_los_meas; // true if there is a valid measurement from the camera + uint32_t _los_meas_time_ms; // system time in milliseconds when los was measured }; #endif diff --git a/libraries/AP_IRLock/AP_IRLock_PX4.cpp b/libraries/AP_IRLock/AP_IRLock_PX4.cpp index 24bc95d2b995b..63454ef56bd66 100644 --- a/libraries/AP_IRLock/AP_IRLock_PX4.cpp +++ b/libraries/AP_IRLock/AP_IRLock_PX4.cpp @@ -31,8 +31,7 @@ extern const AP_HAL::HAL& hal; AP_IRLock_PX4::AP_IRLock_PX4() : - _fd(0), - _last_timestamp(0) + _fd(0) {} void AP_IRLock_PX4::init() @@ -55,29 +54,23 @@ bool AP_IRLock_PX4::update() } // read position of all objects + bool new_data = false; struct irlock_s report; - uint16_t count = 0; - while(::read(_fd, &report, sizeof(struct irlock_s)) == sizeof(struct irlock_s) && report.timestamp >_last_timestamp) { - _target_info[count].timestamp = report.timestamp; - _target_info[count].target_num = report.target_num; - _target_info[count].angle_x = report.angle_x; - _target_info[count].angle_y = report.angle_y; - _target_info[count].size_x = report.size_x; - _target_info[count].size_y = report.size_y; - count++; - _last_timestamp = report.timestamp; - _last_update = AP_HAL::millis(); - } - - // update num_blocks and implement timeout - if (count > 0) { - _num_targets = count; - } else if ((AP_HAL::millis() - _last_update) > IRLOCK_TIMEOUT_MS) { - _num_targets = 0; + while(::read(_fd, &report, sizeof(struct irlock_s)) == sizeof(struct irlock_s)) { + new_data = true; + _num_targets = report.num_targets; + for (uint8_t i=0; i 0); + return new_data; } #endif // CONFIG_HAL_BOARD == HAL_BOARD_PX4 diff --git a/libraries/AP_IRLock/AP_IRLock_PX4.h b/libraries/AP_IRLock/AP_IRLock_PX4.h index f9168dd53e598..e78c1dac4b64b 100644 --- a/libraries/AP_IRLock/AP_IRLock_PX4.h +++ b/libraries/AP_IRLock/AP_IRLock_PX4.h @@ -21,5 +21,4 @@ class AP_IRLock_PX4 : public IRLock private: int _fd; - uint64_t _last_timestamp; }; diff --git a/libraries/AP_IRLock/IRLock.cpp b/libraries/AP_IRLock/IRLock.cpp index be90300479bea..99b2d9c144276 100644 --- a/libraries/AP_IRLock/IRLock.cpp +++ b/libraries/AP_IRLock/IRLock.cpp @@ -9,7 +9,7 @@ // default constructor IRLock::IRLock() : - _last_update(0), + _last_update_ms(0), _num_targets(0) { // clear target info @@ -21,17 +21,34 @@ IRLock::IRLock() : IRLock::~IRLock() {} -// get_angle_to_target - retrieve body frame x and y angles (in radians) to target -// returns true if angles are available, false if not (i.e. no target) -bool IRLock::get_angle_to_target(float &x_angle_rad, float &y_angle_rad) const +// retrieve body frame x and y angles (in radians) to target +// returns true if data is available +bool IRLock::get_angle_to_target_rad(float &x_angle_rad, float &y_angle_rad) const { // return false if we have no target if (_num_targets == 0) { return false; } - // use data from first object - x_angle_rad = _target_info[0].angle_x; - y_angle_rad = _target_info[0].angle_y; + // use data from first (largest) object + x_angle_rad = atanf(_target_info[0].pos_x); + y_angle_rad = atanf(_target_info[0].pos_y); return true; } + +// retrieve body frame unit vector in direction of target +// returns true if data is available +bool IRLock::get_unit_vector_body(Vector3f& ret) const +{ + // return false if we have no target + if (_num_targets == 0) { + return false; + } + + // use data from first (largest) object + ret.x = -_target_info[0].pos_y; + ret.y = _target_info[0].pos_x; + ret.z = 1.0f; + ret /= ret.length(); + return true; +} \ No newline at end of file diff --git a/libraries/AP_IRLock/IRLock.h b/libraries/AP_IRLock/IRLock.h index d134ebf1842ec..4608cc0921db2 100644 --- a/libraries/AP_IRLock/IRLock.h +++ b/libraries/AP_IRLock/IRLock.h @@ -24,7 +24,6 @@ #include #define IRLOCK_MAX_TARGETS 5 // max number of targets that can be detected by IR-LOCK sensor (should match PX4Firmware's irlock driver's IRLOCK_OBJECTS_MAX) -#define IRLOCK_TIMEOUT_MS 100 // target info times out after 0.1 seconds class IRLock { @@ -40,7 +39,7 @@ class IRLock bool healthy() const { return _flags.healthy; } // timestamp of most recent data read from the sensor - uint32_t last_update() const { return _last_update; } + uint32_t last_update_ms() const { return _last_update_ms; } // returns the number of blocks in the current frame size_t num_targets() const { return _num_targets; } @@ -48,9 +47,14 @@ class IRLock // retrieve latest sensor data - returns true if new data is available virtual bool update() = 0; - // get_angle_to_target - retrieve body frame x and y angles (in radians) to target - // returns true if angles are available, false if not (i.e. no target) - bool get_angle_to_target(float &x_angle_rad, float &y_angle_rad) const; + // retrieve body frame x and y angles (in radians) to target + // returns true if data is available + bool get_angle_to_target_rad(float &x_angle_rad, float &y_angle_rad) const; + + // retrieve body frame unit vector in direction of target + // returns true if data is available + bool get_unit_vector_body(Vector3f& ret) const; + protected: struct AP_IRLock_Flags { @@ -58,17 +62,16 @@ class IRLock } _flags; // internals - uint32_t _last_update; + uint32_t _last_update_ms; uint16_t _num_targets; // irlock_target_info is a duplicate of the PX4Firmware irlock_s structure typedef struct { - uint64_t timestamp; // time target was seen in microseconds since system start - uint16_t target_num; // target number prioritised by size (largest is 0) - float angle_x; // x-axis angle in radians from center of image to center of target - float angle_y; // y-axis angle in radians from center of image to center of target - float size_x; // size in radians of target along x-axis - float size_y; // size in radians of target along y-axis + uint64_t timestamp; // microseconds since system start + float pos_x; // x-axis distance from center of image to center of target in units of tan(theta) + float pos_y; // y-axis distance from center of image to center of target in units of tan(theta) + float size_x; // size of target along x-axis in units of tan(theta) + float size_y; // size of target along y-axis in units of tan(theta) } irlock_target_info; irlock_target_info _target_info[IRLOCK_MAX_TARGETS]; diff --git a/modules/PX4Firmware b/modules/PX4Firmware index 4a1d6880549f9..c2d66c8a1cd27 160000 --- a/modules/PX4Firmware +++ b/modules/PX4Firmware @@ -1 +1 @@ -Subproject commit 4a1d6880549f95858686c0faa770bd1c644779dd +Subproject commit c2d66c8a1cd27fb91ee263fe59c764c635531ae7