void communication_send_remote_control(void)
{
if (global_data.param[PARAM_SEND_SLOT_REMOTE_CONTROL] == 1)
{
mavlink_msg_rc_channels_raw_send(global_data.param[PARAM_SEND_DEBUGCHAN],
radio_control_get_channel_raw(1),
radio_control_get_channel_raw(2),
radio_control_get_channel_raw(3),
radio_control_get_channel_raw(4),
radio_control_get_channel_raw(5),
radio_control_get_channel_raw(6),
radio_control_get_channel_raw(7),
radio_control_get_channel_raw(8),
((radio_control_status() > 0) ? 255 : 0));
// Should be global_data.rc_rssi in the future
// rc_to_255(1),
// rc_to_255(2),
// rc_to_255(3),
// rc_to_255(4),
// rc_to_255(5),
// rc_to_255(6),
// rc_to_255(7),
// rc_to_255(8),
}
}
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");
}
}
}
}
}
}
inline void remote_control(void)
{
if (global_data.state.mav_mode == (uint8_t) MAV_MODE_MANUAL || global_data.state.mav_mode
== (uint8_t) MAV_MODE_GUIDED || global_data.state.mav_mode
== (uint8_t) MAV_MODE_AUTO)
{
if (radio_control_status() == RADIO_CONTROL_ON)
{
global_data.state.remote_ok=1;
//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");
}
//switch on motors
global_data.state.status = MAV_STATE_ACTIVE;
// global_data.state.fly = FLY_WAIT_MOTORS;
// this will be done by setpoint
}
// 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;
}
// 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))
{
mavlink_msg_action_send(
global_data.param[PARAM_SEND_DEBUGCHAN],
global_data.param[PARAM_SYSTEM_ID],MAV_COMP_ID_IMU,
MAV_ACTION_REC_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))
{
mavlink_msg_action_send(
global_data.param[PARAM_SEND_DEBUGCHAN],
global_data.param[PARAM_SYSTEM_ID], MAV_COMP_ID_IMU,
MAV_ACTION_REC_STOP);
//should actually go to capturer not IMU
}
// //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
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
{
//already the second time
// Emergency Landing
if (global_data.state.fly != FLY_GROUNDED
&& global_data.state.fly != FLY_RAMP_DOWN)
{
sys_set_state(MAV_STATE_EMERGENCY);
global_data.state.fly = FLY_LANDING;//start landing
debug_message_buffer(
"EMERGENCY LANDING STARTED. No remote signal");
}
else
{
if (global_data.state.fly == FLY_GROUNDED)
{
sys_set_mode(MAV_MODE_LOCKED);
sys_set_state(MAV_STATE_STANDBY);
debug_message_buffer(
"EMERGENCY LANDING FINISHED. No remote signal");
debug_message_buffer(
"EMERGENCY LANDING NOW LOCKED");
}
else
{
//won't come here anymore if once in locked mode
debug_message_buffer(
"EMERGENCY LANDING. No remote signal");
}
}
}
}
}
}
/**
* @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);
}
}
