// run the zigzag controller
// should be called at 100hz or more
void Copter::ModeZigZag::run()
{
// initialize vertical speed and acceleration's range
pos_control->set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);
pos_control->set_max_accel_z(g.pilot_accel_z);
// if not auto armed or motors not enabled set throttle to zero and exit immediately
if (!motors->armed() || !ap.auto_armed || !motors->get_interlock() || ap.land_complete) {
zero_throttle_and_relax_ac();
return;
}
// auto control
if (stage == AUTO) {
// if vehicle has reached destination switch to manual control
if (reached_destination()) {
AP_Notify::events.waypoint_complete = 1;
stage = MANUAL_REGAIN;
loiter_nav->init_target(wp_nav->get_wp_destination());
} else {
auto_control();
}
}
// manual control
if (stage == STORING_POINTS || stage == MANUAL_REGAIN) {
// receive pilot's inputs, do position and attitude control
manual_control();
}
}
//MAIN
int main(void)
{
interrupts();
setup_motor();
TWI_init(CONTROL_ADDRESS);
while(1){
if(x == 0){
manual_control_test();
}
else auto_control();
}
}
void channels_handler(const lcm_recv_buf_t *rbuf, const char *channel,
const channels_t *msg, void *userdata)
{
// create a copy of the received message
channels_t new_msg;
new_msg.utime = msg->utime;
new_msg.num_channels = msg->num_channels;
new_msg.channels = (int16_t*) malloc(msg->num_channels*sizeof(int16_t));
for(int i = 0; i < msg->num_channels; i++)
{
new_msg.channels[i] = msg->channels[i];
}
// set fence switching value based on the data collection from the Quadrotor joystick
double fence_thresh = 1500;
// Copy state to local state struct to minimize mutex lock time
struct state localstate;
pthread_mutex_lock(&state_mutex);
memcpy(&localstate, state, sizeof(struct state));
pthread_mutex_unlock(&state_mutex);
// Decide whether or not to edit the motor message prior to sending it
// set_points[] array is specific to geofencing. You need to add code
// to compute them for our FlightLab application!!!
float pose[8], set_points[8];
// Checking if auto-pilot has been switched on by the pilot
if (msg->channels[7] > fence_thresh)
localstate.fence_on = 1;
else
localstate.fence_on = 0;
if(localstate.fence_on == 1)
{
//printf("FENCE ON\n");
// Cpoy state from processing loop
for(int i = 0; i < 8; i++)
{
pose[i] = (float) localstate.pose[i];
set_points[i] = localstate.set_points[i];
}
// if (prev_local_state.time - localstate.time == 0 || (mcobs[0]->time - mcobs[1]->time)> 1.5)
// Check for data drop outs
if ((!localstate.data) || (prev_local_state.time - localstate.time == 0))
{
// Check if watchdog has not timed out and data has dropped
if (watchdog_count > 0)
watchdog_count--;
// If timed out then stay in base position
if (watchdog_count == 0)
{
printf("WATCHDOG TIMEOUT\n");
new_msg.channels[0] = thrust_PWM_base;
new_msg.channels[1] = roll_PWM_base;
new_msg.channels[2] = pitch_PWM_base;
new_msg.channels[3] = yaw_PWM_base;
}
// If not timed out then still move towards the set point normally with autocontrol
else
{
auto_control(pose, set_points, new_msg.channels);
}
}
// If Data is incoming normally use autocontrol and reset the counters
else
{
auto_control(pose, set_points, new_msg.channels);
watchdog_count++;
// If data starts coming properly then reset watchdog counter
if (watchdog_count > 3)
watchdog_count = 10;
}
printf("%d\n", watchdog_count);
printf( "%d %d %d %d\n", new_msg.channels[0],new_msg.channels[1],new_msg.channels[2],new_msg.channels[3]);
printf( "%f %f %f %f\n", localstate.pose[0],localstate.pose[1],localstate.pose[2],localstate.pose[3]);
sleep(0.5);
}
else
{ // Fence off
// pass user commands through without modifying
// new_msg.channels[7] = 1180;
printf("FENCE OFF\n");
}
// send lcm message to motors
channels_t_publish((lcm_t *) userdata, "CHANNELS_1_TX", &new_msg);
// Save received (msg) and modified (new_msg) command data to file.
// NOTE: Customize as needed (set_points[] is for geofencing)
fprintf(block_txt,"%ld,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%f,%f,%f,%f,%f,%f,%f,%f\n",
(long int) msg->utime,msg->channels[0],msg->channels[1],msg->channels[2],
msg->channels[3], msg->channels[7],
new_msg.channels[0],new_msg.channels[1],new_msg.channels[2],
new_msg.channels[3],new_msg.channels[7],
set_points[0],set_points[1],set_points[2],
set_points[3],set_points[4],set_points[5],set_points[6],
set_points[7]);
fflush(block_txt);
pthread_mutex_lock(&state_mutex);
memcpy(state, &localstate, sizeof(struct state));
pthread_mutex_unlock(&state_mutex);
}
