static bool cutoff_procedure(long code)
{
if (code == 0xdead)
{
#if !(ARCH & ARCH_UNITTEST)
radio_printf("---CUTOFF ACTIVATED---\n");
for (int i = 0; i < 3; i++)
{
for (int c = 0; c < N_CUTOFF; c++)
{
radio_printf("CUTOFF%d pulse %d\n", c+1, i+1);
CUTOFF_ENABLE(c, true);
timer_delay(10000);
CUTOFF_ENABLE(c, false);
radio_printf("CUTOFF%d pulse done\n", c+1);
timer_delay(5000);
}
}
radio_printf("Cutoff completed.\n");
#else
LOG_INFO("---CUTOFF ACTIVATED---\n");
#endif
return true;
}
else
{
LOG_INFO("Wrong cutoff code: %lx\n", code);
return false;
}
}
bool cutoff_check(ticks_t now, int32_t curr_alt, udegree_t lat, udegree_t lon)
{
bool cutoff =(!cutoff_checkTime(now) ||
!cutoff_checkDist(lat, lon, now) ||
!cutoff_checkAltitude(curr_alt, now) ||
!cutoff_checkMaxalt(curr_alt, now));
static bool logged = false;
if (cutoff)
{
if (cutoff_pause_time && timer_clock() - cutoff_pause_time < ms_to_ticks(PAUSE_TIME))
{
if (!logged)
{
radio_printf("CUTOFF pending!\n");
logged = true;
}
}
else
{
cutoff_pause_time = 0;
logged = false;
cutoff_cut();
}
}
else
{
if (logged)
radio_printf("Pending CUTOFF cancelled\n");
logged = false;
}
return cutoff;
}
static bool cutoff_checkTime(ticks_t now)
{
static bool logged = false;
static bool warn_logged = false;
if (now - status_missionStartTicks() > ms_to_ticks(mission_timeout * 1000))
{
if (!logged)
{
radio_printf("CUTOFF:mission timeout\n");
logged = true;
}
return false;
}
else if (now - status_missionStartTicks() > ms_to_ticks(mission_timeout * 1000) - ms_to_ticks(5*60*1000))
{
if (!warn_logged)
{
radio_printf("CUTOFF:mission end in %ds\n", mission_timeout - ticks_to_ms(now - status_missionStartTicks()) / 1000);
warn_logged = true;
}
return true;
}
else
{
logged = false;
warn_logged = false;
return true;
}
}
static void NORETURN curr_process(void)
{
while (1)
{
if (!curr_override && sensor_read(ADC_CURR) > curr_limit)
{
power_enable(false);
if (!curr_logged)
{
radio_printf("Current overrange!\n");
radio_printf("Switching OFF aux power\n");
curr_logged = true;
}
}
timer_delay(1000);
}
}
static bool cutoff_checkDist(udegree_t lat, udegree_t lon, ticks_t now)
{
static ticks_t dist_ko_time;
if (status_currStatus() != BSM2_GROUND_WAIT
&& status_currStatus() != BSM2_NOFIX)
{
float curr_dist = distance(start_latitude, start_longitude, lat / 1e6, lon / 1e6);
if (curr_dist > dist_max_meters)
{
static bool logged = false;
if (dist_ok)
{
radio_printf("CUTOFF:distance over range\n");
radio_printf("CUTOFF in %lds\n", (long)dist_timeout);
LOG_INFO("Current position %8.06f %9.06f, distance from base: %.0fm; limit %ldm, starting %lds timeout\n",
lat / 1e6, lon / 1e6, curr_dist, (long)dist_max_meters, (long)dist_timeout);
dist_ok = false;
dist_ko_time = now;
logged = false;
}
else if (now - dist_ko_time > ms_to_ticks(dist_timeout * 1000))
{
if (!logged)
{
radio_printf("CUTOFF:distance timeout\n");
logged = true;
}
return false;
}
return true;
}
}
if (!dist_ok)
LOG_INFO("Distance from base ok\n");
dist_ok = true;
return true;
}
static bool cutoff_checkAltitude(int32_t curr_alt, ticks_t now)
{
static ticks_t alt_ko_time;
if (status_currStatus() != BSM2_GROUND_WAIT
&& status_currStatus() != BSM2_NOFIX)
{
alt_max = MAX(alt_max, curr_alt);
if (alt_max - curr_alt > delta_altitude)
{
static bool logged = false;
if (alt_ok)
{
radio_printf("CUTOFF:burst detected\n");
radio_printf("CUTOFF in %lds\n", (long)altitude_timeout);
LOG_INFO("Current altitude %ld, peak altitude %ld; current altitude lower than delta, starting %ld s timeout\n",
(long)curr_alt, (long)alt_max, (long)altitude_timeout);
alt_ok = false;
logged = false;
alt_ko_time = now;
}
else if (now - alt_ko_time > ms_to_ticks(altitude_timeout * 1000))
{
if (!logged)
{
radio_printf("CUTOFF:burst timeout\n");
logged = true;
}
return false;
}
return true;
}
}
if (!alt_ok)
LOG_INFO("Current altitude ok\n");
alt_ok = true;
return true;
}
static bool cutoff_checkMaxalt(int32_t curr_alt, ticks_t now)
{
static ticks_t maxalt_ko_time;
if (status_currStatus() != BSM2_GROUND_WAIT
&& status_currStatus() != BSM2_NOFIX)
{
if (curr_alt > altmax_meters)
{
static bool logged = false;
if (maxalt_ok)
{
radio_printf("CUTOFF:altitude over range\n");
radio_printf("CUTOFF in %lds\n", (long)altmax_timeout);
LOG_INFO("Altitude: %ldm; limit %ldm, starting %lds timeout\n",
(long)curr_alt, (long)altmax_meters, (long)altmax_timeout);
maxalt_ok = false;
maxalt_ko_time = now;
logged = false;
}
else if (now - maxalt_ko_time > ms_to_ticks(altmax_timeout * 1000))
{
if (!logged)
{
radio_printf("CUTOFF:altitude timeout\n");
logged = true;
}
return false;
}
return true;
}
}
if (!maxalt_ok)
LOG_INFO("Maximum altitude ok\n");
maxalt_ok = true;
return true;
}
static void status_set(Bsm2Status new_status)
{
ASSERT(new_status < BSM2_CNT);
if (new_status != curr_status)
{
LOG_INFO("Changing status to %s\n", status_names[new_status]);
#if !(ARCH & ARCH_UNITTEST)
radio_printf("Changing status to %s", status_names[new_status]);
#endif
}
curr_status = new_status;
if (curr_status == BSM2_LANDING)
landing_buz_start();
}
