Sonar and range finder disarming properly, possibly removed working engines in HIL bug

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 <maxim.shvetsov@airdog.com>
+        /* Handle commander requests here
+         *
+         * Description: This request is sent from inav while landing by sonar
+         * Author:      Max Shvetsov <maxim.shvetsov@airdog.com>
          */
 		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 <maxim.shvetsov@airdog.com> 
+                                 * 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;
+                                                          }
                         }
                     }
                 }