static int mc_thread_main(int argc, char *argv[]) { /* structures */ struct vehicle_status_s state; struct vehicle_attitude_s att; struct rc_channels_s rc; struct actuator_controls_s actuators; struct actuator_armed_s armed; /* subscribe to attitude, motor setpoints and system state */ int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); int state_sub = orb_subscribe(ORB_ID(vehicle_status)); int rc_sub = orb_subscribe(ORB_ID(rc_channels)); /* rate-limit the attitude subscription to 200Hz to pace our loop */ orb_set_interval(att_sub, 5); struct pollfd fds = { .fd = att_sub, .events = POLLIN }; /* publish actuator controls */ for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) actuators.control[i] = 0.0f; int actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators); armed.armed = true; int armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); /* register the perf counter */ perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "multirotor_control"); /* welcome user */ printf("[multirotor_control] starting\n"); while (!thread_should_exit) { /* wait for a sensor update */ poll(&fds, 1, -1); perf_begin(mc_loop_perf); /* get a local copy of the vehicle state */ orb_copy(ORB_ID(vehicle_status), state_sub, &state); /* get a local copy of rc inputs */ orb_copy(ORB_ID(rc_channels), rc_sub, &rc); /* get a local copy of attitude */ orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); /* run the attitude controller */ multirotor_control_attitude(&rc, &att, &state, &actuators); /* publish the result */ orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); perf_end(mc_loop_perf); } printf("[multirotor_control] stopping\r\n"); /* kill all outputs */ armed.armed = false; orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) actuators.control[i] = 0.0f; orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); close(att_sub); close(state_sub); close(rc_sub); close(actuator_pub); close(armed_pub); perf_print_counter(mc_loop_perf); perf_free(mc_loop_perf); fflush(stdout); exit(0); }
void *multirotor_attitude_control_thread_main() { /* welcome user */ fprintf (stdout, "Multirotor attitude controller started\n"); fflush(stdout); int i; bool_t updated, rates_sp_updated, reset_integral; float rates[3]; /* store last control mode to detect mode switches */ bool_t control_yaw_position = 1 /* true */; bool_t reset_yaw_sp = 1 /* true */; /* declare and safely initialize all structs */ struct parameter_update_s update; struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); struct vehicle_attitude_setpoint_s att_sp; memset(&att_sp, 0, sizeof(att_sp)); struct offboard_control_setpoint_s offboard_sp; memset(&offboard_sp, 0, sizeof(offboard_sp)); struct vehicle_control_flags_s control_flags; memset(&control_flags, 0, sizeof(control_flags)); struct manual_control_setpoint_s manual; memset(&manual, 0, sizeof(manual)); struct sensor_combined_s sensor; memset(&sensor, 0, sizeof(sensor)); struct vehicle_rates_setpoint_s rates_sp; memset(&rates_sp, 0, sizeof(rates_sp)); struct vehicle_status_s status; memset(&status, 0, sizeof(status)); struct actuator_controls_s actuators; memset(&actuators, 0, sizeof(actuators)); /* subscribe */ orb_subscr_t vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); if (vehicle_attitude_sub < 0) { fprintf (stderr, "Attitude controller thread failed to subscribe to vehicle_attitude topic\n"); exit(-1); } orb_subscr_t parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); if (parameter_update_sub < 0) { fprintf (stderr, "Attitude controller thread failed to subscribe to parameter_update topic\n"); exit(-1); } orb_subscr_t vehicle_attitude_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); if (vehicle_attitude_setpoint_sub < 0) { fprintf (stderr, "Attitude controller thread failed to subscribe to vehicle_attitude_setpoint topic\n"); exit(-1); } orb_subscr_t offboard_control_setpoint_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); if (offboard_control_setpoint_sub < 0) { fprintf (stderr, "Attitude controller thread failed to subscribe to offboard_control_setpoint topic\n"); exit(-1); } orb_subscr_t vehicle_control_flags_sub = orb_subscribe(ORB_ID(vehicle_control_flags)); if (vehicle_control_flags_sub < 0) { fprintf (stderr, "Attitude controller thread failed to subscribe to vehicle_control_flags topic\n"); exit(-1); } orb_subscr_t manual_control_setpoint_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); if (manual_control_setpoint_sub < 0) { fprintf (stderr, "Attitude controller thread failed to subscribe to manual_control_setpoint topic\n"); exit(-1); } orb_subscr_t sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); if (sensor_combined_sub < 0) { fprintf (stderr, "Attitude controller thread failed to subscribe to sensor_combined topic\n"); exit(-1); } orb_subscr_t vehicle_rates_setpoint_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); if (vehicle_rates_setpoint_sub < 0) { fprintf (stderr, "Attitude controller thread failed to subscribe to vehicle_rates_setpoint topic\n"); exit(-1); } orb_subscr_t vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); if (vehicle_status_sub < 0) { fprintf (stderr, "Attitude controller thread failed to subscribe to vehicle_status topic\n"); exit(-1); } /* publish actuator controls */ for (i = 0; i < NUM_ACTUATOR_CONTROLS; i++) { actuators.control[i] = 0.0f; } orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); if (actuator_pub == -1) { fprintf (stderr, "Comunicator thread failed to advertise the actutor_controls topic\n"); exit (-1); } orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint)); if (att_sp_pub == -1) { fprintf (stderr, "Comunicator thread failed to advertise the vehicle_attitude_setpoint topic\n"); exit (-1); } orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); orb_advert_t rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint)); if (rates_sp_pub == -1) { fprintf (stderr, "Comunicator thread failed to advertise the vehicle_rates_setpoint topic\n"); exit (-1); } orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); while (!_shutdown_all_systems) { /* wait for a sensor update, check for exit condition every 500 ms */ updated = orb_poll(ORB_ID(vehicle_attitude), vehicle_attitude_sub, 500000); /* timed out - periodic check for _shutdown_all_systems, etc. */ if (!updated) continue; /* this is undesirable but not much we can do - might want to flag unhappy status */ if (updated < 0) { fprintf (stderr, "Attitude controller failed to poll vehicle attitude\n"); continue; } /* only run controller if attitude changed */ /* attitude */ orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); /* parameters */ updated = orb_check (ORB_ID(parameter_update), parameter_update_sub); if (updated) { orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update); /* update parameters */ } /* control mode */ updated = orb_check (ORB_ID(vehicle_control_flags), vehicle_control_flags_sub); if (updated) { orb_copy(ORB_ID(vehicle_control_flags), vehicle_control_flags_sub, &control_flags); } /* manual control setpoint */ updated = orb_check (ORB_ID(manual_control_setpoint), manual_control_setpoint_sub); if (updated) { orb_copy(ORB_ID(manual_control_setpoint), manual_control_setpoint_sub, &manual); } /* attitude setpoint */ updated = orb_check (ORB_ID(vehicle_attitude_setpoint), vehicle_attitude_setpoint_sub); if (updated) { orb_copy(ORB_ID(vehicle_attitude_setpoint), vehicle_attitude_setpoint_sub, &att_sp); } /* offboard control setpoint */ updated = orb_check (ORB_ID(offboard_control_setpoint), offboard_control_setpoint_sub); if (updated) { orb_copy(ORB_ID(offboard_control_setpoint), offboard_control_setpoint_sub, &offboard_sp); } /* vehicle status */ updated = orb_check (ORB_ID(vehicle_status), vehicle_status_sub); if (updated) { orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &status); } /* sensors */ updated = orb_check (ORB_ID(sensor_combined), sensor_combined_sub); if (updated) { orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); } /* set flag to safe value */ control_yaw_position = 1 /* true */; /* reset yaw setpoint if not armed */ if (!control_flags.flag_armed) { reset_yaw_sp = 1 /* true */; } /* define which input is the dominating control input */ if (control_flags.flag_control_offboard_enabled) { /* offboard inputs */ if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES) { rates_sp.roll = offboard_sp.p1; rates_sp.pitch = offboard_sp.p2; rates_sp.yaw = offboard_sp.p3; rates_sp.thrust = offboard_sp.p4; rates_sp.timestamp = get_absolute_time(); orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); } else if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE) { att_sp.roll_body = offboard_sp.p1; att_sp.pitch_body = offboard_sp.p2; att_sp.yaw_body = offboard_sp.p3; att_sp.thrust = offboard_sp.p4; att_sp.timestamp = get_absolute_time(); /* publish the result to the vehicle actuators */ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); } /* reset yaw setpoint after offboard control */ reset_yaw_sp = 1 /* true */; } else if (control_flags.flag_control_manual_enabled) { /* manual input */ if (control_flags.flag_control_attitude_enabled) { /* control attitude, update attitude setpoint depending on mode */ if (att_sp.thrust < 0.1f) { /* no thrust, don't try to control yaw */ rates_sp.yaw = 0.0f; control_yaw_position = 0 /* false */; if (status.condition_landed) { /* reset yaw setpoint if on ground */ reset_yaw_sp = 1 /* true */; } } else { /* only move yaw setpoint if manual input is != 0 */ if (manual.yaw < -yaw_deadzone || yaw_deadzone < manual.yaw) { /* control yaw rate */ control_yaw_position = 0 /* false */; rates_sp.yaw = manual.yaw; reset_yaw_sp = 1 /* true */; // has no effect on control, just for beautiful log } else { control_yaw_position = 1 /* true */; } } if (!control_flags.flag_control_velocity_enabled) { /* update attitude setpoint if not in position control mode */ att_sp.roll_body = manual.roll; att_sp.pitch_body = manual.pitch; if (!control_flags.flag_control_climb_rate_enabled) { /* pass throttle directly if not in altitude control mode */ att_sp.thrust = manual.thrust; } } /* reset yaw setpint to current position if needed */ if (reset_yaw_sp) { att_sp.yaw_body = att.yaw; reset_yaw_sp = 0 /* false */; } /*if (motor_test_mode) { printf("testmode"); att_sp.roll_body = 0.0f; att_sp.pitch_body = 0.0f; att_sp.yaw_body = 0.0f; att_sp.thrust = 0.1f; }*/ att_sp.timestamp = get_absolute_time(); /* publish the attitude setpoint */ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); } else { /* manual rate inputs (ACRO), from RC control or joystick */ if (control_flags.flag_control_rates_enabled) { rates_sp.roll = manual.roll; rates_sp.pitch = manual.pitch; rates_sp.yaw = manual.yaw; rates_sp.thrust = manual.thrust; rates_sp.timestamp = get_absolute_time(); } /* reset yaw setpoint after ACRO */ reset_yaw_sp = 1 /* true */; } } else { if (!control_flags.flag_control_auto_enabled) { /* no control, try to stay on place */ if (!control_flags.flag_control_velocity_enabled) { /* no velocity control, reset attitude setpoint */ att_sp.roll_body = 0.0f; att_sp.pitch_body = 0.0f; att_sp.timestamp = get_absolute_time(); orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); } } /* reset yaw setpoint after non-manual control */ reset_yaw_sp = 1 /* true */; } /* check if we should we reset integrals */ reset_integral = !control_flags.flag_armed || att_sp.thrust < 0.1f; // TODO use landed status instead of throttle /* run attitude controller if needed */ if (control_flags.flag_control_attitude_enabled) { multirotor_control_attitude(&att_sp, &att, &rates_sp, control_yaw_position, reset_integral); orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); } /* run rates controller if needed */ if (control_flags.flag_control_rates_enabled) { /* get current rate setpoint */ rates_sp_updated = orb_check (ORB_ID(vehicle_rates_setpoint), vehicle_rates_setpoint_sub); if (rates_sp_updated) { orb_copy(ORB_ID(vehicle_rates_setpoint), vehicle_rates_setpoint_sub, &rates_sp); } /* apply controller */ rates[0] = att.roll_rate; rates[1] = att.pitch_rate; rates[2] = att.yaw_rate; multirotor_control_rates(&rates_sp, rates, &actuators, reset_integral); } else { /* rates controller disabled, set actuators to zero for safety */ actuators.control[0] = 0.0f; actuators.control[1] = 0.0f; actuators.control[2] = 0.0f; actuators.control[3] = 0.0f; } /* fill in manual control values */ actuators.control[4] = manual.flaps; //actuators.control[5] = manual.aux1; //actuators.control[6] = manual.aux2; //actuators.control[7] = manual.aux3; orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); } fprintf (stdout, "INFO: Attitude controller exiting, disarming motors\n"); fflush (stdout); /* kill all outputs */ for (i = 0; i < NUM_ACTUATOR_CONTROLS; i++) actuators.control[i] = 0.0f; orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); /* * do unsubscriptions */ orb_unsubscribe(ORB_ID(vehicle_attitude), vehicle_attitude_sub, pthread_self()); orb_unsubscribe(ORB_ID(parameter_update), parameter_update_sub, pthread_self()); orb_unsubscribe(ORB_ID(vehicle_attitude_setpoint), vehicle_attitude_setpoint_sub, pthread_self()); orb_unsubscribe(ORB_ID(offboard_control_setpoint), offboard_control_setpoint_sub, pthread_self()); orb_unsubscribe(ORB_ID(vehicle_control_flags), vehicle_control_flags_sub, pthread_self()); orb_unsubscribe(ORB_ID(manual_control_setpoint), manual_control_setpoint_sub, pthread_self()); orb_unsubscribe(ORB_ID(sensor_combined), sensor_combined_sub, pthread_self()); orb_unsubscribe(ORB_ID(vehicle_rates_setpoint), vehicle_rates_setpoint_sub, pthread_self()); orb_unsubscribe(ORB_ID(vehicle_status), vehicle_status_sub, pthread_self()); /* * do unadvertises */ orb_unadvertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, pthread_self()); orb_unadvertise(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, pthread_self()); orb_unadvertise(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, pthread_self()); return 0; }