From a3e3fd27d62890b51bf2cbebb333dc5ab3e15826 Mon Sep 17 00:00:00 2001 From: Max Shvetsov Date: Sun, 16 Nov 2014 15:42:54 +0200 Subject: Sonar and range finder disarming properly, possibly removed working engines in HIL bug --- src/modules/commander/commander.cpp | 20 ++++-- src/modules/commander/state_machine_helper.cpp | 1 - .../ekf_att_pos_estimator_main.cpp | 17 +++-- src/modules/mc_pos_control/mc_pos_control_main.cpp | 6 +- .../position_estimator_inav_main.c | 83 +++++++++++++--------- 5 files changed, 76 insertions(+), 51 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 350062bca..4c5b09152 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1734,20 +1734,28 @@ int commander_thread_main(int argc, char *argv[]) } } - /* Handle commander requests here */ - /* This request is sent from inav while landing by sonar - * Author: Max Shvetsov + /* Handle commander requests here + * + * Description: This request is sent from inav while landing by sonar + * Author: Max Shvetsov */ orb_check(commander_request_inav_sub, &updated); if (updated) { orb_copy(ORB_ID(commander_request_inav), commander_request_inav_sub, &commander_request_inav); - switch(commander_request.request_type) { + switch(commander_request_inav.request_type) { case V_DISARM_INAV: { - arm_disarm(false, mavlink_fd, "Commander request."); + transition_result_t result; + result = arm_disarm(false, mavlink_fd, "sonar request."); + if (result == TRANSITION_DENIED ) { + mavlink_log_info(mavlink_fd, "[commander], disarm by Range finder DECLINED"); + } + else if (result == TRANSITION_CHANGED) { + mavlink_log_info(mavlink_fd, "[commander], DISARMED by Range finder"); + } break; } case AIRD_STATE_CHANGE_INAV: @@ -1758,8 +1766,6 @@ int commander_thread_main(int argc, char *argv[]) default: break; } - - mavlink_log_info(mavlink_fd, "Disarm request by sonar processed\n"); } /* Handle commander requests here */ diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 66085aeaf..cfa1ee39f 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -139,7 +139,6 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s /* enforce lockdown in HIL */ if (status->hil_state == HIL_STATE_ON) { armed->lockdown = true; - armed->lockdown = false; // TODO remove line } else { armed->lockdown = false; diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index a61346c2e..8498ee421 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -1439,13 +1439,16 @@ FixedwingEstimator::task_main() /* crude land detector for fixedwing only, * TODO: adapt so that it works for both, maybe move to another location */ - if (_velocity_xy_filtered < 5 - && _velocity_z_filtered < 10 - && _airspeed_filtered < 10) { - _local_pos.landed = true; - } else { - _local_pos.landed = false; - } + /* commented out by Max Shvetsov + * airdog is not a fixedwing, no need in this thing + */ + //if (_velocity_xy_filtered < 5 + // && _velocity_z_filtered < 10 + // && _airspeed_filtered < 10) { + // _local_pos.landed = true; + //} else { + // _local_pos.landed = false; + //} _local_pos.z_global = false; _local_pos.yaw = _att.yaw; diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index f78bb0c31..d369d193a 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1636,8 +1636,10 @@ MulticopterPositionControl::task_main() if(_params.sonar_correction_on && _local_pos.dist_bottom_valid) { float coeff = _local_pos.dist_bottom/(MAXIMAL_DISTANCE); - _landing_coef = (coeff * _params.land_speed) > (_params.land_speed * 0.5f) ? coeff : 0.5f; - fprintf(stderr, "Landing, _landing_coef: %.3f\n", (double)_landing_coef); + _landing_coef = (coeff * _params.land_speed) > (_params.land_speed * 0.3f) ? coeff : 0.3f; + if (_landing_coef > 1.0f) + _landing_coef = 1.0f; + //fprintf(stderr, "Landing, _landing_coef: %.3f\n", (double)_landing_coef); } _vel_sp(2) = _params.land_speed * _landing_coef; //fprintf(stderr, "Landing, sonar invalid, _vel_sp: %.3f\n", (double)_vel_sp(2)); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index c5787978f..3935a008d 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -1134,40 +1134,55 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } if (vehicle_status.airdog_state == AIRD_STATE_LANDING && params.sonar_on) { // If we are in the landing state and we are using sonar - rely on the sonar - if (range_finder.valid) { - // If sonar is currently working define weather we are descending or ascending - if (dist_bottom < land_sonar_last_val) - land_by_sonar ++; - else - land_by_sonar --; - land_sonar_last_val = dist_bottom; - //fprintf(stderr, "We are landing by sonar, dist_bottom: %.3f sonar_prev: %.3f land_by_sonar: %d\n", - // - // (double)dist_bottom, - // (double)sonar_prev, - // land_by_sonar); - //fprintf(stderr, "We are landing by sonar, land_sonar_last_val: %.3f > dist_bottom: %.3f\n", (double)land_sonar_last_val, (double)dist_bottom); - } - else { - // If sonar is invalid - check if we WERE descending and last sonar value is low - //fprintf(stderr, "Sonar not valid, dist_bottom: %.3f sonar_prev: %.3f land_by_sonar: %d\n", - // (double)dist_bottom, - // (double)sonar_prev, - // land_by_sonar); - - if (land_by_sonar > 0 && dist_bottom < 2.5f) { - if (landed_time == 0.0f) { - landed_time = t; - } - else if (t - landed_time > 1000000.0f) { - // We are alliwing 1 more second to accend - landed = true; - land_by_sonar = 0; - landed_time = 0.0f; - commander_request.request_type = V_DISARM_INAV; - orb_publish(ORB_ID(commander_request_inav), commander_request_inav_pub, &commander_request); - } - } + switch(range_finder.type) { + case RANGE_FINDER_TYPE_ULTRASONIC: { + + if (range_finder.valid) { + // If sonar is currently working define weather we are descending or ascending + if (dist_bottom < land_sonar_last_val) + land_by_sonar ++; + else + land_by_sonar --; + land_sonar_last_val = dist_bottom; + } + else { + if (land_by_sonar > 0 && dist_bottom < 1.5f) { + if (landed_time == 0.0f) { + landed_time = t; + } + else if (t - landed_time > 1000000.0f) { + // We are allowing 1 more second to accend + landed = true; + land_by_sonar = 0; + landed_time = 0.0f; + commander_request.request_type = V_DISARM_INAV; + orb_publish(ORB_ID(commander_request_inav), commander_request_inav_pub, &commander_request); + fprintf(stderr,"[inav] Sending DISARM request from Sonar\n"); + } + } + } + break; + } + case RANGE_FINDER_TYPE_LASER: { + if (dist_bottom > 0.5f /*TODO: <-- to param */&& sonar_valid) { + // If we are too high to land and disarm and range finder is valid + // range finder timeout is already taken into account here and we can be sure + // range finder IS working (not was working) + if (dist_bottom < land_sonar_last_val) + land_by_sonar ++; + else + land_by_sonar --; + land_sonar_last_val = dist_bottom; + } + else if (dist_bottom < 0.2f /*TODO: <-- to param */&& sonar_valid && land_by_sonar > 0) { + landed = true; + land_by_sonar = 0; + commander_request.request_type = V_DISARM_INAV; + orb_publish(ORB_ID(commander_request_inav), commander_request_inav_pub, &commander_request); + fprintf(stderr,"[inav] Sending DISARM request from Laser\n"); + } + break; + } } } } -- cgit v1.2.3