preflight_checks.c

Go to the documentation of this file.

/*
 * Copyright (C) 2022 Freek van Tienen <freek.v.tienen@gmail.com>
 *
 * This file is part of paparazzi
 *
 * paparazzi is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2, or (at your option)
 * any later version.
 *
 * paparazzi is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with paparazzi; see the file COPYING.  If not, see
 * <http://www.gnu.org/licenses/>.
 */
 
#include "preflight_checks.h"
#include "modules/datalink/telemetry.h"
#include "modules/loggers/flight_recorder.h"
#include <stdio.h>
 
#ifndef PREFLIGHT_CHECK_MAX_MSGBUF
#define PREFLIGHT_CHECK_MAX_MSGBUF 1024
#endif
 
#ifndef PREFLIGHT_CHECK_SEPERATOR
#define PREFLIGHT_CHECK_SEPERATOR ';'
#endif
 
#ifndef PREFLIGHT_CHECK_INFO_TIMEOUT
#define PREFLIGHT_CHECK_INFO_TIMEOUT 5
#endif
 
#ifndef PREFLIGHT_CHECK_GROUND
#define PREFLIGHT_CHECK_GROUND TRUE
#endif
 
#ifndef PREFLIGHT_CHECK_BYPASS
#define PREFLIGHT_CHECK_BYPASS FALSE
#endif
 
bool preflight_bypass = PREFLIGHT_CHECK_BYPASS;
bool preflight_ground_done = false;
 
static struct preflight_check_t *preflight_head = NULL;
 
void preflight_check_register(struct preflight_check_t *check, preflight_check_f func)
{
  // Prepend the preflight check
  struct preflight_check_t *next = preflight_head;
  preflight_head = check;
  check->func = func;
  check->next = next;
}
 
bool preflight_check(void)
{
  static float last_info_time = 0;
  char error_msg[PREFLIGHT_CHECK_MAX_MSGBUF];
  struct preflight_result_t result = {
    .message = error_msg,
    .max_len = PREFLIGHT_CHECK_MAX_MSGBUF,
    .fail_cnt = 0,
    .warning_cnt = 0,
    .success_cnt = 0
  };
 
  // Go through all the checks
  struct preflight_check_t *check = preflight_head;
  while (check != NULL) {
    // Peform the check and register errors
    check->func(&result);
    check = check->next;
  }
 
  // Add the ground check
#if PREFLIGHT_CHECK_GROUND
  if (!preflight_ground_done) {
    preflight_error(&result, "Ground checks not done");
  } else {
    preflight_success(&result, "Ground checks done");
  }
#endif
 
  // We failed a check or have a warning
  if (result.fail_cnt > 0 || result.warning_cnt > 0) {
    // Only send every xx amount of seconds
    if ((get_sys_time_float() - last_info_time) > PREFLIGHT_CHECK_INFO_TIMEOUT) {
      // Record the total
      int rc = 0;
      if (result.fail_cnt > 0) {
        rc = snprintf(result.message, result.max_len, "*Preflight fail [fail:%d warn:%d tot:%d]*", result.fail_cnt,
                      result.warning_cnt, (result.fail_cnt + result.warning_cnt + result.success_cnt));
      } else {
        rc = snprintf(result.message, result.max_len, "*Preflight success with warnings [%d/%d]*", result.warning_cnt,
                      (result.fail_cnt + result.warning_cnt + result.success_cnt));
      }
      if (rc > 0) {
        result.max_len -= rc;
      }
 
      // Send the errors seperatly
      uint16_t last_sendi = 0;
      for (uint16_t i = 0; i <= PREFLIGHT_CHECK_MAX_MSGBUF - result.max_len; i++) {
        if (error_msg[i] == PREFLIGHT_CHECK_SEPERATOR || i == (PREFLIGHT_CHECK_MAX_MSGBUF - result.max_len)) {
          DOWNLINK_SEND_INFO_MSG(DefaultChannel, DefaultDevice, i - last_sendi, &error_msg[last_sendi]);
#if FLIGHTRECORDER_SDLOG
          pprz_msg_send_INFO_MSG(&pprzlog_tp.trans_tx, &flightrecorder_sdlog.device, AC_ID, i - last_sendi, &error_msg[last_sendi]);
#endif
          last_sendi = i + 1;
        }
      }
 
      // Update the time
      last_info_time = get_sys_time_float();
    }
 
    // Only if we fail a check
    if (result.fail_cnt > 0) {
      return false;
    } else {
      return true;
    }
  }
 
  // Send success down
  int rc = snprintf(error_msg, PREFLIGHT_CHECK_MAX_MSGBUF, "*Preflight success [%d]*", result.success_cnt);
  if (rc > 0) {
    DOWNLINK_SEND_INFO_MSG(DefaultChannel, DefaultDevice, rc, error_msg);
#if FLIGHTRECORDER_SDLOG
    pprz_msg_send_INFO_MSG(&pprzlog_tp.trans_tx, &flightrecorder_sdlog.device, AC_ID, strlen(error_msg), error_msg);
#endif
  }
 
  // Return success if we didn't fail a preflight check
  return true;
}
 
void preflight_error(struct preflight_result_t *result, const char *fmt, ...)
{
  // Record the error count
  result->fail_cnt++;
 
  // No more space in the message
  if (result->max_len <= 0) {
    return;
  }
 
  // Add the error
  va_list args;
  va_start(args, fmt);
  int rc = vsnprintf(result->message, result->max_len, fmt, args);
  va_end(args);
 
  // Remove the length from the buffer if it was successfull
  if (rc > 0) {
    result->max_len -= rc;
    result->message += rc;
 
    // Add seperator if it fits
    if (result->max_len > 0) {
      result->message[0] = PREFLIGHT_CHECK_SEPERATOR;
      result->max_len--;
      result->message++;
 
      // Add the '\0' character
      if (result->max_len > 0) {
        result->message[0] = 0;
      }
    }
  }
}
 
void preflight_warning(struct preflight_result_t *result, const char *fmt, ...)
{
  // Record the warning count
  result->warning_cnt++;
 
  // No more space in the message
  if (result->max_len <= 0) {
    return;
  }
 
  // Add the warning to the error string
  va_list args;
  va_start(args, fmt);
  int rc = vsnprintf(result->message, result->max_len, fmt, args);
  va_end(args);
 
  // Remove the length from the buffer if it was successfull
  if (rc > 0) {
    result->max_len -= rc;
    result->message += rc;
 
    // Add seperator if it fits
    if (result->max_len > 0) {
      result->message[0] = PREFLIGHT_CHECK_SEPERATOR;
      result->max_len--;
      result->message++;
 
      // Add the '\0' character
      if (result->max_len > 0) {
        result->message[0] = 0;
      }
    }
  }
}
 
void preflight_success(struct preflight_result_t *result, const char *fmt __attribute__((unused)), ...)
{
  // Record the success count
  result->success_cnt++;
}
 
void preflight_checks_log_bypass(bool bypass) {
  char *msg = (bypass)? "*Preflight checks bypassed*" : "*Preflight checks unbypassed*";
  int len = strlen(msg);
  
  DOWNLINK_SEND_INFO_MSG(DefaultChannel, DefaultDevice, len, msg);
#if FLIGHTRECORDER_SDLOG
  pprz_msg_send_INFO_MSG(&pprzlog_tp.trans_tx, &flightrecorder_sdlog.device, AC_ID, len, msg);
#endif
  preflight_bypass = bypass;
}