void loadDefaults() {
state.memoryAvailable = 0;
state.memoryPercentage = 50;
#ifndef NO_GUI
video.sdlStarted = 0;
video.agarStarted = 0;
video.screenWtoApply = 0;
video.screenHtoApply = 0;
video.screenBPP = 0;
#ifdef WITHOUT_AGAR
video.screenFS = 0;
#else
video.screenFS = 1;
#endif
video.screenAA = 0;
strcpy(video.fontFile, "Vera.ttf");
video.fontSize = 11;
#endif
viewInit();
spawnDefaults();
stateInit();
commandInit();
}
static inline void main_init(void)
{
mcu_init();
sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL);
stateInit();
actuators_init();
imu_init();
#if USE_AHRS_ALIGNER
ahrs_aligner_init();
#endif
ahrs_init();
settings_init();
mcu_int_enable();
downlink_init();
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AUTOPILOT_VERSION, send_autopilot_version);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ALIVE, send_alive);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_COMMANDS, send_commands);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ACTUATORS, send_actuators);
// send body_to_imu from here for now
AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu));
}
void NumberParser::run() {
prepare();
States state = States::Init;
Position before;
while ( true ) {
before = _input.position();
char c{};
if ( _input.eof() )
state = States::Quit;
else
c = _input.readChar();
switch ( state ) {
case States::Init:
state = stateInit( c );
break;
case States::Minus:
state = stateMinus( c );
break;
case States::Zero:
state = stateZero( c );
break;
case States::Digits:
state = stateDigits( c );
break;
case States::Point:
state = statePoint( c );
break;
case States::DecimalDigits:
state = stateDecimalDigits( c );
break;
case States::E:
state = stateE( c );
break;
case States::EPlusMinus:
state = stateEplusMinus( c );
break;
case States::EDigits:
state = stateEDigits( c );
break;
default:
break;
}
if ( state == States::Quit )
break;
}
_input.position( before );// return back the last character
if ( _isE )
postProcessing();
if ( _isMinus ) {
if ( isReal() )
_real *= -1;
if ( isInteger() )
_integer *= -1;
}
}
STATIC_INLINE void main_init( void ) {
mcu_init();
electrical_init();
stateInit();
actuators_init();
#if USE_MOTOR_MIXING
motor_mixing_init();
#endif
radio_control_init();
air_data_init();
#if USE_BARO_BOARD
baro_init();
#endif
imu_init();
#if USE_IMU_FLOAT
imu_float_init();
#endif
ahrs_aligner_init();
ahrs_init();
ins_init();
#if USE_GPS
gps_init();
#endif
autopilot_init();
modules_init();
settings_init();
mcu_int_enable();
#if DATALINK == XBEE
xbee_init();
#endif
#if DATALINK == UDP
udp_init();
#endif
// register the timers for the periodic functions
main_periodic_tid = sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL);
modules_tid = sys_time_register_timer(1./MODULES_FREQUENCY, NULL);
radio_control_tid = sys_time_register_timer((1./60.), NULL);
failsafe_tid = sys_time_register_timer(0.05, NULL);
electrical_tid = sys_time_register_timer(0.1, NULL);
telemetry_tid = sys_time_register_timer((1./TELEMETRY_FREQUENCY), NULL);
#if USE_BARO_BOARD
baro_tid = sys_time_register_timer(1./BARO_PERIODIC_FREQUENCY, NULL);
#endif
}
STATIC_INLINE void main_init( void ) {
mcu_init();
electrical_init();
stateInit();
actuators_init();
#if USE_MOTOR_MIXING
// motor_mixing_init();
// servo_mixing_init();
set_servo_init();
#endif
radio_control_init();
baro_init();
imu_init();
autopilot_init();
nav_init();
guidance_h_init();
guidance_v_init();
stabilization_init();
ahrs_aligner_init();
ahrs_init();
ins_init();
#if USE_GPS
gps_init();
#endif
modules_init();
settings_init();
mcu_int_enable();
#if DATALINK == XBEE
xbee_init();
#endif
#if DATALINK == UDP
udp_init();
#endif
// register the timers for the periodic functions
main_periodic_tid = sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL);
modules_tid = sys_time_register_timer(1./MODULES_FREQUENCY, NULL);
radio_control_tid = sys_time_register_timer((1./60.), NULL);
failsafe_tid = sys_time_register_timer(0.05, NULL);
electrical_tid = sys_time_register_timer(0.1, NULL);
baro_tid = sys_time_register_timer(1./BARO_PERIODIC_FREQUENCY, NULL);
telemetry_tid = sys_time_register_timer((1./TELEMETRY_FREQUENCY), NULL);
}
CoreAgent(ros::NodeHandle nh) :
nh_(nh)
{
ROS_INFO("Core Agent Initiated");
if(stateInit())
{
state_ = 1;
}
}
void init_ap( void )
{
#ifndef SINGLE_MCU /** init done in main_fbw in single MCU */
mcu_init();
#endif /* SINGLE_MCU */
/************* Sensors initialization ***************/
#if USE_GPS
gps_init();
#endif
#ifdef USE_GPIO
GpioInit();
#endif
#if USE_IMU
imu_init();
#endif
#if USE_AHRS_ALIGNER
ahrs_aligner_init();
#endif
#if USE_AHRS
ahrs_init();
#endif
#if USE_BAROMETER
baro_init();
#endif
ins_init();
stateInit();
/************* Links initialization ***************/
#if defined MCU_SPI_LINK || defined MCU_UART_LINK
link_mcu_init();
#endif
#if USE_AUDIO_TELEMETRY
audio_telemetry_init();
#endif
/************ Internal status ***************/
autopilot_init();
h_ctl_init();
v_ctl_init();
nav_init();
modules_init();
settings_init();
/**** start timers for periodic functions *****/
sensors_tid = sys_time_register_timer(1./PERIODIC_FREQUENCY, NULL);
navigation_tid = sys_time_register_timer(1./NAVIGATION_FREQUENCY, NULL);
attitude_tid = sys_time_register_timer(1./CONTROL_FREQUENCY, NULL);
modules_tid = sys_time_register_timer(1./MODULES_FREQUENCY, NULL);
telemetry_tid = sys_time_register_timer(1./60, NULL);
monitor_tid = sys_time_register_timer(1.0, NULL);
/** - start interrupt task */
mcu_int_enable();
#if defined DATALINK
#if DATALINK == XBEE
xbee_init();
#endif
#if DATALINK == W5100
w5100_init();
#endif
#endif /* DATALINK */
#if defined AEROCOMM_DATA_PIN
IO0DIR |= _BV(AEROCOMM_DATA_PIN);
IO0SET = _BV(AEROCOMM_DATA_PIN);
#endif
/************ Multi-uavs status ***************/
#ifdef TRAFFIC_INFO
traffic_info_init();
#endif
}
STATIC_INLINE void main_init(void)
{
mcu_init();
electrical_init();
stateInit();
actuators_init();
#if USE_MOTOR_MIXING
motor_mixing_init();
#endif
radio_control_init();
#if USE_BARO_BOARD
baro_init();
#endif
#if USE_IMU
imu_init();
#endif
#if USE_AHRS_ALIGNER
ahrs_aligner_init();
#endif
#if USE_AHRS
ahrs_init();
#endif
ins_init();
#if USE_GPS
gps_init();
#endif
autopilot_init();
modules_init();
settings_init();
mcu_int_enable();
#if DOWNLINK
downlink_init();
#endif
// register the timers for the periodic functions
main_periodic_tid = sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL);
modules_tid = sys_time_register_timer(1. / MODULES_FREQUENCY, NULL);
radio_control_tid = sys_time_register_timer((1. / 60.), NULL);
failsafe_tid = sys_time_register_timer(0.05, NULL);
electrical_tid = sys_time_register_timer(0.1, NULL);
telemetry_tid = sys_time_register_timer((1. / TELEMETRY_FREQUENCY), NULL);
#if USE_BARO_BOARD
baro_tid = sys_time_register_timer(1. / BARO_PERIODIC_FREQUENCY, NULL);
#endif
#if USE_IMU
// send body_to_imu from here for now
AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu));
#endif
// Do a failsafe check first
failsafe_check();
}
int
main(int ac, char *av[])
{
int status = 0;
char* logfname = 0;
/// Data directory, conffile paths may be relative
const char* datadir;
int interval = DEFAULT_INTERVAL;
prod_spec spec;
prod_class_t clss;
int toffset = TOFFSET_NONE;
int loggingToStdErr = 0;
unsigned queue_size = 5000;
const char* progname = basename(av[0]);
unsigned logopts = LOG_CONS|LOG_PID;
/*
* Setup default logging before anything else.
*/
(void)log_init(progname);
const char* pqfname = getQueuePath();
spec.feedtype = DEFAULT_FEEDTYPE;
spec.pattern = DEFAULT_PATTERN;
if(set_timestamp(&clss.from)) /* corrected by toffset below */
{
int errnum = errno;
log_error("Couldn't set timestamp: %s", strerror(errnum));
exit(EXIT_FAILURE);
/*NOTREACHED*/
}
clss.to = TS_ENDT;
clss.psa.psa_len = 1;
clss.psa.psa_val = &spec;
/*
* deal with the command line, set options
*/
{
extern int optind;
extern int opterr;
extern char *optarg;
int ch;
int fterr;
opterr = 1;
while ((ch = getopt(ac, av, "vxel:d:f:q:o:p:i:t:")) != EOF) {
switch (ch) {
case 'v':
if (!log_is_enabled_info)
(void)log_set_level(LOG_LEVEL_INFO);
break;
case 'x':
(void)log_set_level(LOG_LEVEL_DEBUG);
break;
case 'e':
key = ftok("/etc/rc.d/rc.local",'R');
semkey = ftok("/etc/rc.d/rc.local",'e');
shmid = shmget(key, sizeof(edex_message) * queue_size,
0666 | IPC_CREAT);
semid = semget(semkey, 2, 0666 | IPC_CREAT);
break;
case 'l':
logfname = optarg;
(void)log_set_destination(logfname);
break;
case 'd':
setPqactDataDirPath(optarg);
break;
case 'f':
fterr = strfeedtypet(optarg, &spec.feedtype);
if(fterr != FEEDTYPE_OK)
{
log_error("Bad feedtype \"%s\", %s\n",
optarg, strfeederr(fterr));
usage(progname);
}
break;
case 'q':
pqfname = optarg;
break;
case 'o':
toffset = atoi(optarg);
if(toffset == 0 && *optarg != '0')
{
log_error("invalid offset %s\n", optarg);
usage(progname);
}
break;
case 'i':
interval = atoi(optarg);
if(interval == 0 && *optarg != '0')
{
log_error("invalid interval %s\n", optarg);
usage(progname);
}
break;
case 't':
pipe_timeo = atoi(optarg);
if(pipe_timeo == 0 && *optarg != 0)
{
log_error("invalid pipe_timeo %s", optarg);
usage(progname);
}
break;
case 'p':
spec.pattern = optarg;
break;
default:
usage(progname);
break;
}
}
conffilename = getPqactConfigPath();
datadir = getPqactDataDirPath();
{
int numOperands = ac - optind;
if (1 < numOperands) {
log_error("Too many operands");
usage(progname);
}
else if (1 == numOperands) {
conffilename = av[optind];
}
}
}
setQueuePath(pqfname);
log_notice("Starting Up");
if ('/' != conffilename[0]) {
/*
* The pathname of the configuration-file is relative. Convert it
* to absolute so that it can be (re)read even if the current
* working directory changes.
*/
#ifdef PATH_MAX
char buf[PATH_MAX]; /* includes NUL */
#else
char buf[_POSIX_PATH_MAX]; /* includes NUL */
#endif
if (getcwd(buf, sizeof(buf)) == NULL) {
log_syserr("Couldn't get current working directory");
exit(EXIT_FAILURE);
}
(void)strncat(buf, "/", sizeof(buf)-strlen(buf)-1);
(void)strncat(buf, conffilename, sizeof(buf)-strlen(buf)-1);
conffilename = strdup(buf);
if (conffilename == NULL) {
log_syserr("Couldn't duplicate string \"%s\"", buf);
exit(EXIT_FAILURE);
}
}
/*
* Initialize the previous-state module for this process.
*/
if (stateInit(conffilename) < 0) {
log_error("Couldn't initialize previous-state module");
exit(EXIT_FAILURE);
/*NOTREACHED*/
}
/*
* Configure the standard I/O streams for execution of child processes.
*/
if (configure_stdio_file_descriptors()) {
log_error("Couldn't configure standard I/O streams for execution "
"of child processes");
exit(EXIT_FAILURE);
}
/*
* Inform the "filel" module about the number of available file
* descriptors. File descriptors are reserved for stdin, stdout,
* stderr, the product-queue, the configuration-file, and (possibly)
* logging.
*/
if (0 != set_avail_fd_count(openMax() - 6))
{
log_error("Couldn't set number of available file-descriptors");
log_notice("Exiting");
exit(EXIT_FAILURE);
/*NOTREACHED*/
}
/*
* Inform the "filel" module of the shared memory segment
*/
if (shmid != -1 && semid != -1)
{
set_shared_space(shmid, semid, queue_size);
}
/*
* Compile the pattern.
*/
if (re_isPathological(spec.pattern))
{
log_error("Adjusting pathological regular-expression: \"%s\"",
spec.pattern);
re_vetSpec(spec.pattern);
}
status = regcomp(&spec.rgx, spec.pattern, REG_EXTENDED|REG_NOSUB);
if(status != 0)
{
log_error("Can't compile regular expression \"%s\"",
spec.pattern);
log_notice("Exiting");
exit(EXIT_FAILURE);
/*NOTREACHED*/
}
/*
* register exit handler
*/
if(atexit(cleanup) != 0)
{
log_syserr("atexit");
log_notice("Exiting");
exit(EXIT_FAILURE);
/*NOTREACHED*/
}
/*
* set up signal handlers
*/
set_sigactions();
/*
* Read in (compile) the configuration file. We do this first so
* its syntax may be checked without opening a product queue.
*/
if ((status = readPatFile(conffilename)) < 0) {
exit(EXIT_FAILURE);
/*NOTREACHED*/
}
else if (status == 0) {
log_notice("Configuration-file \"%s\" has no entries. "
"You should probably not start this program instead.",
conffilename);
}
/*
* Open the product queue
*/
status = pq_open(pqfname, PQ_READONLY, &pq);
if(status)
{
if (PQ_CORRUPT == status) {
log_error("The product-queue \"%s\" is inconsistent\n",
pqfname);
}
else {
log_error("pq_open failed: %s: %s\n",
pqfname, strerror(status));
}
exit(EXIT_FAILURE);
/*NOTREACHED*/
}
if(toffset != TOFFSET_NONE) {
/*
* Filter and queue position set by "toffset".
*/
clss.from.tv_sec -= toffset;
pq_cset(pq, &clss.from);
}
else {
bool startAtTailEnd = true;
timestampt insertTime;
clss.from = TS_ZERO;
/*
* Try getting the insertion-time of the last,
* successfully-processed data-product from the previous session.
*/
status = stateRead(&insertTime);
if (status) {
log_warning("Couldn't get insertion-time of last-processed "
"data-product from previous session");
}
else {
timestampt now;
(void)set_timestamp(&now);
if (tvCmp(now, insertTime, <)) {
log_warning("Time of last-processed data-product from previous "
"session is in the future");
}
else {
char buf[80];
(void)strftime(buf, sizeof(buf), "%Y-%m-%d %T",
gmtime(&insertTime.tv_sec));
log_notice("Starting from insertion-time %s.%06lu UTC", buf,
(unsigned long)insertTime.tv_usec);
pq_cset(pq, &insertTime);
startAtTailEnd = false;
}
}
if (startAtTailEnd) {
log_notice("Starting at tail-end of product-queue");
(void)pq_last(pq, &clss, NULL);
}
}
void init_ap( void ) {
#ifndef SINGLE_MCU /** init done in main_fbw in single MCU */
mcu_init();
#endif /* SINGLE_MCU */
/****** initialize and reset state interface ********/
stateInit();
/************* Sensors initialization ***************/
#if USE_GPS
gps_init();
#endif
#if USE_IMU
imu_init();
#if USE_IMU_FLOAT
imu_float_init();
#endif
#endif
#if USE_AHRS_ALIGNER
ahrs_aligner_init();
#endif
#if USE_AHRS
ahrs_init();
#endif
#if USE_AHRS && USE_IMU
register_periodic_telemetry(DefaultPeriodic, "STATE_FILTER_STATUS", send_fliter_status);
#endif
air_data_init();
#if USE_BARO_BOARD
baro_init();
#endif
ins_init();
/************* Links initialization ***************/
#if defined MCU_SPI_LINK || defined MCU_UART_LINK
link_mcu_init();
#endif
#if USE_AUDIO_TELEMETRY
audio_telemetry_init();
#endif
/************ Internal status ***************/
autopilot_init();
h_ctl_init();
v_ctl_init();
nav_init();
modules_init();
settings_init();
/**** start timers for periodic functions *****/
sensors_tid = sys_time_register_timer(1./PERIODIC_FREQUENCY, NULL);
navigation_tid = sys_time_register_timer(1./NAVIGATION_FREQUENCY, NULL);
attitude_tid = sys_time_register_timer(1./CONTROL_FREQUENCY, NULL);
modules_tid = sys_time_register_timer(1./MODULES_FREQUENCY, NULL);
telemetry_tid = sys_time_register_timer(1./TELEMETRY_FREQUENCY, NULL);
monitor_tid = sys_time_register_timer(1.0, NULL);
/** - start interrupt task */
mcu_int_enable();
#if defined DATALINK
#if DATALINK == XBEE
xbee_init();
#endif
#if DATALINK == W5100
w5100_init();
#endif
#endif /* DATALINK */
#if defined AEROCOMM_DATA_PIN
IO0DIR |= _BV(AEROCOMM_DATA_PIN);
IO0SET = _BV(AEROCOMM_DATA_PIN);
#endif
/************ Multi-uavs status ***************/
#ifdef TRAFFIC_INFO
traffic_info_init();
#endif
/* set initial trim values.
* these are passed to fbw via inter_mcu.
*/
ap_state->command_roll_trim = COMMAND_ROLL_TRIM;
ap_state->command_pitch_trim = COMMAND_PITCH_TRIM;
ap_state->command_yaw_trim = COMMAND_YAW_TRIM;
}
void init_ap(void)
{
#ifndef SINGLE_MCU /** init done in main_fbw in single MCU */
mcu_init();
#endif /* SINGLE_MCU */
/** - start interrupt task */
mcu_int_enable();
#if defined(PPRZ_TRIG_INT_COMPR_FLASH)
pprz_trig_int_init();
#endif
/****** initialize and reset state interface ********/
stateInit();
/************* Sensors initialization ***************/
#if USE_AHRS
ahrs_init();
#endif
#if USE_BARO_BOARD
baro_init();
#endif
/************* Links initialization ***************/
#if defined MCU_SPI_LINK || defined MCU_UART_LINK || defined MCU_CAN_LINK
link_mcu_init();
#endif
/************ Internal status ***************/
autopilot_init();
modules_init();
// call autopilot implementation init after guidance modules init
// it will set startup mode
#if USE_GENERATED_AUTOPILOT
autopilot_generated_init();
#else
autopilot_static_init();
#endif
settings_init();
/**** start timers for periodic functions *****/
sensors_tid = sys_time_register_timer(1. / PERIODIC_FREQUENCY, NULL);
#if !USE_GENERATED_AUTOPILOT
navigation_tid = sys_time_register_timer(1. / NAVIGATION_FREQUENCY, NULL);
#endif
attitude_tid = sys_time_register_timer(1. / CONTROL_FREQUENCY, NULL);
modules_tid = sys_time_register_timer(1. / MODULES_FREQUENCY, NULL);
telemetry_tid = sys_time_register_timer(1. / TELEMETRY_FREQUENCY, NULL);
monitor_tid = sys_time_register_timer(1.0, NULL);
#if USE_BARO_BOARD
baro_tid = sys_time_register_timer(1. / BARO_PERIODIC_FREQUENCY, NULL);
#endif
#if DOWNLINK
downlink_init();
#endif
/* set initial trim values.
* these are passed to fbw via inter_mcu.
*/
PPRZ_MUTEX_LOCK(ap_state_mtx);
ap_state->command_roll_trim = COMMAND_ROLL_TRIM;
ap_state->command_pitch_trim = COMMAND_PITCH_TRIM;
ap_state->command_yaw_trim = COMMAND_YAW_TRIM;
PPRZ_MUTEX_UNLOCK(ap_state_mtx);
#if USE_IMU
// send body_to_imu from here for now
AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu));
#endif
}
ui_state_t UserInterface::getInput(void)
{
uint32_t switch_depressed = 0;
noInterrupts();
if (getState() != _last_ui_state)
{
stateInit();
_last_ui_state = _ui_state;
_sleep = millis();
}
else if (s_switch_state != SWITCH_STATE__NONE)
{
_switch_state = s_switch_state;
if (s_switch_state != SWITCH_STATE__DEPRESSED)
s_switch_state = SWITCH_STATE__NONE;
switch_depressed = millis() - s_switch_depressed;
s_encoder_turn = 0;
}
else if (s_encoder_turn != 0)
{
_encoder_turn = s_encoder_turn;
s_encoder_turn = 0;
s_switch_state = SWITCH_STATE__NONE;
}
else if (s_brightness != _brightness)
{
_brightness = s_brightness;
}
interrupts();
if (_switch_state == SWITCH_STATE__DEPRESSED)
{
if (switch_depressed >= RESET_TIME)
{
static int display_blink = 0;
static uint32_t blink_last = 0;
timerStop();
if ((switch_depressed - blink_last) > 300)
{
blink_last = switch_depressed;
display_blink ^= 1;
if (display_blink)
Display::dashes();
else
Display::blank();
}
}
return UI_STATE__PASS;
}
if (sleep())
return UI_STATE__PASS;
return _ui_state;
}
STATIC_INLINE void main_init(void)
{
mcu_init();
#if defined(PPRZ_TRIG_INT_COMPR_FLASH)
pprz_trig_int_init();
#endif
electrical_init();
stateInit();
#ifndef INTER_MCU_AP
actuators_init();
#else
intermcu_init();
#endif
#if USE_MOTOR_MIXING
motor_mixing_init();
#endif
#ifndef INTER_MCU_AP
radio_control_init();
#endif
#if USE_BARO_BOARD
baro_init();
#endif
#if USE_AHRS_ALIGNER
ahrs_aligner_init();
#endif
#if USE_AHRS
ahrs_init();
#endif
autopilot_init();
modules_init();
/* temporary hack:
* Since INS is now a module, LTP_DEF is not yet initialized when autopilot_init is called
* This led to the problem that global waypoints were not "localized",
* so as a stop-gap measure we localize them all (again) here..
*/
waypoints_localize_all();
settings_init();
mcu_int_enable();
#if DOWNLINK
downlink_init();
#endif
#ifdef INTER_MCU_AP
intermcu_init();
#endif
// register the timers for the periodic functions
main_periodic_tid = sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL);
#if PERIODIC_FREQUENCY != MODULES_FREQUENCY
modules_tid = sys_time_register_timer(1. / MODULES_FREQUENCY, NULL);
#endif
radio_control_tid = sys_time_register_timer((1. / 60.), NULL);
failsafe_tid = sys_time_register_timer(0.05, NULL);
electrical_tid = sys_time_register_timer(0.1, NULL);
telemetry_tid = sys_time_register_timer((1. / TELEMETRY_FREQUENCY), NULL);
#if USE_BARO_BOARD
baro_tid = sys_time_register_timer(1. / BARO_PERIODIC_FREQUENCY, NULL);
#endif
#if USE_IMU
// send body_to_imu from here for now
AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu));
#endif
// Do a failsafe check first
failsafe_check();
}
