Mutliwii vbat autoland

wareck · Post by **wareck** » Tue Sep 01, 2015 11:20 am

I tried to experiment new function "Autoland on VBAT".
This fuction allow copter to autoland if battery reach the VBATLEVEL_WARN2

what we need:

need a GPS, because before do autoland, copter must be leveled and not drifting
need a BARO, because board need alt information for smooth landing

how it works:

In config.h you can enable or disable this function, and choose after how many battery warning start to land (between 1 and 5)
If battery reach VBATLEVEL_WARN2 before arming your copter, board will not allow arming...Just to be sure not starting flying with bad battery pack
while copter is flying, if battery reach level warning 2, system automaticly stop MISSION, RTH or manual flying and engage LAND process
Automaticly goes in POS-HOLD and STABLE mode, level copter and stop drift, and when copter is stabilized slowly land
When copter is landed, you just have to push back throttle to disarm.

I'll put my code here this evening, I need to make some other flying test, but it's rainning a lot...

Olivier

Kbev5709 · Post by **Kbev5709** » Tue Sep 01, 2015 2:42 pm

Sounds like an awesome feature. Please do follow up on this.

wareck · Post by **wareck** » Tue Sep 01, 2015 10:33 pm

Hi,
This is my beta code (maybe need to be optimized, but working well on mega board)

alarm.cpp:
find

Code: Select all

#if defined(ARMEDTIMEWARNING)
    if (ArmedTimeWarningMicroSeconds > 0 && armedTime >= ArmedTimeWarningMicroSeconds && f.ARMED) alarmArray[ALRM_FAC_RUNTIME] = ALRM_LVL_ON;
    else alarmArray[ALRM_FAC_RUNTIME] = ALRM_LVL_OFF;
  #endif

replace by:

Code: Select all

 #if defined(ARMEDTIMEWARNING)
    if (ArmedTimeWarningMicroSeconds > 0 && armedTime >= ArmedTimeWarningMicroSeconds && f.ARMED) alarmArray[ALRM_FAC_RUNTIME] = ALRM_LVL_ON;
    else alarmArray[ALRM_FAC_RUNTIME] = ALRM_LVL_OFF;
  #endif
  
#if defined (VBAT) && defined (VBAT_ALAND) && !GPS 
#error "Unfortunatly GPS is needed for using VBAT_ALAND"
#endif

config.h:
find

Code: Select all

#define VBAT              // uncomment this line to activate the vbat code
    #define VBATSCALE       131 // (*) (**) change this value if readed Battery voltage is different than real voltage
    #define VBATNOMINAL     126 // 12,6V full battery nominal voltage - only used for lcd.telemetry
    #define VBATLEVEL_WARN1 107 // (*) (**) 10,7V
    #define VBATLEVEL_WARN2  99 // (*) (**) 9.9V
    #define VBATLEVEL_CRIT   93 // (*) (**) 9.3V - critical condition: if vbat ever goes below this value, permanent alarm is triggered
    #define NO_VBAT          16 // Avoid beeping without any battery
    #define VBAT_OFFSET       0 // offset in 0.1Volts, gets added to voltage value  - useful for zener diodes

replace by:

Code: Select all

#define VBAT              // uncomment this line to activate the vbat code
    #define VBATSCALE       131 // (*) (**) change this value if readed Battery voltage is different than real voltage
    #define VBATNOMINAL     126 // 12,6V full battery nominal voltage - only used for lcd.telemetry
    #define VBATLEVEL_WARN1 107 // (*) (**) 10,7V
    #define VBATLEVEL_WARN2  99 // (*) (**) 9.9V
    #define VBATLEVEL_CRIT   93 // (*) (**) 9.3V - critical condition: if vbat ever goes below this value, permanent alarm is triggered
    #define NO_VBAT          16 // Avoid beeping without any battery
    #define VBAT_OFFSET       0 // offset in 0.1Volts, gets added to voltage value  - useful for zener diodes

    #define VBAT_ALAND         // when bat reach VBATLEVEL2_WARN copter, autolanding
    #define VBAT_ALAND_CNT    3 // Number of Bat Warning level 2 heared before activate autoland

multiwii.cpp:

find

Code: Select all

int32_t  AltHold; // in cm
int16_t  sonarAlt;
int16_t  BaroPID = 0;
int16_t  errorAltitudeI = 0;

remplace by:

Code: Select all

int32_t  AltHold; // in cm
int16_t  sonarAlt;
int16_t  BaroPID = 0;
int16_t  errorAltitudeI = 0;

#if defined (VBAT) && defined (VBAT_ALAND)
uint8_t  BatAlarm_Land = 0;
int16_t  vbatland_count = 0;
#endif

find

Code: Select all

 #if defined(VBAT)
        if (analog.vbat > NO_VBAT) vbatMin = analog.vbat;
#endif

replace by

Code: Select all

 #if defined(VBAT)
        if (analog.vbat > NO_VBAT) vbatMin = analog.vbat;
#if defined (VBAT_ALAND)
      if (analog.vbat < conf.vbatlevel_warn2) // don't allow to fly if bat is bad
      { 
         f.ARMED = 0;
         if (alarmArray[1] == 0)
            alarmArray[1] = 1;
         else if (alarmArray[1] == 1)
            alarmArray[1] = 0;
      }
      else {
         vbatland_count = 0;
      }
      #endif
      #endif

