void *run(void *ptr_shared_data) {
char cTmp[MAX_LINE];
int l_change = 0;
g_ptr_shared_data = (struct shared_data *) ptr_shared_data;
// Initialize datarefs
g_comFreq = g_ptr_shared_data->comFreq;
last_mainloop_idle = sys_time_clock_get_time_usec();
// while stop == 0 calculate position.
while (g_ptr_shared_data->stop == 0) {
long loop_start_time = sys_time_clock_get_time_usec();
///////////////////////////////////////////////////////////////////////////
/// CRITICAL FAST 20 Hz functions
///////////////////////////////////////////////////////////////////////////
if (us_run_every(50000, COUNTER3, loop_start_time)) {
// read usb board values
l_change = readDevice(&g_usb_data);
// Update xplane
updateHost();
// Update board
updateBoard();
}
///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////
/// NON-CRITICAL SLOW 10 Hz functions
///////////////////////////////////////////////////////////////////////////
else if (us_run_every(100000, COUNTER6, loop_start_time)) {
//update_screen();
}
///////////////////////////////////////////////////////////////////////////
if (loop_start_time - last_mainloop_idle >= 100000) {
writeLog("CRITICAL WARNING! CPU LOAD TOO HIGH.\n");
last_mainloop_idle = loop_start_time;//reset to prevent multiple messages
} else {
//writeConsole(0, 0, "CPU LOAD OK.");
}
// wait 1 milliseconds
#if IBM
Sleep(1);
#endif
#if LIN
usleep(10);
#endif
g_counter++;
}
sprintf(cTmp, "thread closing usb device %d...\n", 0);
writeLog(cTmp);
closeDevice();
sprintf(cTmp, "thread info : stopping thread #%d, %d!\n",
g_ptr_shared_data->thread_id, g_counter);
writeLog(cTmp);
pthread_exit(NULL);
return 0;
}
void start_rc_calibration(void)
{
if (calibration_enter())
{
// Calibration routine: Read out remote control and wait for all channels.
// Read initial values.
const uint8_t chan_count = 9;
uint32_t chan_initial[chan_count];
uint32_t chan_min[chan_count];
uint32_t chan_max[chan_count];
for (int i = 0; i < chan_count; i++)
{
chan_initial[i] = ppm_get_channel(i + 1);
chan_min[i]=2000;
chan_max[i]=1000;
}
bool abort_rc_cal = 0;
uint64_t timeout = sys_time_clock_get_time_usec() + 30 * 1000000;
while (!abort_rc_cal)
{
// Now save the min and max values for each channel
for (int i = 0; i < chan_count; i++)
{
uint32_t ppm_value = (uint32_t) ppm_get_channel(i + 1);
chan_min[i] = min(chan_min[i], ppm_value);
chan_min[i] = min(chan_min[i], ppm_value);
}
// until motors stop conditions are met.
if ((ppm_get_channel(global_data.param[PARAM_PPM_THROTTLE_CHANNEL])
< PPM_LOW_TRIG) && (ppm_get_channel(
global_data.param[PARAM_PPM_YAW_CHANNEL]) > PPM_HIGH_TRIG))
{
abort_rc_cal = 1;
}
if(sys_time_clock_get_time_usec()>timeout){
abort_rc_cal = 1;
debug_message_buffer("RC calibration abborted after 30 seconds");
}
}
calibration_exit();
}
}
//@{
void handle_mavlink_message(mavlink_channel_t chan,
mavlink_message_t* msg)
{
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
uint32_t len;
switch (chan)
{
case MAVLINK_COMM_0:
{
if (msg->msgid != MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE)
{
// Copy to COMM 1
len = mavlink_msg_to_send_buffer(buf, msg);
for (int i = 0; i < len; i++)
{
uart1_transmit(buf[i]);
}
}
}
break;
case MAVLINK_COMM_1:
{
if (msg->msgid != MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE && msg->msgid != MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE)
{
// Copy to COMM 0
len = mavlink_msg_to_send_buffer(buf, msg);
for (int i = 0; i < len; i++)
{
uart0_transmit(buf[i]);
}
break;
}
}
default:
break;
}
switch (msg->msgid)
{
case MAVLINK_MSG_ID_SET_MODE:
{
mavlink_set_mode_t mode;
mavlink_msg_set_mode_decode(msg, &mode);
// Check if this system should change the mode
if (mode.target == (uint8_t)global_data.param[PARAM_SYSTEM_ID])
{
sys_set_mode(mode.mode);
// Emit current mode
mavlink_msg_sys_status_send(MAVLINK_COMM_0, global_data.state.mav_mode, global_data.state.nav_mode,
global_data.state.status, global_data.cpu_usage, global_data.battery_voltage,
global_data.motor_block, communication_get_uart_drop_rate());
mavlink_msg_sys_status_send(MAVLINK_COMM_1, global_data.state.mav_mode, global_data.state.nav_mode,
global_data.state.status, global_data.cpu_usage, global_data.battery_voltage,
global_data.motor_block, communication_get_uart_drop_rate());
}
}
break;
case MAVLINK_MSG_ID_ACTION:
{
execute_action(mavlink_msg_action_get_action(msg));
//Forwart actions from Xbee to Onboard Computer and vice versa
if (chan == MAVLINK_COMM_1)
{
mavlink_send_uart(MAVLINK_COMM_0, msg);
}
else if (chan == MAVLINK_COMM_0)
{
mavlink_send_uart(MAVLINK_COMM_1, msg);
}
}
break;
case MAVLINK_MSG_ID_SYSTEM_TIME:
{
if (!sys_time_clock_get_unix_offset())
{
int64_t offset = ((int64_t) mavlink_msg_system_time_get_time_usec(
msg)) - (int64_t) sys_time_clock_get_time_usec();
sys_time_clock_set_unix_offset(offset);
debug_message_buffer("UNIX offset updated");
}
else
{
// debug_message_buffer("UNIX offset REFUSED");
}
}
break;
case MAVLINK_MSG_ID_REQUEST_DATA_STREAM:
{
mavlink_request_data_stream_t stream;
mavlink_msg_request_data_stream_decode(msg, &stream);
switch (stream.req_stream_id)
{
case 0: // UNIMPLEMENTED
break;
case 1: // RAW SENSOR DATA
global_data.param[PARAM_SEND_SLOT_RAW_IMU] = stream.start_stop;
break;
case 2: // EXTENDED SYSTEM STATUS
global_data.param[PARAM_SEND_SLOT_ATTITUDE] = stream.start_stop;
break;
case 3: // REMOTE CONTROL CHANNELS
global_data.param[PARAM_SEND_SLOT_REMOTE_CONTROL] = stream.start_stop;
break;
case 4: // RAW CONTROLLER
//global_data.param[PARAM_SEND_SLOT_DEBUG_5] = stream.start_stop;
//global_data.param[PARAM_SEND_SLOT_DEBUG_3] = stream.start_stop;
global_data.param[PARAM_SEND_SLOT_CONTROLLER_OUTPUT] = stream.start_stop;
break;
case 5: // SENSOR FUSION
//LOST IN GROUDNCONTROL
// global_data.param[PARAM_SEND_SLOT_DEBUG_5] = stream.start_stop;
break;
case 6: // POSITION
global_data.param[PARAM_SEND_SLOT_DEBUG_5] = stream.start_stop;
break;
case 7: // EXTRA1
global_data.param[PARAM_SEND_SLOT_DEBUG_2] = stream.start_stop;
break;
case 8: // EXTRA2
global_data.param[PARAM_SEND_SLOT_DEBUG_4] = stream.start_stop;
break;
case 9: // EXTRA3
global_data.param[PARAM_SEND_SLOT_DEBUG_6] = stream.start_stop;
break;
default:
// Do nothing
break;
}
}
break;
case MAVLINK_MSG_ID_PARAM_REQUEST_READ:
{
mavlink_param_request_read_t set;
mavlink_msg_param_request_read_decode(msg, &set);
// Check if this message is for this system
if ((uint8_t) set.target_system
== (uint8_t) global_data.param[PARAM_SYSTEM_ID]
&& (uint8_t) set.target_component
== (uint8_t) global_data.param[PARAM_COMPONENT_ID])
{
char* key = (char*) set.param_id;
if (set.param_id[0] == '\0')
{
// Choose parameter based on index
if (set.param_index < ONBOARD_PARAM_COUNT)
{
// Report back value
mavlink_msg_param_value_send(chan,
(int8_t*) global_data.param_name[set.param_index],
global_data.param[set.param_index], ONBOARD_PARAM_COUNT, set.param_index);
}
}
else
{
for (int i = 0; i < ONBOARD_PARAM_COUNT; i++)
{
bool match = true;
for (int j = 0; j < ONBOARD_PARAM_NAME_LENGTH; j++)
{
// Compare
if (((char) (global_data.param_name[i][j]))
!= (char) (key[j]))
{
match = false;
}
// End matching if null termination is reached
if (((char) global_data.param_name[i][j]) == '\0')
{
break;
}
}
// Check if matched
if (match)
{
// Report back value
mavlink_msg_param_value_send(chan,
(int8_t*) global_data.param_name[i],
global_data.param[i], ONBOARD_PARAM_COUNT, m_parameter_i);
}
}
}
}
}
break;
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
{
// Start sending parameters
m_parameter_i = 0;
}
break;
case MAVLINK_MSG_ID_PARAM_SET:
{
mavlink_param_set_t set;
mavlink_msg_param_set_decode(msg, &set);
// Check if this message is for this system
if ((uint8_t) set.target_system
== (uint8_t) global_data.param[PARAM_SYSTEM_ID]
&& (uint8_t) set.target_component
== (uint8_t) global_data.param[PARAM_COMPONENT_ID])
{
char* key = (char*) set.param_id;
for (int i = 0; i < ONBOARD_PARAM_COUNT; i++)
{
bool match = true;
for (int j = 0; j < ONBOARD_PARAM_NAME_LENGTH; j++)
{
// Compare
if (((char) (global_data.param_name[i][j]))
!= (char) (key[j]))
{
match = false;
}
// End matching if null termination is reached
if (((char) global_data.param_name[i][j]) == '\0')
{
break;
}
}
// Check if matched
if (match)
{
// Only write and emit changes if there is actually a difference
// AND only write if new value is NOT "not-a-number"
// AND is NOT infy
if (global_data.param[i] != set.param_value
&& !isnan(set.param_value)
&& !isinf(set.param_value))
{
global_data.param[i] = set.param_value;
// Report back new value
mavlink_msg_param_value_send(MAVLINK_COMM_0,
(int8_t*) global_data.param_name[i],
global_data.param[i], ONBOARD_PARAM_COUNT, m_parameter_i);
mavlink_msg_param_value_send(MAVLINK_COMM_1,
(int8_t*) global_data.param_name[i],
global_data.param[i], ONBOARD_PARAM_COUNT, m_parameter_i);
debug_message_buffer_sprintf("Parameter received param id=%i",i);
}
}
}
}
}
break;
case MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_SET:
{
mavlink_position_control_setpoint_set_t pos;
mavlink_msg_position_control_setpoint_set_decode(msg, &pos);
if (global_data.param[PARAM_POSITIONSETPOINT_ACCEPT] == 1)
{
// global_data.position_setpoint.x = pos.x;
// global_data.position_setpoint.y = pos.y;
// global_data.position_setpoint.z = pos.z;
debug_message_buffer("Position setpoint updated. OLD?\n");
}
else
{
debug_message_buffer(
"Position setpoint recieved but not updated. OLD?\n");
}
// Send back a message confirming the new position
mavlink_msg_position_control_setpoint_send(MAVLINK_COMM_0, pos.id,
pos.x, pos.y, pos.z, pos.yaw);
mavlink_msg_position_control_setpoint_send(MAVLINK_COMM_1, pos.id,
pos.x, pos.y, pos.z, pos.yaw);
}
break;
case MAVLINK_MSG_ID_POSITION_CONTROL_OFFSET_SET:
{
mavlink_position_control_offset_set_t set;
mavlink_msg_position_control_offset_set_decode(msg, &set);
//global_data.attitude_setpoint_pos_body_offset.z = set.yaw;
//Ball Tracking
if (global_data.param[PARAM_POSITIONSETPOINT_ACCEPT] == 1 && global_data.param[PARAM_POSITION_YAW_TRACKING]==1)
{
global_data.param[PARAM_POSITION_SETPOINT_YAW]
= global_data.attitude.z + set.yaw;
mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 92, set.yaw);
}
}
break;
case MAVLINK_MSG_ID_SET_CAM_SHUTTER:
{
// Decode the desired shutter
mavlink_set_cam_shutter_t cam;
mavlink_msg_set_cam_shutter_decode(msg, &cam);
shutter_set(cam.interval, cam.exposure);
debug_message_buffer_sprintf("set_cam_shutter. interval %i",
cam.interval);
}
break;
case MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL:
{
uint8_t enable = mavlink_msg_image_trigger_control_get_enable(msg);
shutter_control(enable);
if (enable)
{
debug_message_buffer("CAM: Enabling hardware trigger");
}
else
{
debug_message_buffer("CAM: Disabling hardware trigger");
}
}
break;
case MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE:
{
mavlink_vision_position_estimate_t pos;
mavlink_msg_vision_position_estimate_decode(msg, &pos);
vision_buffer_handle_data(&pos);
// Update validity time is done in vision buffer
}
break;
case MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE:
{
mavlink_vicon_position_estimate_t pos;
mavlink_msg_vicon_position_estimate_decode(msg, &pos);
global_data.vicon_data.x = pos.x;
global_data.vicon_data.y = pos.y;
global_data.vicon_data.z = pos.z;
global_data.state.vicon_new_data=1;
// Update validity time
global_data.vicon_last_valid = sys_time_clock_get_time_usec();
global_data.state.vicon_ok=1;
// //Set data from Vicon into vision filter
// global_data.vision_data.ang.x = pos.roll;
// global_data.vision_data.ang.y = pos.pitch;
// global_data.vision_data.ang.z = pos.yaw;
//
// global_data.vision_data.pos.x = pos.x;
// global_data.vision_data.pos.y = pos.y;
// global_data.vision_data.pos.z = pos.z;
//
// global_data.vision_data.new_data = 1;
if (!global_data.state.vision_ok)
{
//Correct YAW
global_data.attitude.z = pos.yaw;
//If yaw goes to infy (no idea why) set it to setpoint, next time will be better
if (global_data.attitude.z > 18.8495559 || global_data.attitude.z
< -18.8495559)
{
global_data.attitude.z = global_data.yaw_pos_setpoint;
debug_message_buffer(
"vicon CRITICAL FAULT yaw was bigger than 6 PI! prevented crash");
}
}
//send the vicon message to UART0 with new timestamp
mavlink_msg_vicon_position_estimate_send(MAVLINK_COMM_0, global_data.vicon_last_valid, pos.x, pos.y, pos.z, pos.roll, pos.pitch, pos.yaw);
}
break;
case MAVLINK_MSG_ID_PING:
{
mavlink_ping_t ping;
mavlink_msg_ping_decode(msg, &ping);
if (ping.target_system == 0 && ping.target_component == 0)
{
// Respond to ping
uint64_t r_timestamp = sys_time_clock_get_unix_time();
mavlink_msg_ping_send(chan, ping.seq, msg->sysid, msg->compid, r_timestamp);
}
}
break;
case MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET:
{
mavlink_local_position_setpoint_set_t sp;
mavlink_msg_local_position_setpoint_set_decode(msg, &sp);
if (sp.target_system == global_data.param[PARAM_SYSTEM_ID])
{
if (global_data.param[PARAM_POSITIONSETPOINT_ACCEPT] == 1)
{
if (sp.x >= global_data.position_setpoint_min.x && sp.y
>= global_data.position_setpoint_min.y && sp.z
>= global_data.position_setpoint_min.z && sp.x
<= global_data.position_setpoint_max.x && sp.y
<= global_data.position_setpoint_max.y && sp.z
<= global_data.position_setpoint_max.z)
{
debug_message_buffer("Setpoint accepted and set.");
global_data.param[PARAM_POSITION_SETPOINT_X] = sp.x;
global_data.param[PARAM_POSITION_SETPOINT_Y] = sp.y;
global_data.param[PARAM_POSITION_SETPOINT_Z] = sp.z;
if (global_data.param[PARAM_POSITION_YAW_TRACKING] == 0)
{
// Only update yaw if we are not tracking ball.
global_data.param[PARAM_POSITION_SETPOINT_YAW] = sp.yaw;
}
//check if we want to start or land
if (global_data.state.status == MAV_STATE_ACTIVE
|| global_data.state.status == MAV_STATE_CRITICAL)
{
if (sp.z > -0.1)
{
if (!(global_data.state.fly == FLY_GROUNDED
|| global_data.state.fly == FLY_SINKING
|| global_data.state.fly
== FLY_WAIT_LANDING
|| global_data.state.fly == FLY_LANDING
|| global_data.state.fly == FLY_RAMP_DOWN))
{
//if setpoint is lower that ground iate landing
global_data.state.fly = FLY_SINKING;
global_data.param[PARAM_POSITION_SETPOINT_Z]
= -0.2;//with lowpass
debug_message_buffer(
"Sinking for LANDING. (z-sp lower than 10cm)");
}
else if (!(global_data.state.fly == FLY_GROUNDED))
{
global_data.param[PARAM_POSITION_SETPOINT_Z]
= -0.2;//with lowpass
}
}
else if (global_data.state.fly == FLY_GROUNDED && sp.z
< -0.50)
{
//start if we were grounded and get a sp over 0.5m
global_data.state.fly = FLY_WAIT_MOTORS;
debug_message_buffer(
"STARTING wait motors. (z-sp higher than 50cm)");
//set point changed with lowpass, after 5s it will be ok.
}
}
//SINK TO 0.7m if we are critical or emergency
if (global_data.state.status == MAV_STATE_EMERGENCY
|| global_data.state.status == MAV_STATE_CRITICAL)
{
global_data.param[PARAM_POSITION_SETPOINT_Z] = -0.7;//with lowpass
}
}
else
{
debug_message_buffer("Setpoint refused. Out of range.");
}
}
else
{
debug_message_buffer("Setpoint refused. Param setpoint accept=0.");
}
}
}
break;
default:
break;
}
}
/**
* @brief This is the main loop
*
* It will be executed at maximum MCU speed (60 Mhz)
*/
void main_loop_ground_car(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();
///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////
/// 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);
// TODO READ OUT MOUSE SENSOR
}
///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////
/// 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();
// 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))
{
// 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_remote_control();
// Pressure sensor driver works, but not tested regarding stability
sensors_pressure_bmp085_read_out();
}
///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////
/// NON-CRITICAL SLOW 20 Hz functions
///////////////////////////////////////////////////////////////////////////
else if (us_run_every(50000, COUNTER7, loop_start_time))
{
led_toggle(LED_YELLOW);
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 Send one of the buffered messages
* @param pos data from vision
*/
void vision_buffer_handle_data(mavlink_vision_position_estimate_t* pos)
{
if (vision_buffer_index_write == vision_buffer_index_read)
{
//buffer empty
return;
}
vision_buffer_index_read = (vision_buffer_index_read + 1)
% VISION_BUFFER_COUNT;
//TODO: find data and process it
uint8_t for_count = 0;
uint8_t i = vision_buffer_index_read;
for (; (vision_buffer[i].time_captured < pos->usec)
&& (vision_buffer_index_write - i != 1); i = (i + 1)
% VISION_BUFFER_COUNT)
{
if (++for_count > VISION_BUFFER_COUNT)
{
debug_message_buffer("vision_buffer PREVENTED HANG");
break;
}
}
if (vision_buffer[i].time_captured == pos->usec)
{
//we found the right data
if (!isnumber(pos->x) || !isnumber(pos->y) || !isnumber(pos->z)
|| !isnumber(pos->roll) || !isnumber(pos->pitch) || !isnumber(pos->yaw)
|| pos->x == 0.0 || pos->y == 0.0 || pos->z == 0.0)
{
//reject invalid data
debug_message_buffer("vision_buffer invalid data (inf,nan,0) rejected");
}
else if (fabs(vision_buffer[i].ang.x - pos->roll)
< global_data.param[PARAM_VISION_ANG_OUTLAYER_TRESHOLD]
&& fabs(vision_buffer[i].ang.y - pos->pitch)
< global_data.param[PARAM_VISION_ANG_OUTLAYER_TRESHOLD])
{
// Update validity time
global_data.pos_last_valid = sys_time_clock_get_time_usec();
//Pack new vision_data package
global_data.vision_data.time_captured
= vision_buffer[i].time_captured;
global_data.vision_data.comp_end = sys_time_clock_get_unix_time();
//Set data from Vision directly
global_data.vision_data.ang.x = pos->roll;
global_data.vision_data.ang.y = pos->pitch;
global_data.vision_data.ang.z = pos->yaw;
global_data.vision_data.pos.x = pos->x;
global_data.vision_data.pos.y = pos->y;
global_data.vision_data.pos.z = pos->z;
// If yaw input from vision is enabled, feed vision
// directly into state estimator
global_data.vision_magnetometer_replacement.x = 200.0f*lookup_cos(pos->yaw);
global_data.vision_magnetometer_replacement.y = -200.0f*lookup_sin(pos->yaw);
global_data.vision_magnetometer_replacement.z = 0.f;
//If yaw goes to infinity (no idea why) set it to setpoint, next time will be better
if (global_data.attitude.z > 18.8495559 || global_data.attitude.z < -18.8495559)
{
global_data.attitude.z = global_data.yaw_pos_setpoint;
debug_message_buffer("vision_buffer CRITICAL FAULT yaw was bigger than 6 PI! prevented crash");
}
global_data.vision_data.new_data = 1;
//TODO correct also all buffer data needed if we are going to have overlapping vision data
}
else
{
//rejected outlayer
if (vision_buffer_reject_count++ % 16 == 0)
{
debug_message_buffer_sprintf("vision_buffer rejected outlier #%u",
vision_buffer_reject_count);
}
}
if (global_data.param[PARAM_SEND_SLOT_DEBUG_1] == 1)
{
//mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 202, global_data.attitude.z);
mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 0, 210, pos->x);
mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 0, 211, pos->y);
mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 0, 212, pos->z);
mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 0, 215, pos->yaw);
//mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 212, pos.z);
//mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 203, pos.r1);
//mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 204, pos.confidence);
}
if (for_count)
{
debug_message_buffer_sprintf(
"vision_buffer data found skipped %i data sets", for_count);
}
}
else
{
//we didn't find it
// debug_message_buffer("vision_buffer data NOT found");
if (for_count)
{
debug_message_buffer_sprintf(
"vision_buffer data NOT found skipped %i data sets",
for_count);
}
}
vision_buffer_index_read = i;//skip all images that are older;
// if (for_count)
// {
// debug_message_buffer_sprintf("vision_buffer skipped %i data sets",
// for_count);
// }
}
/**
* @brief Send one of the buffered messages
* @param pos data from vision
*/
void vision_buffer_handle_global_data(mavlink_global_vision_position_estimate_t* pos)
{
//#define PROJECT_GLOBAL_DATA_FORWARD
#ifdef PROJECT_GLOBAL_DATA_FORWARD
if (vision_buffer_index_write == vision_buffer_index_read)
{
//buffer empty
debug_message_buffer("ERROR VIS BUF: buffer empty");
return;
}
vision_buffer_index_read = (vision_buffer_index_read + 1) % VISION_BUFFER_COUNT;
//TODO: find data and process it
uint8_t for_count = 0;
uint8_t i = vision_buffer_index_read;
for (; (vision_buffer[i].time_captured < pos->usec)
&& (vision_buffer_index_write - i != 1); i = (i + 1)
% VISION_BUFFER_COUNT)
{
if (++for_count > VISION_BUFFER_COUNT) break;
}
if (vision_buffer[i].time_captured == pos->usec)
{
//we found the right data
if (!isnumber(pos->x) || !isnumber(pos->y) || !isnumber(pos->z)
|| !isnumber(pos->roll) || !isnumber(pos->pitch) || !isnumber(pos->yaw)
|| pos->x == 0.0 || pos->y == 0.0 || pos->z == 0.0)
{
//reject invalid data
debug_message_buffer("ERROR VIS BUF: invalid data (inf,nan,0) rejected");
}
else if (fabs(vision_buffer[i].ang.x - pos->roll) < global_data.param[PARAM_VISION_ANG_OUTLAYER_TRESHOLD]
&& fabs(vision_buffer[i].ang.y - pos->pitch) < global_data.param[PARAM_VISION_ANG_OUTLAYER_TRESHOLD])
{
// Update validity time
global_data.pos_last_valid = sys_time_clock_get_time_usec();
//Pack new vision_data package
global_data.vision_data_global.time_captured
= vision_buffer[i].time_captured;
global_data.vision_data_global.comp_end = sys_time_clock_get_unix_time();
// FIXME currently visodo is not allowed to run in parallel, else race condititions!
// Project position measurement
global_data.vision_data_global.pos.x = pos->x + (global_data.position.x - vision_buffer[i].pos.x);
global_data.vision_data_global.pos.y = pos->y + (global_data.position.y - vision_buffer[i].pos.y);
global_data.vision_data_global.pos.z = pos->z + (global_data.position.z - vision_buffer[i].pos.z);
// Set roll and pitch absolutely
global_data.vision_data_global.ang.x = pos->roll;
global_data.vision_data_global.ang.y = pos->pitch;
global_data.vision_data_global.ang.z = pos->yaw;
// Project yaw
//global_data.vision_data_global.ang.z = pos->yaw-vision_buffer[i].ang.z;
for (uint8_t j = (i+1) % VISION_BUFFER_COUNT; j != i; j = (j + 1) % VISION_BUFFER_COUNT)
{
vision_buffer[j].pos.x = vision_buffer[j].pos.x + (pos->x - vision_buffer[i].pos.x);
vision_buffer[j].pos.y = vision_buffer[j].pos.y + (pos->y - vision_buffer[i].pos.y);
vision_buffer[j].pos.z = vision_buffer[j].pos.z + (pos->z - vision_buffer[i].pos.z);
}
// If yaw input from vision is enabled, feed vision
// directly into state estimator
float lpYaw = pos->yaw*0.5f+global_data.attitude.z*0.5f;
global_data.vision_global_magnetometer_replacement.x = 200.0f*lookup_cos(lpYaw);
global_data.vision_global_magnetometer_replacement.y = -200.0f*lookup_sin(lpYaw);
global_data.vision_global_magnetometer_replacement.z = 0.f;
//If yaw goes to infinity (no idea why) set it to setpoint, next time will be better
if (global_data.attitude.z > 18.8495559 || global_data.attitude.z < -18.8495559)
{
global_data.attitude.z = global_data.yaw_pos_setpoint;
debug_message_buffer("vision_buffer CRITICAL FAULT yaw was bigger than 6 PI! prevented crash");
}
global_data.vision_data_global.new_data = 1;
global_data.state.global_vision_attitude_new_data = 1;
debug_message_buffer_sprintf("vision_buffer data found skipped %i data sets", for_count);
//TODO correct also all buffer data needed if we are going to have overlapping vision data
if (global_data.param[PARAM_SEND_SLOT_DEBUG_1] == 1)
{
//mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 202, global_data.attitude.z);
mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 0, 220, pos->x);
mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 0, 221, pos->y);
mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 0, 222, pos->z);
mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 0, 225, pos->yaw);
//mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 212, pos.z);
//mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 203, pos.r1);
//mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 204, pos.confidence);
}
}
else
{
//rejected outlier
//if (vision_buffer_reject_count++ % 16 == 0)
{
debug_message_buffer_sprintf("vision_buffer rejected outlier #%u",
vision_buffer_reject_count);
}
}
}
else
{
//we didn't find it
debug_message_buffer_sprintf("vision_buffer data NOT found skipped %i data sets", for_count);
}
vision_buffer_index_read = i;//skip all images that are older;
#else
global_data.vision_data_global.pos.x = pos->x;
global_data.vision_data_global.pos.y = pos->y;
global_data.vision_data_global.pos.z = pos->z;
//Set data from Vision directly
global_data.vision_data_global.ang.x = pos->roll;
global_data.vision_data_global.ang.y = pos->pitch;
global_data.vision_data_global.ang.z = pos->yaw;
global_data.vision_data_global.new_data = 1;
global_data.state.global_vision_attitude_new_data = 1;
#endif
mavlink_msg_global_vision_position_estimate_send(MAVLINK_COMM_0, sys_time_clock_get_unix_loop_start_time(), global_data.vision_data_global.pos.x, global_data.vision_data_global.pos.y, global_data.vision_data_global.pos.z, global_data.vision_data_global.ang.x, global_data.vision_data_global.ang.y, global_data.vision_data_global.ang.z);
}
/**
* @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)
}
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 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);
}
}
void *spRun(void *ptr_thread_data) {
int inReportBytesCount = 0;
#if IBM
Sleep(SLEEP_TIME * 3);
#else
usleep(SLEEP_TIME * 3);
#endif
sp_init();
gPtrThreadData = (struct sp_thread_data *) ptr_thread_data;
int result = sp_panel_open();
if (result < 0) {
XPLMDebugString("-> CP: sp_controller.spRun: shutdown thread.\n");
pthread_exit(NULL);
return 0;
}
XPLMDebugString("-> CP: sp_controller.spRun: panel opened.\n");
last_mainloop_idle = sys_time_clock_get_time_usec();
// while stop == 0 calculate position.
while (gPtrThreadData->stop == 0) {
long loop_start_time = sys_time_clock_get_time_usec();
///////////////////////////////////////////////////////////////////////////
/// Read panel for new messages. CRITICAL FAST 100 Hz functions
///////////////////////////////////////////////////////////////////////////
if (us_run_every(10000, COUNTER3, loop_start_time)) {
// read/write board
sp_led_update();
inReportBytesCount = sp_panel_read_non_blocking(buf);
if (inReportBytesCount > 0) {
if (inReportBytesCount > 3) {
sprintf(tmp, "-> CP: sp_controller.run: bytes in report %d: %#0x,%#0x,%#0x\n", inReportBytesCount, buf[2], buf[1], buf[0]);
XPLMDebugString(tmp);
}
uint32_t msg = 0;
msg += buf[2] << 16;
msg += buf[1] << 8;
msg += buf[0];
sp_process(msg);
}
}
///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////
/// Update Panel. CRITICAL 20 Hz functions:
///////////////////////////////////////////////////////////////////////////
if (us_run_every(50000, COUNTER4, loop_start_time)) {
// Update local DataRefs.
sp_update_datarefs();
// update Panel.
inReportBytesCount = sp_panel_read_non_blocking(buf);
if (inReportBytesCount > 0) {
if (inReportBytesCount > 3) {
sprintf(tmp, "-> CP: sp_controller.run: bytes in report %d: %#0x,%#0x,%#0x\n", inReportBytesCount, buf[2], buf[1], buf[0]);
XPLMDebugString(tmp);
}
uint32_t msg = 0;
msg += buf[2] << 16;
msg += buf[1] << 8;
msg += buf[0];
sp_process(msg);
}
if (gEngineKnobState == 0x020000) {
// Start engine
if (gSpNumberOfEngines == 1) {
XPLMCommandOnce(gSpEngineStart1CmdRef);
} else if (gSpNumberOfEngines == 2) {
XPLMCommandOnce(gSpEngineStart1CmdRef);
XPLMCommandOnce(gSpEngineStart2CmdRef);
} else if (gSpNumberOfEngines == 3) {
XPLMCommandOnce(gSpEngineStart1CmdRef);
XPLMCommandOnce(gSpEngineStart2CmdRef);
XPLMCommandOnce(gSpEngineStart3CmdRef);
} else if (gSpNumberOfEngines == 4) {
XPLMCommandOnce(gSpEngineStart1CmdRef);
XPLMCommandOnce(gSpEngineStart2CmdRef);
XPLMCommandOnce(gSpEngineStart3CmdRef);
XPLMCommandOnce(gSpEngineStart4CmdRef);
}
}
if (gSpGearRetract > 0) {
if (gearledPrev != gearled) {
gearledPrev = gearled;
writeBuf[1] = gearled;
sp_panel_write(writeBuf);
}
}
}
///////////////////////////////////////////////////////////////////////////
if (loop_start_time - last_mainloop_idle >= MAX_DELAY_TIME) {
XPLMDebugString("-> CP: sp_controller.run: CRITICAL WARNING! CPU LOAD TOO HIGH.\n");
last_mainloop_idle = loop_start_time;//reset to prevent multiple messages
} else {
//writeConsole(0, 0, "CPU LOAD OK.");
}
// wait 1 milliseconds
#if IBM
Sleep(SLEEP_TIME * 3);
#else
usleep(SLEEP_TIME * 3);
#endif
}
sp_panel_close();
#if IBM
Sleep(SLEEP_TIME * 3);
#else
usleep(SLEEP_TIME * 3);
#endif
pthread_exit(NULL);
return 0;
}
