void main_loop_quadrotor(void)
{
/**
* @brief Initialize the whole system
*
* All functions that need to be called before the first mainloop iteration
* should be placed here.
*/
main_init_generic();
control_quadrotor_position_init();
control_quadrotor_attitude_init();
attitude_tobi_laurens_init();
// FIXME XXX Make proper mode switching
// outdoor_position_kalman_init();
//vision_position_kalman_init();
// Default filters, allow Vision, Vicon and optical flow inputs
vicon_position_kalman_init();
optflow_speed_kalman_init();
/**
* @brief This is the main loop
*
* It will be executed at maximum MCU speed (60 Mhz)
*/
// Executiontime debugging
time_debug.x = 0;
time_debug.y = 0;
time_debug.z = 0;
last_mainloop_idle = sys_time_clock_get_time_usec();
debug_message_buffer("Starting main loop");
led_off(LED_GREEN);
led_off(LED_RED);
while (1)
{
// Time Measurement
uint64_t loop_start_time = sys_time_clock_set_loop_start_time(); // loop_start_time should not be used anymore
///////////////////////////////////////////////////////////////////////////
/// CRITICAL 200 Hz functions
///////////////////////////////////////////////////////////////////////////
if (us_run_every(5000, COUNTER2, loop_start_time))
{
// Kalman Attitude filter, used on all systems
gyro_read();
sensors_read_acc();
sensors_pressure_bmp085_read_out();
// Read out magnetometer at its default 50 Hz rate
static uint8_t mag_count = 0;
if (mag_count == 3)
{
sensors_read_mag();
//attitude_observer_correct_magnet(global_data.magnet_corrected);
mag_count = 0;
}else if(mag_count==1){
hmc5843_start_read();
mag_count++;
}
else
{
mag_count++;
}
// Correction step of observer filter
attitude_tobi_laurens();
if (global_data.state.position_estimation_mode == POSITION_ESTIMATION_MODE_VICON_ONLY ||
global_data.state.position_estimation_mode == POSITION_ESTIMATION_MODE_VISION_VICON_BACKUP)
{
vicon_position_kalman();
}
else if (global_data.state.position_estimation_mode == POSITION_ESTIMATION_MODE_GPS_ONLY)
{
outdoor_position_kalman();
}
control_quadrotor_attitude();
//debug counting number of executions
count++;
}
///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////
/// Camera Shutter - This takes 50 usecs!!!
///////////////////////////////////////////////////////////////////////////
// Set camera shutter with 2.5ms resolution
else if (us_run_every(5000, COUNTER1, loop_start_time)) //was 2500 !!!
{
camera_shutter_handling(loop_start_time);
// Measure time for debugging
time_debug.x = max(time_debug.x, sys_time_clock_get_time_usec()
- loop_start_time);
}
///////////////////////////////////////////////////////////////////////////
/// CRITICAL FAST 50 Hz functions
///////////////////////////////////////////////////////////////////////////
else if (us_run_every(20000, COUNTER3, loop_start_time))
{
// Read infrared sensor
//adc_read();
// Control the quadrotor position
control_quadrotor_position();
// Read remote control
remote_control();
control_camera_angle();
// //float_vect3 opt;
// static float_vect3 opt_int;
// uint8_t valid = optical_flow_get_dxy(80, &global_data.optflow.x, &global_data.optflow.y, &global_data.optflow.z);
// if (valid)
// {
// opt_int.x += global_data.optflow.x;
// opt_int.y += global_data.optflow.y;
//
// }
//
// uint8_t supersampling = 10;
// for (int i = 0; i < supersampling; ++i)
// {
// global_data.sonar_distance += sonar_distance_get(ADC_5_CHANNEL);
// }
//
// global_data.sonar_distance /= supersampling;
//
// opt_int.z = valid;
// static unsigned int i = 0;
// if (i == 10)
// {
// mavlink_msg_optical_flow_send(global_data.param[PARAM_SEND_DEBUGCHAN], sys_time_clock_get_unix_loop_start_time(), 0, global_data.optflow.x, global_data.optflow.y, global_data.optflow.z, global_data.sonar_distance_filtered);
//
// i = 0;
// }
// i++;
//optical_flow_debug_vect_send();
//debug_vect("opt_int", opt_int);
// optical_flow_start_read(80);
if (global_data.state.position_estimation_mode
== POSITION_ESTIMATION_MODE_OPTICAL_FLOW_ULTRASONIC_INTEGRATING
|| global_data.state.position_estimation_mode
== POSITION_ESTIMATION_MODE_OPTICAL_FLOW_ULTRASONIC_NON_INTEGRATING
|| global_data.state.position_estimation_mode
== POSITION_ESTIMATION_MODE_OPTICAL_FLOW_ULTRASONIC_ADD_VICON_AS_OFFSET
|| global_data.state.position_estimation_mode
== POSITION_ESTIMATION_MODE_OPTICAL_FLOW_ULTRASONIC_ADD_VISION_AS_OFFSET
|| global_data.state.position_estimation_mode
== POSITION_ESTIMATION_MODE_OPTICAL_FLOW_ULTRASONIC_ODOMETRY_ADD_VISION_AS_OFFSET
|| global_data.state.position_estimation_mode
== POSITION_ESTIMATION_MODE_OPTICAL_FLOW_ULTRASONIC_VICON
|| global_data.state.position_estimation_mode
== POSITION_ESTIMATION_MODE_GPS_OPTICAL_FLOW
|| global_data.state.position_estimation_mode
== POSITION_ESTIMATION_MODE_OPTICAL_FLOW_ULTRASONIC_GLOBAL_VISION
|| global_data.state.position_estimation_mode
== POSITION_ESTIMATION_MODE_OPTICAL_FLOW_ULTRASONIC_VISUAL_ODOMETRY_GLOBAL_VISION)
{
optflow_speed_kalman();
}
// Send the raw sensor/ADC values
communication_send_raw_data(loop_start_time);
float_vect3 yy; yy.x = global_data.yaw_lowpass; yy.y = 0.f; yy.z = 0.f;
debug_vect("yaw_low", yy);
}
///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////
/// CRITICAL FAST 20 Hz functions
///////////////////////////////////////////////////////////////////////////
else if (us_run_every(50000, COUNTER4, loop_start_time))
{
//*** this happens in handle_controller_timeouts already!!!!! ***
// //update global_data.state
// if (global_data.param[PARAM_VICON_MODE] == 1)
// {
// //VICON_MODE 1 only accepts vicon position
// global_data.state.position_fix = global_data.state.vicon_ok;
// }
// else
// {
// //VICON_MODEs 0, 2, 3 accepts vision additionally, so check vision
// global_data.state.position_fix = global_data.state.vision_ok;
// }
update_system_statemachine(loop_start_time);
update_controller_setpoints();
mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(
global_data.param[PARAM_SEND_DEBUGCHAN],
sys_time_clock_get_loop_start_time_boot_ms(),
global_data.attitude_setpoint.x,
global_data.attitude_setpoint.y,
global_data.position_yaw_control_output,
global_data.thrust_control_output);
//STARTING AND LANDING
quadrotor_start_land_handler(loop_start_time);
}
///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////
/// NON-CRITICAL SLOW 100 Hz functions
///////////////////////////////////////////////////////////////////////////
else if (us_run_every(5000, COUNTER6, loop_start_time))
{
if (global_data.param[PARAM_SEND_SLOT_DEBUG_6])
{
debug_vect("att_ctrl_o", global_data.attitude_control_output);
}
}
///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////
/// UNCRITICAL SLOW 5 Hz functions
///////////////////////////////////////////////////////////////////////////
else if (us_run_every(200000, COUNTER8, loop_start_time))
{
// The onboard controllers go into failsafe mode once
// position data is missing
handle_controller_timeouts(loop_start_time);
// Send buffered data such as debug text messages
// Empty one message out of the buffer
debug_message_send_one();
// Toggle status led
led_toggle(LED_RED);
// Toggle active mode led
if (global_data.state.mav_mode & MAV_MODE_FLAG_SAFETY_ARMED)
{
led_on(LED_GREEN);
}
else
{
led_off(LED_GREEN);
}
handle_eeprom_write_request();
handle_reset_request();
update_controller_parameters();
communication_send_controller_feedback();
communication_send_remote_control();
// Pressure sensor driver works, but not tested regarding stability
// sensors_pressure_bmp085_read_out();
if (global_data.param[PARAM_POSITION_YAW_TRACKING] == 1)
{
mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 0,
90, global_data.param[PARAM_POSITION_SETPOINT_YAW]);
mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 0,
91, global_data.yaw_pos_setpoint);
}
}
///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////
/// NON-CRITICAL SLOW 1 Hz functions
///////////////////////////////////////////////////////////////////////////
else if (us_run_every(1000000, COUNTER9, loop_start_time))
{
// Send system state, mode, battery voltage, etc.
send_system_state();
// Send position setpoint offset
//debug_vect("pos offs", global_data.position_setpoint_offset);
// Send current onboard time
mavlink_msg_system_time_send(MAVLINK_COMM_1, sys_time_clock_get_unix_loop_start_time(),sys_time_clock_get_loop_start_time_boot_ms());
mavlink_msg_system_time_send(MAVLINK_COMM_0, sys_time_clock_get_unix_loop_start_time(),sys_time_clock_get_loop_start_time_boot_ms());
//update state from received parameters
sync_state_parameters();
//debug number of execution
count = 0;
if (global_data.param[PARAM_GPS_MODE] >= 10)
{
//Send GPS information
float_vect3 gps;
gps.x = gps_utm_north / 100.0f;//m
gps.y = gps_utm_east / 100.0f;//m
gps.z = gps_utm_zone;// gps_week;
debug_vect("GPS", gps);
}
else if (global_data.param[PARAM_GPS_MODE] == 9
|| global_data.param[PARAM_GPS_MODE] == 8)
{
if (global_data.param[PARAM_GPS_MODE] == 8)
{
gps_set_local_origin();
// gps_local_home_init = false;
}
if (gps_lat == 0)
{
debug_message_buffer("GPS Signal Lost");
}
else
{
float_vect3 gps_local, gps_local_velocity;
gps_get_local_position(&gps_local);
debug_vect("GPS local", gps_local);
gps_get_local_velocity(&gps_local_velocity);
debug_vect("GPS loc velocity", gps_local_velocity);
}
}
if (global_data.state.gps_mode)
{
gps_send_local_origin();
}
beep_on_low_voltage();
}
///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////
/// NON-CRITICAL SLOW 20 Hz functions
///////////////////////////////////////////////////////////////////////////
else if (us_run_every(50000, COUNTER7, loop_start_time))
{
//led_toggle(LED_YELLOW);
if (global_data.param[PARAM_GPS_MODE] >= 10)
{
//get thru all gps messages
debug_message_send_one();
}
communication_send_attitude_position(loop_start_time);
// Send parameter
communication_queued_send();
// //infrared distance
// float_vect3 infra;
// infra.x = global_data.ground_distance;
// infra.y = global_data.ground_distance_unfiltered;
// infra.z = global_data.state.ground_distance_ok;
// debug_vect("infrared", infra);
}
///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////
/// NON-CRITICAL SLOW 200 Hz functions //
///////////////////////////////////////////////////////////////////////////
else if (us_run_every(5000, COUNTER5, loop_start_time))
{
if (global_data.state.status == MAV_STATE_STANDBY)
{
//Check if parameters should be written or read
param_handler();
}
}
///////////////////////////////////////////////////////////////////////////
else
{
// All Tasks are fine and we have no starvation
last_mainloop_idle = loop_start_time;
}
// Read out comm at max rate - takes only a few microseconds in worst case
communication_receive();
// MCU load measurement
uint64_t loop_stop_time = sys_time_clock_get_time_usec();
global_data.cpu_usage = measure_avg_cpu_load(loop_start_time,
loop_stop_time, min_mainloop_time);
global_data.cpu_peak = measure_peak_cpu_load(loop_start_time,
loop_stop_time, min_mainloop_time);
time_debug.y = max(time_debug.y, global_data.cpu_usage);
time_debug.z = max(time_debug.z, global_data.cpu_peak);
if (loop_start_time - last_mainloop_idle >= 20000)
{
debug_message_buffer(
"CRITICAL WARNING! CPU LOAD TO HIGH. STARVATION!");
last_mainloop_idle = loop_start_time;//reset to prevent multiple messages
}
if (global_data.cpu_usage > 800)
{
// CPU load higher than 80%
debug_message_buffer("CRITICAL WARNING! CPU LOAD HIGHER THAN 80%");
}
} // End while(1)
}
/**
* @brief This is the main loop
*
* It will be executed at maximum MCU speed (60 Mhz)
*/
void main_loop_fixed_wing(void)
{
last_mainloop_idle = sys_time_clock_get_time_usec();
debug_message_buffer("Starting main loop");
while (1)
{
// Time Measurement
uint64_t loop_start_time = sys_time_clock_get_time_usec();
///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////
/// Camera Shutter
///////////////////////////////////////////////////////////////////////////
// Set camera shutter with 2.5ms resolution
if (us_run_every(2500, COUNTER1, loop_start_time))
{
camera_shutter_handling(loop_start_time);
}
if (global_data.state.mav_mode == MAV_MODE_RC_TRAINING)
{
///////////////////////////////////////////////////////////////////////////
/// RC INTERFACE HACK AT 50 Hz
///////////////////////////////////////////////////////////////////////////
if (us_run_every(20000, COUNTER8, loop_start_time))
{
// Write start byte
uart1_transmit(0xFF);
// Write channels 1-6
for (int i = 1; i < 7; i++)
{
uart1_transmit((radio_control_get_channel(1)+1)*127);
}
}
led_toggle(LED_GREEN);
led_toggle(LED_RED);
// Do not execute any of the functions below
continue;
}
///////////////////////////////////////////////////////////////////////////
/// CRITICAL 200 Hz functions
///////////////////////////////////////////////////////////////////////////
if (us_run_every(5000, COUNTER2, loop_start_time))
{
// Kalman Attitude filter, used on all systems
gyro_read();
sensors_read_acc();
// Read out magnetometer at its default 50 Hz rate
static uint8_t mag_count = 0;
if (mag_count == 3)
{
sensors_read_mag();
attitude_observer_correct_magnet(global_data.magnet_corrected);
mag_count = 0;
}
else
{
mag_count++;
}
// Correction step of observer filter
attitude_observer_correct_accel(global_data.accel_raw);
// Write in roll and pitch
static float_vect3 att; //if not static we have spikes in roll and pitch on bravo !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
attitude_observer_get_angles(&att);
global_data.attitude.x = att.x;
global_data.attitude.y = att.y;
if (global_data.param[PARAM_ATT_KAL_IYAW])
{
global_data.attitude.z += 0.005 * global_data.gyros_si.z;
}
else
{
global_data.attitude.z = att.z;
}
// Prediction step of observer
attitude_observer_predict(global_data.gyros_si);
control_fixed_wing_attitude();
}
///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////
/// CRITICAL FAST 50 Hz functions
///////////////////////////////////////////////////////////////////////////
else if (us_run_every(20000, COUNTER3, loop_start_time))
{
// Read Analog-to-Digital converter
adc_read();
// Read remote control
remote_control();
}
///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////
/// NON-CRITICAL SLOW 100 Hz functions
///////////////////////////////////////////////////////////////////////////
else if (us_run_every(10000, COUNTER6, loop_start_time))
{
// Send the raw sensor/ADC values
communication_send_raw_data(loop_start_time);
}
///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////
/// UNCRITICAL SLOW 5 Hz functions
///////////////////////////////////////////////////////////////////////////
else if (us_run_every(200000, COUNTER8, loop_start_time))
{
// The onboard controllers go into failsafe mode once
// position data is missing
handle_controller_timeouts(loop_start_time);
// Send buffered data such as debug text messages
communication_queued_send();
// Empty one message out of the buffer
debug_message_send_one();
// Toggle status led
//led_toggle(LED_YELLOW);
led_toggle(LED_RED); // just for green LED on alpha at the moment
// Toggle active mode led
if (global_data.state.mav_mode == MAV_MODE_MANUAL || global_data.state.mav_mode
== MAV_MODE_GUIDED || global_data.state.mav_mode == MAV_MODE_AUTO)
{
led_on(LED_GREEN);
}
else
{
led_off(LED_GREEN);
}
handle_eeprom_write_request();
handle_reset_request();
communication_send_controller_feedback();
communication_send_remote_control();
// Pressure sensor driver works, but not tested regarding stability
sensors_pressure_bmp085_read_out();
if (global_data.param[PARAM_POSITION_YAW_TRACKING] == 1)
{
mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN],
90, global_data.param[PARAM_POSITION_SETPOINT_YAW]);
mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN],
91, global_data.yaw_pos_setpoint);
}
}
///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////
/// NON-CRITICAL SLOW 1 Hz functions
///////////////////////////////////////////////////////////////////////////
else if (us_run_every(1000000, COUNTER9, loop_start_time))
{
// Send system state, mode, battery voltage, etc.
send_system_state();
if (global_data.param[PARAM_GPS_MODE] >= 10)
{
//Send GPS information
float_vect3 gps;
gps.x = gps_utm_north / 100.0f;//m
gps.y = gps_utm_east / 100.0f;//m
gps.z = gps_utm_zone;// gps_week;
debug_vect("GPS", gps);
}
beep_on_low_voltage();
}
///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////
/// NON-CRITICAL SLOW 20 Hz functions
///////////////////////////////////////////////////////////////////////////
else if (us_run_every(50000, COUNTER7, loop_start_time))
{
led_toggle(LED_YELLOW);
if (global_data.param[PARAM_GPS_MODE] >= 10)
{
//get thru all gps messages
debug_message_send_one();
}
communication_send_attitude_position(loop_start_time);
}
///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////
/// NON-CRITICAL SLOW 200 Hz functions //
///////////////////////////////////////////////////////////////////////////
else if (us_run_every(5000, COUNTER5, loop_start_time))
{
if (global_data.state.status == MAV_STATE_STANDBY)
{
//Check if parameters should be written or read
param_handler();
}
}
///////////////////////////////////////////////////////////////////////////
else {
// All Tasks are fine and we have no starvation
last_mainloop_idle = loop_start_time;
}
// Read out comm at max rate - takes only a few microseconds in worst case
communication_receive();
// MCU load measurement
uint64_t loop_stop_time = sys_time_clock_get_time_usec();
global_data.cpu_usage = measure_avg_cpu_load(loop_start_time, loop_stop_time, min_mainloop_time);
global_data.cpu_peak = measure_peak_cpu_load(loop_start_time, loop_stop_time, min_mainloop_time);
if (loop_start_time - last_mainloop_idle >= 5000)
{
debug_message_buffer(
"CRITICAL WARNING! CPU LOAD TO HIGH. STARVATION!");
last_mainloop_idle = loop_start_time;//reset to prevent multiple messages
}
if (global_data.cpu_usage > 800)
{
// CPU load higher than 80%
debug_message_buffer("CRITICAL WARNING! CPU LOAD HIGHER THAN 80%");
}
} // End while(1)
}
/**
* @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);
}
}