find

Code: Select all

if (f.ARMED ) {                       //Check GPS status and armed
      //TODO: implement f.GPS_Trusted flag, idea from Dramida - Check for degraded HDOP and sudden speed jumps
      if (f.GPS_FIX) {

replace by

Code: Select all

  if (f.ARMED ) {                       //Check GPS status and armed
      //TODO: implement f.GPS_Trusted flag, idea from Dramida - Check for degraded HDOP and sudden speed jumps
#if defined (VBAT) && defined (VBAT_ALAND)
      if (analog.vbat <= conf.vbatlevel_warn2 && BatAlarm_Land == 0) // If battery reach WARN2 start process
      {
         vbatland_count++;
         if (vbatland_count >= 60 * VBAT_ALAND_CNT) //compare counter with value choosen in config.h
         {
            f.VBAT_AUTOLAND = 1; // start autoland   
            BatAlarm_Land = 1;
         }
      }
      if (f.VBAT_AUTOLAND == 1){ // Land start
         vbatland_count = 0;
         f.GPS_mode = GPS_MODE_HOLD;
         f.GPS_BARO_MODE = true;
         GPS_set_next_wp(&GPS_coord[LAT], &GPS_coord[LON], &GPS_coord[LAT], &GPS_coord[LON]);
         set_new_altitude(alt.EstAlt);
         NAV_state = NAV_STATE_LAND_START;
         f.VBAT_AUTOLAND = 0;
      }
#endif
      if (f.GPS_FIX) {

multiwii.h
find

Code: Select all

#if defined(LOG_VALUES) || defined(LCD_TELEMETRY) || defined(ARMEDTIMEWARNING) || defined(LOG_PERMANENT)
  extern uint32_t armedTime;
#endif

replace by

Code: Select all

#if defined(LOG_VALUES) || defined(LCD_TELEMETRY) || defined(ARMEDTIMEWARNING) || defined(LOG_PERMANENT)
  extern uint32_t armedTime;
#endif

#if defined (VBAT) && defined (VBAT_ALAND)
  extern int16_t vbatland_count;
  extern uint8_t  land_BatAlarm;
#endif

type.h
find

Code: Select all

#if GPS
  uint8_t GPS_FIX :1 ;
  uint8_t GPS_FIX_HOME :1 ;
  uint8_t GPS_BARO_MODE : 1;         // This flag is used when GPS controls baro mode instead of user (it will replace rcOptions[BARO]
  uint8_t GPS_head_set: 1;           // it is 1 if the navigation engine got commands to control heading (SET_POI or SET_HEAD) CLEAR_HEAD will zero it
  uint8_t LAND_COMPLETED: 1;
  uint8_t LAND_IN_PROGRESS: 1;
#endif
} flags_struct_t;

replace by

Code: Select all

#if GPS
  uint8_t GPS_FIX :1 ;
  uint8_t GPS_FIX_HOME :1 ;
  uint8_t GPS_BARO_MODE : 1;         // This flag is used when GPS controls baro mode instead of user (it will replace rcOptions[BARO]
  uint8_t GPS_head_set: 1;           // it is 1 if the navigation engine got commands to control heading (SET_POI or SET_HEAD) CLEAR_HEAD will zero it
  uint8_t LAND_COMPLETED: 1;
  uint8_t LAND_IN_PROGRESS: 1;
#endif
#if defined (VBAT) && defined (VBAT_ALAND)
  uint8_t VBAT_AUTOLAND : 1;
#endif
} flags_struct_t;

That's all...
I tried it in real condition with a mega board (DROTEK Drofly V3)
After battery reach warnlevel2 I can heard 3 buzzer pattern (battery level warning) and just after, copter start to autoland...

Is to risky to enable RTH instead of land, because if copter is drifting and no more battery to go home, it may flip and crash.
In land mode, copter is leveled ans slow goes down. if something goes wrong, it will stay almost level and minimise crash.

I'm a little busy this week, but I'm still working on this code, so if someone have idea or find better solution, it will be awesome to make this code better...

ps: sorry for my english :oops:

wareck · Post by **wareck** » Mon Sep 07, 2015 4:27 pm

Here 2.4 already patched...

edsimmons3 · Post by **edsimmons3** » Fri Sep 11, 2015 6:37 am

Nice work - this looks like it's a very good idea... I'll give this a try once I've resolved my probably memory related issues...

Is your modified code stored on github or anything? Will it be possible at some point for this to be vetted and merged into the main project?

Thanks

Ed

wareck · Post by **wareck** » Fri Sep 11, 2015 10:25 am

I've just build a github for this code...

https://github.com/wareck/Multiwii_2.4_autoland_onbat

edsimmons3 · Post by **edsimmons3** » Fri Sep 11, 2015 12:33 pm

Brilliant... ;-)

Thanks for your work... I will test and hopefully this can be merged with the main code as an option.

:-)

MultiWii

Mutliwii vbat autoland

Mutliwii vbat autoland

Re: Mutliwii vbat autoland

Re: Mutliwii vbat autoland

Re: Mutliwii vbat autoland

Re: Mutliwii vbat autoland

Re: Mutliwii vbat autoland

Re: Mutliwii vbat autoland