void execute_action(uint8_t action)
{
switch (action)
{
case MAV_ACTION_LAUNCH:
if (global_data.state.mav_mode > (uint8_t)MAV_MODE_LOCKED)
{
global_data.state.status = (uint8_t)MAV_STATE_ACTIVE;
}
break;
case MAV_ACTION_MOTORS_START:
if (global_data.state.mav_mode > (uint8_t)MAV_MODE_LOCKED)
{
global_data.state.status = (uint8_t)MAV_STATE_ACTIVE;
}
break;
case MAV_ACTION_MOTORS_STOP:
global_data.state.status = (uint8_t)MAV_STATE_STANDBY;
break;
case MAV_ACTION_EMCY_KILL:
global_data.state.status = (uint8_t)MAV_STATE_EMERGENCY;
break;
case MAV_ACTION_STORAGE_READ:
param_read_all();
debug_message_buffer("Started reading params from eeprom");
break;
case MAV_ACTION_STORAGE_WRITE:
debug_message_buffer("Started writing params to eeprom");
param_write_all();
break;
case MAV_ACTION_CALIBRATE_GYRO:
start_gyro_calibration();
break;
case MAV_ACTION_CALIBRATE_RC:
start_rc_calibration();
break;
case MAV_ACTION_CALIBRATE_MAG:
start_mag_calibration();
break;
case MAV_ACTION_CALIBRATE_PRESSURE:
start_pressure_calibration();
break;
case MAV_ACTION_SET_ORIGIN:
// If not flying
if (!sys_state_is_flying())
{
gps_set_local_origin();
altitude_set_local_origin();
}
break;
default:
// Should never be reached, ignore unknown commands
debug_message_buffer_sprintf("Rejected unknown action Number: %u", action);
break;
}
}
void handle_eeprom_write_request(void)
{
//GET UPDATES FROM PARAMS
//testing eeprom params
if (global_data.state.status == MAV_STATE_STANDBY)
{
switch ((uint8_t) global_data.param[PARAM_IMU_RESET])
{
case 2:
debug_message_buffer("start writing params to eeprom");
param_write_all();
global_data.param[PARAM_IMU_RESET] = 0;
break;
case 3:
param_read_all();
debug_message_buffer("start reading params from eeprom");
global_data.param[PARAM_IMU_RESET] = 0;
break;
case 4:
global_data.param[PARAM_IMU_RESET] = 0;
debug_message_buffer("params reset to defaults");
global_data_reset_param_defaults();
break;
case 5:
global_data.param[PARAM_IMU_RESET] = 0;
if (param_size_check())
{
debug_message_buffer("check true");
}
else
{
debug_message_buffer("check false");
}
break;
case 6:
global_data.param[PARAM_IMU_RESET] = 0;
eeprom_check_start();
break;
default:
break;
}
}
}
/**
* @brief Initialize the whole system
*
* All functions that need to be called before the first mainloop iteration
* should be placed here.
*/
void main_init_generic(void)
{
// Reset to safe values
global_data_reset();
// Load default eeprom parameters as fallback
global_data_reset_param_defaults();
// LOWLEVEL INIT, ONLY VERY BASIC SYSTEM FUNCTIONS
hw_init();
enableIRQ();
led_init();
led_on(LED_GREEN);
buzzer_init();
sys_time_init();
sys_time_periodic_init();
sys_time_clock_init();
ppm_init();
pwm_init();
// Lowlevel periphel support init
adc_init();
// FIXME SDCARD
// MMC_IO_Init();
spi_init();
i2c_init();
// Sensor init
sensors_init();
debug_message_buffer("Sensor initialized");
// Shutter init
shutter_init();
shutter_control(0);
// Debug output init
debug_message_init();
debug_message_buffer("Text message buffer initialized");
// MEDIUM LEVEL INIT, INITIALIZE I2C, EEPROM, WAIT FOR MOTOR CONTROLLERS
// Try to reach the EEPROM
eeprom_check_start();
// WAIT FOR 2 SECONDS FOR THE USER TO NOT TOUCH THE UNIT
while (sys_time_clock_get_time_usec() < 2000000)
{
}
// Do the auto-gyro calibration for 1 second
// Get current temperature
led_on(LED_RED);
gyro_init();
// uint8_t timeout = 3;
// // Check for SD card
// while (sys_time_clock_get_time_usec() < 2000000)
// {
// while (GetDriveInformation() != F_OK && timeout--)
// {
// debug_message_buffer("MMC/SD-Card not found ! retrying..");
// }
// }
//
// if (GetDriveInformation() == F_OK)
// {
// debug_message_buffer("MMC/SD-Card SUCCESS: FOUND");
// }
// else
// {
// debug_message_buffer("MMC/SD-Card FAILURE: NOT FOUND");
// }
//FIXME redo init because of SD driver decreasing speed
//spi_init();
led_off(LED_RED);
// Stop trying to reach the EEPROM - if it has not been found by now, assume
// there is no EEPROM mounted
if (eeprom_check_ok())
{
param_read_all();
debug_message_buffer("EEPROM detected - reading parameters from EEPROM");
for (int i = 0; i < ONBOARD_PARAM_COUNT * 2 + 20; i++)
{
param_handler();
//sleep 1 ms
sys_time_wait(1000);
}
}
else
{
debug_message_buffer("NO EEPROM - reading onboard parameters from FLASH");
}
// Set mavlink system
mavlink_system.compid = MAV_COMP_ID_IMU;
mavlink_system.sysid = global_data.param[PARAM_SYSTEM_ID];
//Magnet sensor
hmc5843_init();
acc_init();
// Comm parameter init
mavlink_system.sysid = global_data.param[PARAM_SYSTEM_ID]; // System ID, 1-255
mavlink_system.compid = global_data.param[PARAM_COMPONENT_ID]; // Component/Subsystem ID, 1-255
// Comm init has to be
// AFTER PARAM INIT
comm_init(MAVLINK_COMM_0);
comm_init(MAVLINK_COMM_1);
// UART initialized, now initialize COMM peripherals
communication_init();
gps_init();
us_run_init();
servos_init();
//position_kalman3_init();
// Calibration starts (this can take a few seconds)
// led_on(LED_GREEN);
// led_on(LED_RED);
// Read out first time battery
global_data.battery_voltage = battery_get_value();
global_data.state.mav_mode = MAV_MODE_PREFLIGHT;
global_data.state.status = MAV_STATE_CALIBRATING;
send_system_state();
float_vect3 init_state_accel;
init_state_accel.x = 0.0f;
init_state_accel.y = 0.0f;
init_state_accel.z = -1000.0f;
float_vect3 init_state_magnet;
init_state_magnet.x = 1.0f;
init_state_magnet.y = 0.0f;
init_state_magnet.z = 0.0f;
//auto_calibration();
attitude_observer_init(init_state_accel, init_state_magnet);
debug_message_buffer("Attitude Filter initialized");
led_on(LED_RED);
send_system_state();
debug_message_buffer("System is initialized");
// Calibration stopped
led_off(LED_RED);
global_data.state.mav_mode = MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
global_data.state.status = MAV_STATE_STANDBY;
send_system_state();
debug_message_buffer("Checking if remote control is switched on:");
// Initialize remote control status
remote_control();
remote_control();
if (radio_control_status() == RADIO_CONTROL_ON && global_data.state.remote_ok)
{
global_data.state.mav_mode = MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_TEST_ENABLED;
debug_message_buffer("RESULT: remote control switched ON");
debug_message_buffer("Now in MAV_MODE_TEST2 position hold tobi_laurens");
led_on(LED_GREEN);
}
else
{
global_data.state.mav_mode = MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
debug_message_buffer("RESULT: remote control switched OFF");
led_off(LED_GREEN);
}
}
