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.hfind
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.hfind
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