bool calibration_enter(void)
{
// If not flying
if (!sys_state_is_flying())
{
calibration_prev_state = sys_get_state();
calibration_prev_mode = sys_get_mode();
// Lock vehicle during calibration
sys_set_mode((uint8_t)MAV_MODE_LOCKED);
sys_set_state((uint8_t)MAV_STATE_CALIBRATING);
debug_message_buffer("Starting calibration.");
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());
debug_message_send_one();
debug_message_send_one();
return true;
}
else
{
//Can't calibrate during flight
debug_message_buffer("Can't calibrate during flight!!!");
return false;
}
}
/*
* Starts all the other tasks, then starts the scheduler.
*/
void main( void )
{
#ifdef DEBUG
debug();
#endif
/* Setup any hardware that has not already been configured by the low
level init routines. */
prvSetupHardware();
/* Create the queue used to send data to the LCD task. */
xLCDQueue = xQueueCreate( mainLCD_QUEUE_LEN, sizeof( xLCDMessage ) );
/* Start all the standard demo application tasks. */
vStartIntegerMathTasks( tskIDLE_PRIORITY );
vStartLEDFlashTasks( mainLED_TASK_PRIORITY );
vStartPolledQueueTasks( mainQUEUE_POLL_PRIORITY );
vStartSemaphoreTasks( mainSEM_TEST_PRIORITY );
vStartBlockingQueueTasks( mainBLOCK_Q_PRIORITY );
vStartDynamicPriorityTasks();
vStartMathTasks( tskIDLE_PRIORITY );
vAltStartComTestTasks( mainCOM_TEST_PRIORITY, mainCOM_TEST_BAUD_RATE, mainCOM_TEST_LED );
vStartGenericQueueTasks( mainGENERIC_QUEUE_PRIORITY );
vStartQueuePeekTasks();
/* Start the tasks which are defined in this file. */
xTaskCreate( vErrorChecks, "Check", configMINIMAL_STACK_SIZE, NULL, mainCHECK_TASK_PRIORITY, NULL );
xTaskCreate( prvLCDTask, "LCD", configMINIMAL_STACK_SIZE, ( void * ) &xLCDQueue, mainLCD_TASK_PRIORITY, NULL );
xTaskCreate( prvLCDMessageTask, "MSG", configMINIMAL_STACK_SIZE, ( void * ) &xLCDQueue, mainMSG_TASK_PRIORITY, NULL );
/* Start either the uIP TCP/IP stack or the lwIP TCP/IP stack. */
#ifdef STACK_UIP
/* Finally, create the WEB server task. */
xTaskCreate( vuIP_Task, "uIP", configMINIMAL_STACK_SIZE * 3, NULL, mainCHECK_TASK_PRIORITY - 1, NULL );
#endif
#ifdef STACK_LWIP
/* Create the lwIP task. This uses the lwIP RTOS abstraction layer.*/
vlwIPInit();
sys_set_state( ( signed portCHAR * ) "httpd", lwipBASIC_SERVER_STACK_SIZE );
sys_thread_new( vBasicWEBServer, ( void * ) NULL, basicwebWEBSERVER_PRIORITY );
sys_set_default_state();
#endif
/* Start the scheduler.
NOTE : Tasks run in system mode and the scheduler runs in Supervisor mode.
The processor MUST be in supervisor mode when vTaskStartScheduler is
called. The demo applications included in the FreeRTOS.org download switch
to supervisor mode prior to main being called. If you are not using one of
these demo application projects then ensure Supervisor mode is used here. */
vTaskStartScheduler();
/* We should never get here as control is now taken by the scheduler. */
for( ;; );
}
void vlwIPInit( void )
{
/* Initialize lwIP and its interface layer. */
sys_init();
mem_init();
memp_init();
pbuf_init();
netif_init();
ip_init();
sys_set_state(( signed char * ) "lwIP", lwipTCP_STACK_SIZE);
tcpip_init( NULL, NULL );
sys_set_default_state();
}
void calibration_exit(void)
{
// Go back to old state
sys_set_mode(calibration_prev_mode);
sys_set_state(calibration_prev_state);
// Clear debug message buffers
for (int i = 0; i < DEBUG_COUNT; i++)
{
debug_message_send_one();
}
// Clear UART buffers
while (uart0_char_available())
{uart0_get_char();}
while (uart1_char_available())
{uart1_get_char();}
debug_message_buffer("Calibration finished. UART buffers cleared.");
}
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");
}
}
}
}
}
}
