diff --git a/ArducopterNG/Arducopter.h b/ArducopterNG/Arducopter.h index 737aba3e96..afc7b814d0 100644 --- a/ArducopterNG/Arducopter.h +++ b/ArducopterNG/Arducopter.h @@ -325,6 +325,7 @@ float baro_altitude_D; int target_sonar_altitude; // target altitude in cm long press_sonar_altitude = 0; int sonar_status = SONAR_STATUS_BAD; // indicates if sonar values can be trusted +int sonar_valid_count = 0; // from -5 ~ 5 -ve = number of invalid readings, +ve = number of valid readings (in a row) long sonar_threshold; // threshold at which we should transfer control to barometer (normally 80% of sonar's max distance) byte sonar_new_data = 0; float sonar_altitude_I; diff --git a/ArducopterNG/ArducopterNG.pde b/ArducopterNG/ArducopterNG.pde index 5c104f88c1..a5c50bf888 100644 --- a/ArducopterNG/ArducopterNG.pde +++ b/ArducopterNG/ArducopterNG.pde @@ -410,29 +410,32 @@ void loop() } // Switch on altitude control if we have a barometer or Sonar - #if defined(UseBMP) || defined(IsSONAR) + #if (defined(UseBMP) || defined(IsSONAR)) if( altitude_control_method == ALTITUDE_CONTROL_NONE ) { // by default turn on altitude hold using barometer #ifdef UseBMP - altitude_control_method = ALTITUDE_CONTROL_BARO; - target_baro_altitude = press_baro_altitude; - baro_altitude_I = 0; // don't carry over any I values from previous times user may have switched on altitude control + if( press_baro_altitude != 0 ) + { + altitude_control_method = ALTITUDE_CONTROL_BARO; + target_baro_altitude = press_baro_altitude; + baro_altitude_I = 0; // don't carry over any I values from previous times user may have switched on altitude control + } #endif // use sonar if it's available #ifdef IsSONAR - if( sonar_status == SONAR_STATUS_OK ) + if( sonar_status == SONAR_STATUS_OK && press_sonar_altitude != 0 ) { altitude_control_method = ALTITUDE_CONTROL_SONAR; - target_sonar_altitude = press_sonar_altitude; + target_sonar_altitude = constrain(press_sonar_altitude,AP_RangeFinder_down.min_distance*3,sonar_threshold); } sonar_altitude_I = 0; // don't carry over any I values from previous times user may have switched on altitude control #endif // capture current throttle to use as base for altitude control initial_throttle = ch_throttle; - ch_throttle_altitude_hold = ch_throttle; + ch_throttle_altitude_hold = ch_throttle; } // Sonar Altitude Control @@ -440,22 +443,22 @@ void loop() if( sonar_new_data ) // if new sonar data has arrived { // Allow switching between sonar and barometer - #if defined(UseBMP) + #ifdef UseBMP // if SONAR become invalid switch to barometer - if( altitude_control_method == ALTITUDE_CONTROL_SONAR && sonar_status == SONAR_STATUS_BAD ) + if( altitude_control_method == ALTITUDE_CONTROL_SONAR && sonar_valid_count <= -3 ) { // next target barometer altitude to current barometer altitude + user's desired change over last sonar altitude (i.e. keeps up the momentum) altitude_control_method = ALTITUDE_CONTROL_BARO; - target_baro_altitude = press_baro_altitude + constrain((target_sonar_altitude - press_sonar_altitude),-50,50); + target_baro_altitude = press_baro_altitude;// + constrain((target_sonar_altitude - press_sonar_altitude),-50,50); } // if SONAR becomes valid switch to sonar control - if( altitude_control_method == ALTITUDE_CONTROL_BARO && sonar_status == SONAR_STATUS_OK ) + if( altitude_control_method == ALTITUDE_CONTROL_BARO && sonar_valid_count >= 3 ) { altitude_control_method = ALTITUDE_CONTROL_SONAR; if( target_sonar_altitude == 0 ) { // if target sonar altitude hasn't been intialised before.. - target_sonar_altitude = press_sonar_altitude + constrain((target_baro_altitude - press_baro_altitude),-50,50); // maybe this should just use the user's last valid target sonar altitude + target_sonar_altitude = press_sonar_altitude;// + constrain((target_baro_altitude - press_baro_altitude),-50,50); // maybe this should just use the user's last valid target sonar altitude } // ensure target altitude is reasonable target_sonar_altitude = constrain(target_sonar_altitude,AP_RangeFinder_down.min_distance*3,sonar_threshold); @@ -481,7 +484,7 @@ void loop() #if !defined(UseBMP) // limit the upper altitude if no barometer used if( target_sonar_altitude > sonar_threshold) target_sonar_altitude = sonar_threshold; - #endif + #endif } } sonar_new_data = 0; // record that we've consumed the sonar data @@ -517,7 +520,7 @@ void loop() target_position=0; if( altitude_control_method != ALTITUDE_CONTROL_NONE ) { - altitude_control_method = ALTITUDE_CONTROL_NONE; // turn off altitude control + altitude_control_method = ALTITUDE_CONTROL_NONE; // turn off altitude control } } // if (AP_mode == AP_AUTOMATIC_MODE) } @@ -534,11 +537,6 @@ void loop() heli_moveSwashPlate(); #endif -#ifdef IsSONAR - // read altitude from Sonar at 60Hz but only process every 3rd value (=20hz) - read_Sonar(); -#endif - // Each of the six cases executes at 10Hz switch (medium_loopCounter){ case 0: // Magnetometer reading (10Hz) @@ -560,6 +558,7 @@ void loop() } #endif #ifdef IsSONAR + read_Sonar(); sonar_new_data = 1; // process sonar values at 20Hz #endif #ifdef IsRANGEFINDER @@ -588,6 +587,7 @@ void loop() } #endif #ifdef IsSONAR + read_Sonar(); sonar_new_data = 1; // process sonar values at 20Hz #endif #ifdef IsRANGEFINDER diff --git a/ArducopterNG/Motors.pde b/ArducopterNG/Motors.pde index b477658f25..f1b5a52b37 100644 --- a/ArducopterNG/Motors.pde +++ b/ArducopterNG/Motors.pde @@ -42,7 +42,7 @@ void motor_output() byte throttle_mode=0; throttle = ch_throttle; - #if defined(UseBMP) || defined(IsRANGEFINDER) + #if (defined(UseBMP) || defined(IsSONAR)) if (AP_mode == AP_AUTOMATIC_MODE) { throttle = ch_throttle_altitude_hold; diff --git a/ArducopterNG/Navigation.pde b/ArducopterNG/Navigation.pde index 308c65dde2..b9f11fd7ae 100644 --- a/ArducopterNG/Navigation.pde +++ b/ArducopterNG/Navigation.pde @@ -170,11 +170,12 @@ int Altitude_control_baro(int altitude, int target_altitude) err_altitude_old = err_altitude; err_altitude = target_altitude - altitude; - baro_altitude_D = (float)(err_altitude-err_altitude_old)/0.05; // 20Hz baro_altitude_I += (float)err_altitude*0.05; baro_altitude_I = constrain(baro_altitude_I,-140,140); + baro_altitude_D = (float)(err_altitude-err_altitude_old)/0.05; // 20Hz command_altitude = KP_ALTITUDE*err_altitude + KD_ALTITUDE*baro_altitude_D + KI_ALTITUDE*baro_altitude_I; command_altitude = initial_throttle + constrain(command_altitude,-ALTITUDE_CONTROL_BARO_OUTPUT_MIN,ALTITUDE_CONTROL_BARO_OUTPUT_MAX); + Log_Write_PID(5,KP_ALTITUDE*err_altitude,KI_ALTITUDE*baro_altitude_I,KD_ALTITUDE*baro_altitude_D,command_altitude); return command_altitude; } @@ -183,18 +184,21 @@ int Altitude_control_baro(int altitude, int target_altitude) #define GdT_SONAR_ALTITUDE 0.05 #define ALTITUDE_CONTROL_SONAR_OUTPUT_MIN 60 #define ALTITUDE_CONTROL_SONAR_OUTPUT_MAX 80 -int Altitude_control_Sonar(int Sonar_altitude, int target_sonar_altitude) +int Altitude_control_Sonar(int altitude, int target_altitude) { + static int err_altitude = 0; int command_altitude; + int err_altitude_old; err_altitude_old = err_altitude; - err_altitude = target_sonar_altitude - Sonar_altitude; - sonar_altitude_D = (float)(err_altitude-err_altitude_old)/GdT_SONAR_ALTITUDE; + err_altitude = target_altitude - altitude; sonar_altitude_I += (float)err_altitude*GdT_SONAR_ALTITUDE; sonar_altitude_I = constrain(sonar_altitude_I,-1000,1000); - command_altitude = KP_SONAR_ALTITUDE*err_altitude + KD_SONAR_ALTITUDE*sonar_altitude_D + KI_SONAR_ALTITUDE*sonar_altitude_I; + sonar_altitude_D = (float)(err_altitude-err_altitude_old)/GdT_SONAR_ALTITUDE; + command_altitude = KP_SONAR_ALTITUDE*err_altitude + KI_SONAR_ALTITUDE*sonar_altitude_I + KD_SONAR_ALTITUDE*sonar_altitude_D ; + command_altitude = initial_throttle + constrain(command_altitude,-ALTITUDE_CONTROL_SONAR_OUTPUT_MIN,ALTITUDE_CONTROL_SONAR_OUTPUT_MAX); Log_Write_PID(4,KP_SONAR_ALTITUDE*err_altitude,KI_SONAR_ALTITUDE*sonar_altitude_I,KD_SONAR_ALTITUDE*sonar_altitude_D,command_altitude); - return (initial_throttle + constrain(command_altitude,-ALTITUDE_CONTROL_SONAR_OUTPUT_MIN,ALTITUDE_CONTROL_SONAR_OUTPUT_MAX)); + return command_altitude; } /* ************************************************************ */ diff --git a/ArducopterNG/Sensors.pde b/ArducopterNG/Sensors.pde index acaf9c9a58..e85b1c7e66 100644 --- a/ArducopterNG/Sensors.pde +++ b/ArducopterNG/Sensors.pde @@ -103,34 +103,32 @@ void read_baro(void) #ifdef IsSONAR /* This function reads in the values from the attached range finders (currently only downward pointing sonar) */ void read_Sonar() -{ - // -ve = number of invalid readings, +ve = number of valid readings (in a row) - static int sonar_valid_count = 0; - +{ // calculate altitude from down sensor AP_RangeFinder_down.read(); - if( AP_RangeFinder_down.distance >= AP_RangeFinder_down.max_distance * 0.8 ) { - // if we get too many invalid distances, mark sonar as invalid - if( sonar_status == SONAR_STATUS_OK ) - { - sonar_valid_count--; - if( sonar_valid_count <= -5 ) - sonar_status = SONAR_STATUS_BAD; - } + + // translate into an altitude + press_sonar_altitude = DCM_Matrix[2][2] * AP_RangeFinder_down.distance; + + // deal with the unusual case that we're up-side-down + if( press_sonar_altitude < 0 ) + press_sonar_altitude = 0; + + // set sonar status to OK and update sonar_valid_count which shows reliability of sonar (i.e. are we out of range?) + if( AP_RangeFinder_down.distance > sonar_threshold ) { + sonar_status = SONAR_STATUS_BAD; + if( sonar_valid_count > 0 ) + sonar_valid_count = -1; + else + sonar_valid_count--; }else{ - if( sonar_status == SONAR_STATUS_BAD ) - { + sonar_status = SONAR_STATUS_OK; + if( sonar_valid_count < 0 ) + sonar_valid_count = 1; + else sonar_valid_count++; - if( sonar_valid_count >= 5 ) - sonar_status = SONAR_STATUS_OK; - }else { - press_sonar_altitude = DCM_Matrix[2][2] * AP_RangeFinder_down.distance;\ - - // deal with the unusual case that we're up-side-down - if( press_sonar_altitude < 0 ) - press_sonar_altitude = 0; - } } + sonar_valid_count = constrain(sonar_valid_count,-10,10); #if LOG_RANGEFINDER && !defined(IsRANGEFINDER) Log_Write_RangeFinder(AP_RangeFinder_down.distance,0,0,0,0,0); diff --git a/ArducopterNG/System.pde b/ArducopterNG/System.pde index 22bb807363..31b8944645 100644 --- a/ArducopterNG/System.pde +++ b/ArducopterNG/System.pde @@ -185,7 +185,9 @@ void APM_Init() { // Sonar for Altitude hold #ifdef IsSONAR AP_RangeFinder_down.init(AP_RANGEFINDER_PITOT_TUBE, &adc); AP_RangeFinder_down.set_orientation(AP_RANGEFINDER_ORIENTATION_DOWN); + //AP_RangeFinder_down.init(AN5); AP_RangeFinder_down.set_orientation(AP_RANGEFINDER_ORIENTATION_DOWN); sonar_threshold = AP_RangeFinder_down.max_distance * 0.8; + sonar_status = SONAR_STATUS_OK; // assume sonar is ok to start with #endif // RangeFinders for obstacle avoidance