DWORD get_fattime(void)
{
uint32_t time = 0;
double time_in_seconds;
time_in_seconds = time_keeper_get_time();
uint8_t seconds;
uint8_t minutes;
uint8_t hours;
minutes = (uint8_t) floor(((float)time_in_seconds) / 60.0f);
seconds = (uint8_t) ((((uint32_t)time_in_seconds) % 60)/2);
hours = (uint8_t) floor(((float)minutes) / 60.0f);
minutes = (uint8_t) (minutes % 60);
uint8_t day = 26;
uint8_t month = 8;
uint8_t year = 2014-1980;
time += (seconds & 0b00000000000000000000000000011111);
time += ((minutes << 5) & 0b00000000000000000000011111100000);
time += ((hours << 11) & 0b00000000000000001111100000000000);
time += ((day << 16) & 0b00000000000111110000000000000000);
time += ((month << 21) & 0b00000001111000000000000000000000);
time += ((year << 25) & 0b11111110000000000000000000000000);
return time;
}
bool state_init(state_t *state, state_t* state_config, const analog_monitor_t* analog_monitor)
{
bool init_success = true;
// Init dependencies
state->analog_monitor = analog_monitor;
// Init parameters
state->autopilot_type = state_config->autopilot_type;
state->autopilot_name = state_config->autopilot_name;
state->mav_state = state_config->mav_state;
state->mav_mode = state_config->mav_mode;
state->source_mode = state_config->source_mode;
state->mav_mode_custom = CUSTOM_BASE_MODE;
state->simulation_mode = state_config->simulation_mode;
init_success &= battery_init(&state->battery,state_config->battery.type,state_config->battery.low_level_limit);
if (state->simulation_mode == HIL_ON)
{
// state->mav_mode |= MAV_MODE_FLAG_HIL_ENABLED;
state->mav_mode.HIL = HIL_ON;
}
else
{
// state->mav_mode |= !MAV_MODE_FLAG_HIL_ENABLED;
state->mav_mode.HIL = HIL_OFF;
}
state->fence_1_xy = state_config->fence_1_xy;
state->fence_1_z = state_config->fence_1_z;
state->fence_2_xy = state_config->fence_2_xy;
state->fence_2_z = state_config->fence_2_z;
state->out_of_fence_1 = false;
state->out_of_fence_2 = false;
state->nav_plan_active = false;
state->in_the_air = false;
state->reset_position = false;
state->last_heartbeat_msg = time_keeper_get_time();
state->max_lost_connection = state_config->max_lost_connection;
state->connection_lost = false;
state->first_connection_set = false;
state->msg_count = 0;
state->remote_active = state_config->remote_active;
print_util_dbg_print("[STATE] Initialized.\r\n");
return init_success;
}
void state_connection_status(state_t* state)
{
if ( ((time_keeper_get_time()-state->last_heartbeat_msg) > state->max_lost_connection)&&(state->first_connection_set) )
{
state->connection_lost = true;
}
else
{
state->connection_lost = false;
}
}
