/* I2C1 bus operation starter function */
void i2c1_op(i2c_package * package)
{
//setup error codes
// package.i2c_error_code=I2C_CODE_NOT_KNOWN;
// package.i2c_error_counter=0;
//TODO why doesn't this work?
// If Buffer is full we can't send a package
if (i2c_package_buffer1_current_idx - i2c_package_buffer1_insert_idx == 1
|| (i2c_package_buffer1_current_idx == 0
&& i2c_package_buffer1_insert_idx
== (I2C_PACKAGE_BUFFER_SIZE - 1)))
{
//TODO Change function to return an error code and adapt all calls to check if they where successful. 20100428 Laurens
// DIRTY HACK: a motorcontroller is pulling SDA down we will restart
I2C1CONSET = 1 << STO; // generate I2C stop condition on the bus without restart
I2C1CONSET = 1 << STA;
// I2C1CONCLR = 1 << SIC; // clear I2C interrupt flag
// debug_message_buffer_sprintf(
// "i2c I2C1STAT is: %i.",
// I2C1STAT);
if (global_data.err_reporting_i2c)
{
debug_message_buffer_sprintf("*****Restarted I2C1*** I2C1STAT=%i",I2C1STAT);
//END HACK
if (i2c1_reject_count++ % 256 == 0)
{
debug_message_buffer_sprintf(
"i2c1 buffer full. Rejected package. Total: %i",
i2c1_reject_count);
}
}
return;
}
package_buffer1[i2c_package_buffer1_insert_idx] = *package; // add new i2c package to package buffer 1
i2c_package_buffer1_insert_idx = (i2c_package_buffer1_insert_idx + 1)
% I2C_PACKAGE_BUFFER_SIZE; // increment package insertion pointer
if (!i2c1_busy)
{ // check whether I2C1 is busy: if busy, data will I2C operation will be executed later, but program execution can continue
i2c1_busy = 1; // process locks I2C resource
I2C1CONSET = 1 << STA; // I2C start condition is generated on the bus -> interrupt is generated when ready; next state code: 0x08
error_counter1 = 0;
}
return;
}
uint8_t handle_reset_request(void)
{
static int reset_countdown;
if ((global_data.param[PARAM_IMU_RESET] == 1))
{
if (global_data.state.status == MAV_STATE_STANDBY)
{
if (reset_countdown == 0)
{
debug_message_buffer("RESET command received");// Won't come thru...
reset_countdown = 7;
}
else if (reset_countdown == 1)
{
watchdog_wait_reset();
}
reset_countdown--;
debug_message_buffer_sprintf("RESET count down: %u",
reset_countdown);
}
else
{
reset_countdown = 0;
global_data.param[PARAM_IMU_RESET] = 0;
debug_message_buffer("RESET command REFUSED (I'm flying!)");
}
}
return 0;
}
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 vision_buffer_buffer_camera_triggered(uint64_t usec,
uint64_t loop_start_time, uint32_t seq)
{
//debug_message_buffer("cam triggered");
// Emit timestamp of this image
// mavlink_msg_image_triggered_send(MAVLINK_COMM_0,
// usec);
if (vision_buffer_index_read - vision_buffer_index_write == 1
|| (vision_buffer_index_read == 0 && vision_buffer_index_write
== VISION_BUFFER_COUNT - 1))
{
//overwrite oldest
vision_buffer_index_read = (vision_buffer_index_read + 1)
% VISION_BUFFER_COUNT;
// if (vision_buffer_full_count++ % 16 == 0)
if (vision_buffer_full_count++ % 256 == 0)
{
debug_message_buffer_sprintf("vision_buffer buffer full %u",
vision_buffer_full_count);
}
}
vision_buffer_index_write = (vision_buffer_index_write + 1)
% VISION_BUFFER_COUNT;
//vision_buffer_count++;
// save position in buffer
vision_buffer[vision_buffer_index_write].pos = global_data.position;
vision_buffer[vision_buffer_index_write].ang = global_data.attitude;
vision_buffer[vision_buffer_index_write].time_captured = usec;
// vision_buffer[vision_buffer_index_write].time_captured = loop_start_time;
//debug_message_buffer("vision_buffer stored data");
// Emit sensor data matching to this image
//TODO: get this from buffer
// mavlink_msg_attitude_send(MAVLINK_COMM_0,
// usec,
// global_data.attitude.x, global_data.attitude.y,
// global_data.attitude.z, global_data.gyros_si.x,
// global_data.gyros_si.y, global_data.gyros_si.z);
}
//@{
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;
}
}
inline void remote_control(void)
{
static uint32_t lossCounter = 0;
if (global_data.state.mav_mode & (uint8_t) MAV_MODE_FLAG_MANUAL_INPUT_ENABLED)
{
if (radio_control_status() == RADIO_CONTROL_ON)
{
global_data.state.remote_ok=1;
if (lossCounter > 0)
{
debug_message_buffer("REGAINED REMOTE SIGNAL AFTER LOSS!");
}
lossCounter = 0;
//get remote controll values
float gas_remote = PPM_SCALE_FACTOR * (ppm_get_channel(
global_data.param[PARAM_PPM_THROTTLE_CHANNEL]) - PPM_OFFSET);
// message_debug_send(MAVLINK_COMM_1, 12, gas_remote);
float yaw_remote = PPM_SCALE_FACTOR * (ppm_get_channel(
global_data.param[PARAM_PPM_YAW_CHANNEL]) - PPM_CENTRE);
// message_debug_send(MAVLINK_COMM_1, 13, yaw_remote);
float nick_remote = PPM_SCALE_FACTOR * (ppm_get_channel(
global_data.param[PARAM_PPM_NICK_CHANNEL]) - PPM_CENTRE);
// message_debug_send(MAVLINK_COMM_1, 12, gas_remote);
float roll_remote = PPM_SCALE_FACTOR * (ppm_get_channel(
global_data.param[PARAM_PPM_ROLL_CHANNEL]) - PPM_CENTRE);
// message_debug_send(MAVLINK_COMM_1, 13, yaw_remote);
// if(radio_control_status()==RADIO_CONTROL_OFF){
// gas_remote=0.3;
// yaw_remote=0;
// nick_remote=0;
// roll_remote=0;
// }
//MANUAL ATTITUDE CONTROL
global_data.attitude_setpoint_remote.x = -roll_remote;
global_data.attitude_setpoint_remote.y = -nick_remote;
global_data.attitude_setpoint_remote.z = -5 * yaw_remote;// used as yaw speed!!! yaw offset1
global_data.gas_remote = gas_remote;
//MANUAL POSITION CONTROL
//TODO
// Switch on MAV_STATE_ACTIVE
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_LOW_TRIG))
{
// //Reset YAW integration(only needed if no vision)
// global_data.attitude.z = 0;
// global_data.yaw_pos_setpoint = 0;
if (global_data.state.status != MAV_STATE_ACTIVE)
{
debug_message_buffer("MAV_STATE_ACTIVE Motors started");
//debug_message_buffer("CALIBRATING GYROS");
//start_gyro_calibration();
}
//switch on motors
global_data.state.status = MAV_STATE_ACTIVE;
global_data.state.fly = FLY_WAIT_MOTORS;
global_data.state.mav_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
//this will be done by setpoint
if (global_data.state.mav_mode & MAV_MODE_FLAG_TEST_ENABLED)
{
global_data.state.fly = FLY_FLYING;
}
}
// Switch off MAV_STATE_STANDBY
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))
{
if (global_data.state.status != MAV_STATE_STANDBY)
{
debug_message_buffer("MAV_STATE_STANDBY Motors off");
}
//switch off motors
global_data.state.status = MAV_STATE_STANDBY;
global_data.state.fly = FLY_GROUNDED;
global_data.state.mav_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
}
// Start Gyro calibration left stick: left down. right stick right down.
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)
&& (ppm_get_channel(
global_data.param[PARAM_PPM_NICK_CHANNEL])
< PPM_LOW_TRIG) && (ppm_get_channel(
global_data.param[PARAM_PPM_ROLL_CHANNEL]) < PPM_LOW_TRIG))
{
start_gyro_calibration();
}
// Start capture: left down. right stick right up.
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)
&& (ppm_get_channel(
global_data.param[PARAM_PPM_NICK_CHANNEL])
> PPM_HIGH_TRIG) && (ppm_get_channel(
global_data.param[PARAM_PPM_ROLL_CHANNEL]) < PPM_LOW_TRIG))
{
// FIXME LORENZ CAPTURE START
//should actually go to capturer not IMU
}
// Stop capture: left down. right stick right up.
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)
&& (ppm_get_channel(
global_data.param[PARAM_PPM_NICK_CHANNEL])
> PPM_HIGH_TRIG) && (ppm_get_channel(
global_data.param[PARAM_PPM_ROLL_CHANNEL]) > PPM_HIGH_TRIG))
{
// FIXME LORENZ CAPTURE END
//should actually go to capturer not IMU
}
if (global_data.state.mav_mode & MAV_MODE_FLAG_GUIDED_ENABLED)
{
// Reset position to 0,0, mostly useful for optical flow with setpoint at 0,0
if (ppm_get_channel(global_data.param[PARAM_PPM_TUNE4_CHANNEL])
< PPM_LOW_TRIG)
{
global_data.position.x = 0;
global_data.position.y = 0;
}
}
if (global_data.state.mav_mode & MAV_MODE_FLAG_TEST_ENABLED)
{
if (ppm_get_channel(global_data.param[PARAM_PPM_TUNE1_CHANNEL])
< PPM_LOW_TRIG)
{
//z-controller disabled
global_data.param[PARAM_MIX_POSITION_Z_WEIGHT] = 0;
}
else
{
//z-controller enabled
global_data.param[PARAM_MIX_POSITION_Z_WEIGHT] = 1;
}
if (ppm_get_channel(global_data.param[PARAM_PPM_TUNE4_CHANNEL])
< PPM_LOW_TRIG)
{
//low - Position Hold off
global_data.param[PARAM_MIX_POSITION_WEIGHT] = 0;
global_data.param[PARAM_MIX_POSITION_YAW_WEIGHT] = 0;
global_data.position.x = 0;
global_data.position.y = 0;
}
else if (ppm_get_channel(
global_data.param[PARAM_PPM_TUNE4_CHANNEL])
> PPM_HIGH_TRIG)
{
//high - Position Hold with setpoint movement TODO
global_data.param[PARAM_MIX_POSITION_WEIGHT] = 1;
global_data.param[PARAM_MIX_POSITION_YAW_WEIGHT] = 1;
}
else
{
//center - Position Hold
global_data.param[PARAM_MIX_POSITION_WEIGHT] = 1;
global_data.param[PARAM_MIX_POSITION_YAW_WEIGHT] = 1;
}
}
// //Integrate YAW setpoint
// if (fabs(
// (ppm_get_channel(global_data.param[PARAM_PPM_GIER_CHANNEL])
// - PPM_CENTRE)) > 10)
// {
// global_data.yaw_pos_setpoint -= 0.02 * 0.03
// * (ppm_get_channel(
// global_data.param[PARAM_PPM_GIER_CHANNEL])
// - PPM_CENTRE);
// }
// Set PID VALUES
float tune2 = 1 * (ppm_get_channel(
global_data.param[PARAM_PPM_TUNE2_CHANNEL]) - 1109);
float tune3 = 1 * (ppm_get_channel(
global_data.param[PARAM_PPM_TUNE3_CHANNEL]) - 1109);
if (tune2 < 0)
{
tune2 = 0;
}
if (tune2 > 1000)
{
tune2 = 1000;
}
if (tune3 < 0)
{
tune3 = 0;
}
if (tune3 > 1000)
{
tune3 = 1000;
}
//TUNING FOR TOBIS REMOTE CONTROL
if (global_data.param[PARAM_TRIMCHAN] == 1)
{
global_data.param[PARAM_PID_ATT_P] = 0.1 * tune2;
// global_data.param[PARAM_PID_ATT_I] = 0;
global_data.param[PARAM_PID_ATT_D] = 0.1 * tune3;
}
else if (global_data.param[PARAM_TRIMCHAN] == 2)
{
global_data.param[PARAM_PID_POS_P] = 0.01 * tune2;
// global_data.param[PARAM_PID_POS_I] = 0;
global_data.param[PARAM_PID_POS_D] = 0.01 * tune3;
}
else if (global_data.param[PARAM_TRIMCHAN] == 3)
{
global_data.param[PARAM_PID_POS_Z_P] = 0.01 * tune2;
// global_data.param[PARAM_PID_POS_Z_I] = 0;
global_data.param[PARAM_PID_POS_Z_D] = 0.01 * tune3;
}
else if (global_data.param[PARAM_TRIMCHAN] == 4)
{
global_data.param[PARAM_PID_YAWSPEED_P] = 0.1 * tune2;
// global_data.param[PARAM_PID_YAWSPEED_I] = 0;
global_data.param[PARAM_PID_YAWSPEED_D] = 0.1 * tune3;
}
else if (global_data.param[PARAM_TRIMCHAN] == 5)
{
global_data.param[PARAM_PID_YAWPOS_P] = 0.1 * tune2;
// global_data.param[PARAM_PID_YAWPOS_I] = 0;
global_data.param[PARAM_PID_YAWPOS_D] = 0.1 * tune3;
}
//this is done at 10 Hz
// pid_set_parameters(&nick_controller,
// global_data.param[PARAM_PID_ATT_P],
// global_data.param[PARAM_PID_ATT_I],
// global_data.param[PARAM_PID_ATT_D],
// global_data.param[PARAM_PID_ATT_AWU]);
// pid_set_parameters(&roll_controller,
// global_data.param[PARAM_PID_ATT_P],
// global_data.param[PARAM_PID_ATT_I],
// global_data.param[PARAM_PID_ATT_D],
// global_data.param[PARAM_PID_ATT_AWU]);
//
// pid_set_parameters(&x_axis_controller,
// global_data.param[PARAM_PID_POS_P],
// global_data.param[PARAM_PID_POS_I],
// global_data.param[PARAM_PID_POS_D],
// global_data.param[PARAM_PID_POS_AWU]);
// pid_set_parameters(&y_axis_controller,
// global_data.param[PARAM_PID_POS_P],
// global_data.param[PARAM_PID_POS_I],
// global_data.param[PARAM_PID_POS_D],
// global_data.param[PARAM_PID_POS_AWU]);
//global_data.param_name[PARAM_MIX_REMOTE_WEIGHT] = 1;// add position only keep - tune3;
//global_data.param[PARAM_MIX_POSITION_WEIGHT] = 1;
if (global_data.param[PARAM_PPM_TUNE1_CHANNEL] != -1)
{
global_data.param[PARAM_CAM_ANGLE_Y_OFFSET]
= ((float) (ppm_get_channel(
global_data.param[PARAM_PPM_TUNE1_CHANNEL]))
- 1000.0f) / 1000.0f;
}
}
else
{
//No Remote signal
lossCounter++;
if (global_data.state.remote_ok == 1)
{
//Wait one round and start sinking
global_data.state.remote_ok = 0;
debug_message_buffer("No remote signal (1st time)");
}
else
{
static uint16_t countdown;
//already the second time
// Emergency Landing
if (global_data.state.fly != FLY_GROUNDED
&& global_data.state.fly != FLY_RAMP_DOWN && global_data.state.fly != FLY_LANDING)
{
debug_message_buffer_sprintf("global_data.state.fly=%i",global_data.state.fly);
sys_set_state(MAV_STATE_EMERGENCY);
global_data.state.fly = FLY_LANDING;//start landing
debug_message_buffer(
"EMERGENCY LANDING STARTED. No remote signal");
countdown = 50 * 5; // 5 seconds
}
else
{
if (global_data.state.fly == FLY_GROUNDED || global_data.state.fly == FLY_WAIT_MOTORS)
{
if (global_data.state.mav_mode & MAV_MODE_FLAG_SAFETY_ARMED)
{
if (lossCounter < 5)
{
debug_message_buffer(
"EMERGENCY LANDING FINISHED. No remote signal");
debug_message_buffer(
"EMERGENCY LANDING NOW LOCKED");
}
}
// Set to disarmed
sys_set_mode(global_data.state.mav_mode & ~MAV_MODE_FLAG_SAFETY_ARMED);
sys_set_state(MAV_STATE_STANDBY);
}
else
{
//won't come here anymore if once in locked mode
debug_message_buffer(
"EMERGENCY LANDING. No remote signal");
}
}
if((global_data.state.mav_mode & MAV_MODE_FLAG_TEST_ENABLED) && global_data.state.fly != FLY_GROUNDED){
//z-controller enabled
global_data.param[PARAM_MIX_POSITION_Z_WEIGHT] = 1;
global_data.position_setpoint.z = global_data.position.z
+ 0.01;
global_data.param[PARAM_POSITION_SETPOINT_Z]
= global_data.position.z + 0.01;
if (countdown-- <= 1)
{
global_data.state.fly = FLY_GROUNDED;
//global_data.param[PARAM_MIX_POSITION_Z_WEIGHT] = 0;
debug_message_buffer(
"EMERGENCY LANDING Z control finished");
}
}
}
}
}
}
/* Interrupt handler for I2C0 interrupt */
static void I2C0_ISR(void)
{
//sprintf((char*)buffer, "h ");
//message_send_debug(COMM_1, buffer);
//uint8_t buffer[200]; // string buffer for debug messages
ISR_ENTRY();
if (error_counter0 > I2C_PERMANENT_ERROR_LIMIT)
{
//uint8_t buffer[50];
//sprintf(buffer, "i2c error limit %d \n", 1);
// sprintf((char*)buffer, package_buffer0[i2c_package_buffer0_current_idx].slave_address) ;
//message_send_debug(COMM_1, buffer);
// debug_message_buffer("i2c error limit reached");
debug_message_buffer_sprintf("i2c0 error limit reached. Dest: %i",
package_buffer0[i2c_package_buffer0_current_idx].slave_address);
if (i2c0_permanent_error_count++ % 256 == 0)
{
debug_message_buffer_sprintf(
"i2c0 error limit reached. Total: %i errors.",
i2c0_permanent_error_count);
}
package_buffer0[i2c_package_buffer0_current_idx].i2c_error_code
= I2C_CODE_ERROR;//set error code to error
//TODO set this to ok if everzthing was ok
if (package_buffer0[i2c_package_buffer0_current_idx].i2c_done_handler
!= NULL)
{ // check if there is a package handler registered
package_buffer0[i2c_package_buffer0_current_idx].i2c_done_handler(
&(package_buffer0[i2c_package_buffer0_current_idx])); // call package handler
}
i2c_package_buffer0_insert_idx = (i2c_package_buffer0_insert_idx + 1)
% I2C_PACKAGE_BUFFER_SIZE; // increment package insertion pointer
error_counter0 = 0;
if (i2c_package_buffer0_current_idx == i2c_package_buffer0_insert_idx)
{ // no unhandled packages
I2C0CONCLR = 1 << STAC;
I2C0CONSET = 1 << STO; // generate I2C stop condition on the bus without restart
I2C0CONCLR = 1 << SIC; // clear I2C interrupt flag
i2c0_busy = 0; // release I2C resource
}
else
{ // unhandled packages in package buffer
if (!(package_buffer0[i2c_package_buffer0_current_idx].write_read
== 1))
{
I2C0CONSET = 1 << STO; // generate stop condition on the bus if no repeated start is necessary
}
I2C0CONSET = 1 << STA; // generate "repeated start" condition on the bus -> next state: 0x10
I2C0CONCLR = 1 << SIC; // clear I2C interrupt flag
}
}
if (i2c0_busy == 1)
{ //Return if Package error
//message_debug_send(MAVLINK_COMM_1, 30, I2C0STAT);
switch (I2C0STAT)
{ // check current I2C0 state
case 0x08: // I2C start condition has been generated on the bus -> transmit slave address and read/write bit -> next state: 0x18 or 0x40
I2C0DAT
= package_buffer0[i2c_package_buffer0_current_idx].slave_address
| package_buffer0[i2c_package_buffer0_current_idx].direction;
I2C0CONCLR = 1 << STAC; // do not restart (continue normal I2C operation)
I2C0CONCLR = 1 << SIC; // clear I2C interrupt flag
break;
case 0x10: // "repeated start" condition has been generated on the bus -> transmit slave address and read/write bit -> next state: 0x18 or 0x40
I2C0DAT
= package_buffer0[i2c_package_buffer0_current_idx].slave_address
| package_buffer0[i2c_package_buffer0_current_idx].direction;
I2C0CONCLR = 1 << STAC;
I2C0CONCLR = 1 << SIC;
break;
case 0x18: // slave address and write bit has been transmitted -> transmit first data byte -> next state: 0x28
current_data_byte_i2c0 = 0;
I2C0CONCLR = 1 << STAC;
I2C0DAT
= package_buffer0[i2c_package_buffer0_current_idx].data[current_data_byte_i2c0];
current_data_byte_i2c0++;
I2C0CONCLR = 1 << SIC;
break;
case 0x28: // data byte has been transmitted
// if there's another byte to be transmitted, do it
if (current_data_byte_i2c0
< package_buffer0[i2c_package_buffer0_current_idx].length)
{
I2C0CONCLR = 1 << STAC;
I2C0DAT
= package_buffer0[i2c_package_buffer0_current_idx].data[current_data_byte_i2c0];
current_data_byte_i2c0++;
I2C0CONCLR = 1 << SIC;
}
// it the last byte has been transmitted, check if there are unhandled packages in the package buffer
else
{
i2c_package_buffer0_current_idx
= (i2c_package_buffer0_current_idx + 1)
% I2C_PACKAGE_BUFFER_SIZE; // increment pointer to package in processing
error_counter0 = 0;
if (i2c_package_buffer0_current_idx
== i2c_package_buffer0_insert_idx)
{ // no unhandled packages
I2C0CONCLR = 1 << STAC;
I2C0CONSET = 1 << STO; // generate I2C stop condition on the bus without restart
I2C0CONCLR = 1 << SIC; // clear I2C interrupt flag
i2c0_busy = 0; // release I2C resource
}
else
{ // unhandled packages in package buffer
if (!(package_buffer0[i2c_package_buffer0_current_idx].write_read
== 1))
{
I2C0CONSET = 1 << STO; // generate stop condition on the bus if no repeated start is necessary
}
I2C0CONSET = 1 << STA; // generate "repeated start" condition on the bus -> next state: 0x10
I2C0CONCLR = 1 << SIC; // clear I2C interrupt flag
}
}
break;
case 0x20: // I2C error state detection
I2C0CONSET = 1 << STO;
I2C0CONSET = 1 << STA; // restart I2C state machine with current package
error_counter0++;
//debug_message_buffer("I2C error: slave address not acknowledged (write)\n");
debug_message_buffer_sprintf(
"I2C0 error: slave address not acknowledged (write). Dest: %i",
package_buffer0[i2c_package_buffer0_current_idx].slave_address);
//message_send_debug(COMM_1, buffer);
I2C0CONCLR = 1 << SIC;
break;
case 0x30: // I2C error state detection
I2C0CONSET = 1 << STO;
I2C0CONSET = 1 << STA; // restart I2C state machine with current package
error_counter0++;
// debug_message_buffer("I2C error: data not acknowledged\n");
debug_message_buffer_sprintf(
"I2C0 error: data not acknowledged. Dest: %i",
package_buffer0[i2c_package_buffer0_current_idx].slave_address);
//message_send_debug(COMM_1, buffer);
I2C0CONCLR = 1 << SIC;
break;
case 0x38: // I2C error state detection
I2C0CONSET = 1 << STA; // restart I2C state machine with current package
error_counter0++;
// debug_message_buffer("I2C error: arbitration lost\n");
debug_message_buffer_sprintf(
"I2C0 error: arbitration lost. Dest: %i",
package_buffer0[i2c_package_buffer0_current_idx].slave_address);
//message_send_debug(COMM_1, buffer);
I2C0CONCLR = 1 << SIC;
break;
case 0x40: // slave address and read bit has been transmitted -> clear interrupt and wait for first data byte -> next state: 0x50 or 0x58
current_data_byte_i2c0 = 0;
if (package_buffer0[i2c_package_buffer0_current_idx].length > 1)
I2C0CONSET = 1 << AA; // if there's more than one byte to be received -> next state: 0x50
else
I2C0CONCLR = 1 << AAC; // if there's only one byte to be received -> next state: 0x58
I2C0CONCLR = 1 << SIC; // clear I2C interrupt flag
break;
case 0x50: // data byte has been received
package_buffer0[i2c_package_buffer0_current_idx].data[current_data_byte_i2c0]
= I2C0DAT; // copy data byte to data array in I2C package
current_data_byte_i2c0++; // increment data byte
if ((current_data_byte_i2c0 + 1)
< package_buffer0[i2c_package_buffer0_current_idx].length)
{ // there's more than one byte left to be received
I2C0CONSET = 1 << AA; // acknowledge next data byte -> next state: 0x50
}
else
{ // there's only one byte left to be received
I2C0CONCLR = 1 << AAC; // do not acknowledge next data byte -> next state: 0x58
}
I2C0CONCLR = 1 << SIC;
break;
case 0x58: // last data byte has been received
package_buffer0[i2c_package_buffer0_current_idx].data[current_data_byte_i2c0]
= I2C0DAT; // copy data byte to data array in I2C package
I2C0CONSET = 1 << STO; // generate STOP condition on the I2C bus
//uart0_transmit(package_buffer0[i2c_package_buffer0_current_idx].data[current_data_byte_i2c0]);
if (package_buffer0[i2c_package_buffer0_current_idx].i2c_done_handler
!= NULL)
{ // check if there is a package handler registered
package_buffer0[i2c_package_buffer0_current_idx].i2c_done_handler(
&(package_buffer0[i2c_package_buffer0_current_idx])); // call package handler
}
i2c_package_buffer0_current_idx = (i2c_package_buffer0_current_idx
+ 1) % I2C_PACKAGE_BUFFER_SIZE; // increment pointer to package in processing
error_counter0 = 0;
// check if there are unhandled packages in the package buffer
if (i2c_package_buffer0_current_idx
== i2c_package_buffer0_insert_idx)
{ // no unhandled packages
I2C0CONCLR = 1 << SIC; // clear I2C interrupt flag
i2c0_busy = 0; // release I2C resource
}
else
{ // unhandled packages in package buffer
I2C0CONSET = 1 << STA; // generate "repeated start" condition on the bus -> next state: 0x10
I2C0CONCLR = 1 << SIC; // clear I2C interrupt flag
}
break;
case 0x48: // I2C error state detection
I2C0CONSET = 1 << STO;
I2C0CONSET = 1 << STA; // restart I2C state machine with current package
error_counter0++;
debug_message_buffer(
"I2C0 error: slave address not acknowledged (read)\n");
//message_send_debug(COMM_1, buffer);
I2C0CONCLR = 1 << SIC;
break;
default: // I2C error state detection
I2C0CONSET = 1 << STO;
I2C0CONSET = 1 << STA; // restart I2C state machine with current package
error_counter0++;
debug_message_buffer_sprintf("I2C0 error: undefined I2C state: %i",I2C0STAT);
debug_message_buffer_sprintf("I2C0 error: prior state: %X",i2c0stat_prior_state);
// message_send_debug(COMM_1, buffer);
I2C0CONCLR = 1 << SIC;
}
}
// Sum up errors
global_data.i2c0_err_count += error_counter0;
VICVectAddr = 0x00000000; // clear this interrupt from the VIC
ISR_EXIT();
// exit ISR
}
/**
* @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);
}
