int set_hil_on_off(bool hil_enabled) { int ret = OK; /* Enable HIL */ if (hil_enabled && !mavlink_hil_enabled) { /* Advertise topics */ pub_hil_attitude = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude); pub_hil_global_pos = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos); /* sensore level hil */ pub_hil_sensors = orb_advertise(ORB_ID(sensor_combined), &hil_sensors); pub_hil_gps = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps); mavlink_hil_enabled = true; /* ramp up some HIL-related subscriptions */ unsigned hil_rate_interval; if (baudrate < 19200) { /* 10 Hz */ hil_rate_interval = 100; } else if (baudrate < 38400) { /* 10 Hz */ hil_rate_interval = 100; } else if (baudrate < 115200) { /* 20 Hz */ hil_rate_interval = 50; } else { /* 200 Hz */ hil_rate_interval = 5; } orb_set_interval(mavlink_subs.spa_sub, hil_rate_interval); set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, hil_rate_interval); } if (!hil_enabled && mavlink_hil_enabled) { mavlink_hil_enabled = false; orb_set_interval(mavlink_subs.spa_sub, 200); } else { ret = ERROR; } return ret; }
int set_hil_on_off(bool hil_enabled) { int ret = OK; /* Enable HIL */ if (hil_enabled && !mavlink_hil_enabled) { //printf("\n HIL ON \n"); /* Advertise topics */ pub_hil_attitude = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude); pub_hil_global_pos = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos); printf("\n pub_hil_attitude :%i\n", pub_hil_attitude); printf("\n pub_hil_global_pos :%i\n", pub_hil_global_pos); mavlink_hil_enabled = true; /* ramp up some HIL-related subscriptions */ unsigned hil_rate_interval; if (baudrate < 19200) { /* 10 Hz */ hil_rate_interval = 100; } else if (baudrate < 38400) { /* 10 Hz */ hil_rate_interval = 100; } else if (baudrate < 115200) { /* 20 Hz */ hil_rate_interval = 50; } else if (baudrate < 460800) { /* 50 Hz */ hil_rate_interval = 20; } else { /* 100 Hz */ hil_rate_interval = 10; } orb_set_interval(mavlink_subs.spa_sub, hil_rate_interval); } if (!hil_enabled && mavlink_hil_enabled) { mavlink_hil_enabled = false; orb_set_interval(mavlink_subs.spa_sub, 200); } else { ret = ERROR; } return ret; }
/** * Main execution thread */ int unibo_ECF_standard_thread_main(int argc, char *argv[]) { warnx("[unibo_ECF_standard] starting\n"); thread_running = true; warnx("Hello Sky!\n"); model = ECF_stand_q(); //Init model! /* subscribe sensor measurements from sensor_combined */ struct sensor_combined_s sens_mes; int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined)); /* set data to 1Hz */ orb_set_interval(sensor_sub_fd, 3); //1000 = 1Hz (ms) /* advertise attitude topic */ struct vehicle_attitude_s ECF_out; memset(&ECF_out, 0, sizeof(ECF_out)); int att_pub_fd = orb_advertise(ORB_ID(vehicle_attitude), &ECF_out); /* one could wait for multiple topics with this technique, just using one here */ struct pollfd fds[] = { { .fd = sensor_sub_fd, .events = POLLIN }, /* there could be more file descriptors here, in the form like: * { .fd = other_sub_fd, .events = POLLIN }, */ };
void simpleapp_task() { printf("Initializing /dev/ttyS2...\n"); int uartfd = open("/dev/ttyS2", O_RDWR | O_NOCTTY); // TELEM2 port assert(uartfd >= 0); // manage terminal settings int termios_state_ttyS2; struct termios existing_config_ttyS2; // get existing terminal config and store it. assert((termios_state_ttyS2 = tcgetattr(uartfd, &existing_config_ttyS2)) >= 0); struct termios config_ttyS2; // duplicate into the new config tcgetattr(uartfd, &config_ttyS2); // memcpy(config_ttyS2, existing_config_ttyS2); // clear ONLCR flag config_ttyS2.c_oflag &= ~ONLCR; // set baud rate assert(cfsetispeed(&config_ttyS2, B921600) >= 0 || cfsetospeed(&config_ttyS2, B921600) >= 0); // go ahead and set the config i am setting up assert((termios_state_ttyS2 = tcsetattr(uartfd, TCSANOW, &config_ttyS2)) >= 0); /* subscribe to sensor_combined topic */ int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined)); orb_set_interval(sensor_sub_fd, 2); /* one could wait for multiple topics with this technique, just using one here */ struct pollfd fds[] = { { .fd = sensor_sub_fd, .events = POLLIN }, /* there could be more file descriptors here, in the form like: { .fd = other_sub_fd, .events = POLLIN }, */ };
/** * Main loop function */ void PCA9685::i2cpwm() { if (_mode == IOX_MODE_TEST_OUT) { setPin(0, PCA9685_PWMCENTER); _should_run = true; } else if (_mode == IOX_MODE_OFF) { _should_run = false; } else { if (!_mode_on_initialized) { /* Subscribe to actuator control 2 (payload group for gimbal) */ _actuator_controls_sub = orb_subscribe(ORB_ID(actuator_controls_2)); /* set the uorb update interval lower than the driver pwm interval */ orb_set_interval(_actuator_controls_sub, 1000.0f / PCA9685_PWMFREQ - 5); _mode_on_initialized = true; } /* Read the servo setpoints from the actuator control topics (gimbal) */ bool updated; orb_check(_actuator_controls_sub, &updated); if (updated) { orb_copy(ORB_ID(actuator_controls_2), _actuator_controls_sub, &_actuator_controls); for (int i = 0; i < NUM_ACTUATOR_CONTROLS; i++) { /* Scale the controls to PWM, first multiply by pi to get rad, * the control[i] values are on the range -1 ... 1 */ uint16_t new_value = PCA9685_PWMCENTER + (_actuator_controls.control[i] * M_PI_F * PCA9685_SCALE); DEVICE_DEBUG("%d: current: %u, new %u, control %.2f", i, _current_values[i], new_value, (double)_actuator_controls.control[i]); if (new_value != _current_values[i] && isfinite(new_value) && new_value >= PCA9685_PWMMIN && new_value <= PCA9685_PWMMAX) { /* This value was updated, send the command to adjust the PWM value */ setPin(i, new_value); _current_values[i] = new_value; } } } _should_run = true; } // check if any activity remains, else stop if (!_should_run) { _running = false; return; } // re-queue ourselves to run again later _running = true; work_queue(LPWORK, &_work, (worker_t)&PCA9685::i2cpwm_trampoline, this, _i2cpwm_interval); }
Subscription<T>::Subscription( List<SubscriptionBase *> * list, const struct orb_metadata *meta, unsigned interval) : T(), // initialize data structure to zero SubscriptionBase(list, meta) { setHandle(orb_subscribe(getMeta())); orb_set_interval(getHandle(), interval); }
int att_read_simple_main(int argc, char *argv[]) { PX4_INFO("Hello Sky!"); int att = orb_subscribe(ORB_ID(vehicle_attitude)); orb_set_interval(att, 1000); px4_pollfd_struct_t fds[] = { { .fd = att, .events = POLLIN }, };
// thread principale con loop int unibo_motor_output_thread_main(int argc, char *argv[]) { int count=0; warnx("[unibo_motor_output] starting\n"); unibomo_thread_running = true; // subscribe a ORB int motor_output_fd = orb_subscribe(ORB_ID(motor_output)); orb_set_interval(motor_output_fd, 3); struct pollfd fds[] = { { .fd = motor_output_fd, .events = POLLIN } };
int test_main(int argc, char *argv[]) { printf("Hello There!\n"); // TESTING for ADC input int adc_prox_sub_fd = orb_subscribe(ORB_ID(adc_prox)); orb_set_interval(adc_prox_sub_fd, 100); struct pollfd fds[] = { {.fd=adc_prox_sub_fd, .events =POLLIN}, };
int InputMavlinkCmdMount::initialize() { if ((_vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command))) < 0) { return -errno; } // rate-limit inputs to 100Hz. If we don't do this and the output is configured to mavlink mode, // it will publish vehicle_command's as well, causing the input poll() in here to return // immediately, which in turn will cause an output update and thus a busy loop. orb_set_interval(_vehicle_command_sub, 10); return 0; }
int uuv_example_app_main(int argc, char *argv[]) { PX4_INFO("auv_hippocampus_example_app has been started!"); /* subscribe to sensor_combined topic */ int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined)); /* limit the update rate to 5 Hz */ orb_set_interval(sensor_sub_fd, 200); /* subscribe to control_state topic */ int vehicle_attitude_sub_fd = orb_subscribe(ORB_ID(vehicle_attitude)); /* limit the update rate to 5 Hz */ orb_set_interval(vehicle_attitude_sub_fd, 200); /* advertise to actuator_control topic */ struct actuator_controls_s act; memset(&act, 0, sizeof(act)); orb_advert_t act_pub = orb_advertise(ORB_ID(actuator_controls_0), &act); /* one could wait for multiple topics with this technique, just using one here */ px4_pollfd_struct_t fds[] = { { .fd = sensor_sub_fd, .events = POLLIN }, { .fd = vehicle_attitude_sub_fd, .events = POLLIN },
void listener(const orb_id_t &id, unsigned num_msgs, unsigned topic_instance, unsigned topic_interval) { if (orb_exists(id, topic_instance) != 0) { printf("never published\n"); return; } int sub = orb_subscribe_multi(id, topic_instance); orb_set_interval(sub, topic_interval); bool updated = false; unsigned i = 0; hrt_abstime start_time = hrt_absolute_time(); while (i < num_msgs) { orb_check(sub, &updated); if (i == 0) { updated = true; } else { usleep(500); } if (updated) { start_time = hrt_absolute_time(); i++; printf("\nTOPIC: %s instance %d #%d\n", id->o_name, topic_instance, i); T container; if (orb_copy(id, sub, &container) == PX4_OK) { print_message(container); } else { PX4_ERR("orb_copy failed"); } } else { if (hrt_elapsed_time(&start_time) > 2 * 1000 * 1000) { printf("Waited for 2 seconds without a message. Giving up.\n"); break; } } } orb_unsubscribe(sub); }
int helloSky_main(int argc, char *argv[]) { printf("Hello Sky!\n"); /* subscribe to sensor_combined topic */ int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined)); // set up the discretization time orb_set_interval(sensor_sub_fd, 1000); /* one could wait for multiple topics with this technique, just using one here */ struct pollfd fds[] = { { .fd = sensor_sub_fd, .events = POLLIN }, /* there could be more file descriptors here, in the form like: * { .fd = other_sub_fd, .events = POLLIN }, */ };
SubscriptionBase::SubscriptionBase(const struct orb_metadata *meta, unsigned interval, unsigned instance) : _meta(meta), _instance(instance), _handle() { if (_instance > 0) { _handle = orb_subscribe_multi( getMeta(), instance); } else { _handle = orb_subscribe(getMeta()); } if (_handle < 0) { warnx("sub failed"); } orb_set_interval(getHandle(), interval); }
int px4_simple_app_main(int argc, char *argv[]) { printf("Hello Sky!\n"); /* subscribe to sensor_combined topic */ int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined)); orb_set_interval(sensor_sub_fd, 1000); /* advertise attitude topic */ struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); orb_advert_t att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att); /* one could wait for multiple topics with this technique, just using one here */ struct pollfd fds[] = { { .fd = sensor_sub_fd, .events = POLLIN }, /* there could be more file descriptors here, in the form like: * { .fd = other_sub_fd, .events = POLLIN }, */ };
int wastegate_ctrl_main(int argc, char *argv[]) { PX4_INFO("Starting Fireblade Wastegate Control Application"); /* subscribe to sensor_combined topic */ int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_wastegate)); orb_set_interval(sensor_sub_fd, 1000); /* advertise attitude topic */ struct actuator_wastegate_s act; memset(&act, 0, sizeof(act)); orb_advert_t act_pub = orb_advertise(ORB_ID(actuator_wastegate), &act); /* one could wait for multiple topics with this technique, just using one here */ px4_pollfd_struct_t fds[] = { { .fd = sensor_sub_fd, .events = POLLIN }, /* there could be more file descriptors here, in the form like: * { .fd = other_sub_fd, .events = POLLIN }, */ };
static int rpgRateControllerThreadMain(int argc, char *argv[]) { struct imu_msg_s imu_msg; memset(&imu_msg, 0, sizeof(imu_msg)); struct offboard_control_setpoint_s offboard_sp; memset(&offboard_sp, 0, sizeof(offboard_sp)); struct offboard_control_setpoint_s laird_sp; memset(&laird_sp, 0, sizeof(laird_sp)); struct torques_and_thrust_s desired_torques_and_thrust; memset(&desired_torques_and_thrust, 0, sizeof(desired_torques_and_thrust)); int param_sub = orb_subscribe(ORB_ID(parameter_update)); int offboard_setpoint_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); int laird_sub = orb_subscribe(ORB_ID(laird_control_setpoint)); int imu_sub = orb_subscribe(ORB_ID(imu_msg)); orb_advert_t rotor_thrusts_pub = orb_advertise(ORB_ID(torques_and_thrust), &desired_torques_and_thrust); // Limit this loop frequency to 200Hz orb_set_interval(imu_sub, 5); struct pollfd fds[2] = { {.fd = imu_sub, .events = POLLIN}, {.fd = param_sub, .events = POLLIN}};
int px4_simple_main(int argc, char *argv[]) { PX4_INFO("Hello Sky!"); //ORB_DECLARE(distance_around); /* subscribe to sensor_combined topic */ int sonardata_sub_fd = orb_subscribe(ORB_ID(SonarGroupDistance)); orb_set_interval(sonardata_sub_fd, 1000); /* advertise attitude topic */ struct SonarGroupDistance_s sonardata; memset(&sonardata, 0, sizeof(sonardata)); /* one could wait for multiple topics with this technique, just using one here */ px4_pollfd_struct_t fds[] = { { .fd = sonardata_sub_fd, .events = POLLIN }, // { .fd = distancearound_sub_fd, .events = POLLIN }, /* there could be more file descriptors here, in the form like: * { .fd = other_sub_fd, .events = POLLIN }, */ };
/** * Main execution thread */ int unibo_trajectory_ref_thread_main(int argc, char *argv[]) //int simple_test_app_main(int argc, char *argv[]) { warnx("[unibo_trajectory_ref] starting\n"); thread_running = true; model = TRAJECTORY_GENERATOR_APP(); //Init model! /* subscribe to attitude topic */ int vehicle_attitude_fd = orb_subscribe(ORB_ID(vehicle_attitude)); /* subscribe to joystick topic */ int joystick_fd = orb_subscribe(ORB_ID(unibo_joystick)); orb_set_interval(joystick_fd, 10); //1000 = 1Hz (ms) 100 HZ!! /* subscribe to position topic */ int local_position_fd = orb_subscribe(ORB_ID(vehicle_local_position)); /* subscribe to parameters topic */ int unibo_parameters_fd = orb_subscribe(ORB_ID(unibo_parameters)); /* advertise reference topic */ struct unibo_reference_s reference; memset(&reference, 0, sizeof(reference)); int reference_pub_fd = orb_advertise(ORB_ID(unibo_reference), &reference); /* advertise local_pos_setpoint topic */ struct vehicle_local_position_setpoint_s setpoint; memset(&setpoint, 0, sizeof(setpoint)); int setpoint_pub_fd = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &setpoint); /* advertise local_pos_setpoint topic */ struct position_setpoint_triplet_s setpoint_triplet; memset(&setpoint_triplet, 0, sizeof(setpoint_triplet)); int setpoint_triplet_pub_fd = orb_advertise(ORB_ID(position_setpoint_triplet), &setpoint_triplet); /* one could wait for multiple topics with this technique, just using one here */ // struct pollfd fds[] = { // { .fd = joystick_fd, .events = POLLIN }, //CAMBIARE in joystick // /* there could be more file descriptors here, in the form like: // * { .fd = other_sub_fd, .events = POLLIN }, // */ // }; // // int error_counter = 0; /* * |-----------------------------------------------------| * | INIT VARIABLES! | * |-----------------------------------------------------| */ // inizializzazione middle-layer struct vehicle_attitude_s ahrs; struct unibo_joystick_s joystick; struct vehicle_local_position_s position; struct unibo_parameters_s param; static uint64_t time_pre = 0; static uint64_t nowT = 0; float deltaT; TRAJ_start(); TRAJ_control(); /* Bool for topics update */ bool updated; float yawoffset = 0; //offset between onboard yaw and optitrack yaw /* * |-----------------------------------------------------| * | MAIN THREAD LOOP! | * |-----------------------------------------------------| */ while (!thread_should_exit) { //TECNICAMENTE SIAMO GIA' A 50HZ, freq del topic di joystick (default 50HZ) nowT = hrt_absolute_time(); deltaT = (nowT-time_pre)/1000.0f; if (deltaT>=20){ //50 Hz time_pre = nowT; //warnx("Trajectory APP: deltaT = %.2f",deltaT); //milliseconds deltaT, should be 20 /* copy raw JOYSTICK data into local buffer */ orb_check(joystick_fd, &updated); if (updated){ orb_copy(ORB_ID(unibo_joystick), joystick_fd, &joystick); for (int i = 0; i<4;i++){ TRAJECTORY_GENERATOR_APP_U.JOYSTICK[i] = joystick.axis[i]; //position and yaw command } TRAJECTORY_GENERATOR_APP_U.JOYSTICK[4] = joystick.buttons; //button } //warnx("Joystick packet received: CH1: %d - CH2: %d - CH3: %d - CH4: %d BUTTON: %d",joystick.axis[0],joystick.axis[1],joystick.axis[2],joystick.axis[3],joystick.buttons); // orb_check(unibo_parameters_fd, &updated); //TODO // if (updated){ // orb_copy(ORB_ID(unibo_parameters), unibo_parameters_fd, ¶m); // yawoffset = param.in24; // } /* copy attitude into local buffer */ orb_check(vehicle_attitude_fd, &updated); if (updated){ orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_fd, &ahrs); TRAJECTORY_GENERATOR_APP_U.PSI = ahrs.yaw; } /* copy position data into local buffer */ //TODO change to local position for compatibility orb_check(local_position_fd, &updated); if (updated){ orb_copy(ORB_ID(vehicle_local_position), local_position_fd, &position); TRAJECTORY_GENERATOR_APP_U.Position [0] = position.x; TRAJECTORY_GENERATOR_APP_U.Position [1] = position.y; TRAJECTORY_GENERATOR_APP_U.Position [2] = position.z; } TRAJECTORY_GENERATOR_APP_U.BODY_INERT = true; //position references in body frame (take into account actual yaw) TRAJECTORY_GENERATOR_APP_U.TSTAMP = 0; //TODO add timestamp /* * |-----------------------------------------------------| * | EXECUTION LOOP! | * |-----------------------------------------------------| */ // ----------- EXECUTION ----------- TRAJ_control(); /* Fill References structure and write into topic*/ reference.p_x = TRAJECTORY_GENERATOR_APP_Y.REF_POS[0]; //Position reference.p_y = TRAJECTORY_GENERATOR_APP_Y.REF_POS[1]; reference.p_z = TRAJECTORY_GENERATOR_APP_Y.REF_POS[2]; reference.dp_x = TRAJECTORY_GENERATOR_APP_Y.REF_POS[3]; //Velocity reference.dp_y = TRAJECTORY_GENERATOR_APP_Y.REF_POS[4]; reference.dp_z = TRAJECTORY_GENERATOR_APP_Y.REF_POS[5]; reference.ddp_x = TRAJECTORY_GENERATOR_APP_Y.REF_POS[6]; //Acceleration reference.ddp_y = TRAJECTORY_GENERATOR_APP_Y.REF_POS[7]; reference.ddp_z = TRAJECTORY_GENERATOR_APP_Y.REF_POS[8]; // reference.d3p_x = 0; //Jerk // reference.d3p_y = 0; // reference.d3p_z = 0; // reference.d4p_x = 0; //Snap // reference.d4p_y = 0; // reference.d4p_z = 0; reference.d3p_x = TRAJECTORY_GENERATOR_APP_Y.REF_POS[9]; //Jerk reference.d3p_y = TRAJECTORY_GENERATOR_APP_Y.REF_POS[10]; reference.d3p_z = TRAJECTORY_GENERATOR_APP_Y.REF_POS[11]; reference.d4p_x = TRAJECTORY_GENERATOR_APP_Y.REF_POS[12]; //Snap reference.d4p_y = TRAJECTORY_GENERATOR_APP_Y.REF_POS[13]; reference.d4p_z = TRAJECTORY_GENERATOR_APP_Y.REF_POS[14]; reference.psi = TRAJECTORY_GENERATOR_APP_Y.REF_YAW[0]; //Yaw reference.d_psi = TRAJECTORY_GENERATOR_APP_Y.REF_YAW[1]; reference.dd_psi = TRAJECTORY_GENERATOR_APP_Y.REF_YAW[2]; reference.button = TRAJECTORY_GENERATOR_APP_Y.REF_BUTTONS; //Button reference.timestamp = TRAJECTORY_GENERATOR_APP_Y.REF_TSTAMP; //Tstamp /*End Filling Reference */ //warnx("Posx: %.2f - Velx: %.2f - Accx: %.2f - Jerk: %.2f - Snap: %.2f", reference.p_x, reference.dp_x, reference.ddp_x, TRAJECTORY_GENERATOR_APP_Y.REF_POS[9],TRAJECTORY_GENERATOR_APP_Y.REF_POS[12]); //publishing references orb_publish(ORB_ID(unibo_reference), reference_pub_fd, &reference); //warnx("Actual yaw: %.3f - YawREF: %.3f - DYawREF: %.3f - D2YawREF: %.3f", TRAJECTORY_GENERATOR_APP_U.PSI, reference.psi, reference.d_psi, reference.dd_psi); setpoint.x = reference.p_x; //publish in vehicle_setpoint setpoint.y = reference.p_y; setpoint.z = reference.p_z; setpoint.yaw = reference.psi; orb_publish(ORB_ID(vehicle_local_position_setpoint), setpoint_pub_fd, &setpoint); setpoint_triplet.current.type = SETPOINT_TYPE_POSITION; //publish in setpoint_triplet setpoint_triplet.nav_state = 0; //NAV_STATE_NONE; setpoint_triplet.current.lat = reference.p_x; //lat-->x setpoint_triplet.current.lon = reference.p_y; //lon-->y setpoint_triplet.current.alt = reference.p_z; //alt-->z setpoint_triplet.current.yaw = reference.psi; // IN MAVLINK MESSAGES VENGONO MANIPOLATI. ES: *1e7, lo yaw è trasformato, etc,...TODO check setpoint_triplet.previous.valid = true; setpoint_triplet.current.valid = true; setpoint_triplet.next.valid = true; orb_publish(ORB_ID(position_setpoint_triplet), setpoint_triplet_pub_fd, &setpoint_triplet); } usleep(10000); } warnx("[unibo_trajectory_ref_daemon] exiting.\n"); thread_running = false; return 0; }
pthread_t uorb_receive_start(void) { /* --- SENSORS RAW VALUE --- */ mavlink_subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); /* rate limit set externally based on interface speed, set a basic default here */ orb_set_interval(mavlink_subs.sensor_sub, 200); /* 5Hz updates */ /* --- ATTITUDE VALUE --- */ mavlink_subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); /* rate limit set externally based on interface speed, set a basic default here */ orb_set_interval(mavlink_subs.att_sub, 200); /* 5Hz updates */ /* --- GPS VALUE --- */ mavlink_subs.gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); orb_set_interval(mavlink_subs.gps_sub, 200); /* 5Hz updates */ /* --- HOME POSITION --- */ mavlink_subs.home_sub = orb_subscribe(ORB_ID(home_position)); orb_set_interval(mavlink_subs.home_sub, 1000); /* 1Hz updates */ /* --- SYSTEM STATE --- */ status_sub = orb_subscribe(ORB_ID(vehicle_status)); orb_set_interval(status_sub, 300); /* max 3.33 Hz updates */ /* --- RC CHANNELS VALUE --- */ rc_sub = orb_subscribe(ORB_ID(rc_channels)); orb_set_interval(rc_sub, 100); /* 10Hz updates */ /* --- RC RAW VALUE --- */ mavlink_subs.input_rc_sub = orb_subscribe(ORB_ID(input_rc)); orb_set_interval(mavlink_subs.input_rc_sub, 100); /* --- GLOBAL POS VALUE --- */ mavlink_subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); orb_set_interval(mavlink_subs.global_pos_sub, 100); /* 10 Hz active updates */ /* --- LOCAL POS VALUE --- */ mavlink_subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); orb_set_interval(mavlink_subs.local_pos_sub, 1000); /* 1Hz active updates */ /* --- GLOBAL SETPOINT VALUE --- */ mavlink_subs.spg_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint)); orb_set_interval(mavlink_subs.spg_sub, 2000); /* 0.5 Hz updates */ /* --- LOCAL SETPOINT VALUE --- */ mavlink_subs.spl_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); orb_set_interval(mavlink_subs.spl_sub, 2000); /* 0.5 Hz updates */ /* --- ATTITUDE SETPOINT VALUE --- */ mavlink_subs.spa_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); orb_set_interval(mavlink_subs.spa_sub, 2000); /* 0.5 Hz updates */ /* --- RATES SETPOINT VALUE --- */ mavlink_subs.rates_setpoint_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); orb_set_interval(mavlink_subs.rates_setpoint_sub, 2000); /* 0.5 Hz updates */ /* --- ACTUATOR OUTPUTS --- */ mavlink_subs.act_0_sub = orb_subscribe(ORB_ID(actuator_outputs_0)); mavlink_subs.act_1_sub = orb_subscribe(ORB_ID(actuator_outputs_1)); mavlink_subs.act_2_sub = orb_subscribe(ORB_ID(actuator_outputs_2)); mavlink_subs.act_3_sub = orb_subscribe(ORB_ID(actuator_outputs_3)); /* rate limits set externally based on interface speed, set a basic default here */ orb_set_interval(mavlink_subs.act_0_sub, 100); /* 10Hz updates */ orb_set_interval(mavlink_subs.act_1_sub, 100); /* 10Hz updates */ orb_set_interval(mavlink_subs.act_2_sub, 100); /* 10Hz updates */ orb_set_interval(mavlink_subs.act_3_sub, 100); /* 10Hz updates */ /* --- ACTUATOR ARMED VALUE --- */ mavlink_subs.armed_sub = orb_subscribe(ORB_ID(actuator_armed)); orb_set_interval(mavlink_subs.armed_sub, 100); /* 10Hz updates */ /* --- MAPPED MANUAL CONTROL INPUTS --- */ mavlink_subs.man_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); /* rate limits set externally based on interface speed, set a basic default here */ orb_set_interval(mavlink_subs.man_control_sp_sub, 100); /* 10Hz updates */ /* --- ACTUATOR CONTROL VALUE --- */ mavlink_subs.actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE); orb_set_interval(mavlink_subs.actuators_sub, 100); /* 10Hz updates */ /* --- DEBUG VALUE OUTPUT --- */ mavlink_subs.debug_key_value = orb_subscribe(ORB_ID(debug_key_value)); orb_set_interval(mavlink_subs.debug_key_value, 100); /* 10Hz updates */ /* --- FLOW SENSOR --- */ mavlink_subs.optical_flow = orb_subscribe(ORB_ID(optical_flow)); orb_set_interval(mavlink_subs.optical_flow, 200); /* 5Hz updates */ /* start the listener loop */ pthread_attr_t uorb_attr; pthread_attr_init(&uorb_attr); /* Set stack size, needs less than 2k */ pthread_attr_setstacksize(&uorb_attr, 2048); pthread_t thread; pthread_create(&thread, &uorb_attr, uorb_receive_thread, NULL); return thread; }
calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]) { calibrate_return result = calibrate_return_ok; mag_worker_data_t worker_data; worker_data.mavlink_fd = mavlink_fd; worker_data.done_count = 0; worker_data.calibration_points_perside = 40; worker_data.calibration_interval_perside_seconds = 20; worker_data.calibration_interval_perside_useconds = worker_data.calibration_interval_perside_seconds * 1000 * 1000; // Collect: Right-side up, Left Side, Nose down worker_data.side_data_collected[DETECT_ORIENTATION_RIGHTSIDE_UP] = false; worker_data.side_data_collected[DETECT_ORIENTATION_LEFT] = false; worker_data.side_data_collected[DETECT_ORIENTATION_NOSE_DOWN] = false; worker_data.side_data_collected[DETECT_ORIENTATION_TAIL_DOWN] = false; worker_data.side_data_collected[DETECT_ORIENTATION_UPSIDE_DOWN] = false; worker_data.side_data_collected[DETECT_ORIENTATION_RIGHT] = false; for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) { // Initialize to no subscription worker_data.sub_mag[cur_mag] = -1; // Initialize to no memory allocated worker_data.x[cur_mag] = NULL; worker_data.y[cur_mag] = NULL; worker_data.z[cur_mag] = NULL; worker_data.calibration_counter_total[cur_mag] = 0; } const unsigned int calibration_points_maxcount = calibration_sides * worker_data.calibration_points_perside; char str[30]; for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) { worker_data.x[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount)); worker_data.y[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount)); worker_data.z[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount)); if (worker_data.x[cur_mag] == NULL || worker_data.y[cur_mag] == NULL || worker_data.z[cur_mag] == NULL) { mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: out of memory"); result = calibrate_return_error; } } // Setup subscriptions to mag sensors if (result == calibrate_return_ok) { for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) { if (device_ids[cur_mag] != 0) { // Mag in this slot is available worker_data.sub_mag[cur_mag] = orb_subscribe_multi(ORB_ID(sensor_mag), cur_mag); if (worker_data.sub_mag[cur_mag] < 0) { mavlink_and_console_log_critical(mavlink_fd, "[cal] Mag #%u not found, abort", cur_mag); result = calibrate_return_error; break; } } } } // Limit update rate to get equally spaced measurements over time (in ms) if (result == calibrate_return_ok) { for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) { if (device_ids[cur_mag] != 0) { // Mag in this slot is available unsigned int orb_interval_msecs = (worker_data.calibration_interval_perside_useconds / 1000) / worker_data.calibration_points_perside; //mavlink_and_console_log_info(mavlink_fd, "Orb interval %u msecs", orb_interval_msecs); orb_set_interval(worker_data.sub_mag[cur_mag], orb_interval_msecs); } } } if (result == calibrate_return_ok) { int cancel_sub = calibrate_cancel_subscribe(); result = calibrate_from_orientation(mavlink_fd, // Mavlink fd to write output cancel_sub, // Subscription to vehicle_command for cancel support worker_data.side_data_collected, // Sides to calibrate mag_calibration_worker, // Calibration worker &worker_data, // Opaque data for calibration worked true); // true: lenient still detection calibrate_cancel_unsubscribe(cancel_sub); } // Close subscriptions for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) { if (worker_data.sub_mag[cur_mag] >= 0) { px4_close(worker_data.sub_mag[cur_mag]); } } // Calculate calibration values for each mag float sphere_x[max_mags]; float sphere_y[max_mags]; float sphere_z[max_mags]; float sphere_radius[max_mags]; // Sphere fit the data to get calibration values if (result == calibrate_return_ok) { for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) { if (device_ids[cur_mag] != 0) { // Mag in this slot is available and we should have values for it to calibrate sphere_fit_least_squares(worker_data.x[cur_mag], worker_data.y[cur_mag], worker_data.z[cur_mag], worker_data.calibration_counter_total[cur_mag], 100, 0.0f, &sphere_x[cur_mag], &sphere_y[cur_mag], &sphere_z[cur_mag], &sphere_radius[cur_mag]); if (!PX4_ISFINITE(sphere_x[cur_mag]) || !PX4_ISFINITE(sphere_y[cur_mag]) || !PX4_ISFINITE(sphere_z[cur_mag])) { mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: NaN in sphere fit for mag #%u", cur_mag); result = calibrate_return_error; } } } } // Print uncalibrated data points if (result == calibrate_return_ok) { printf("RAW DATA:\n--------------------\n"); for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) { printf("RAW: MAG %u with %u samples:\n", (unsigned)cur_mag, (unsigned)worker_data.calibration_counter_total[cur_mag]); for (size_t i = 0; i < worker_data.calibration_counter_total[cur_mag]; i++) { float x = worker_data.x[cur_mag][i]; float y = worker_data.y[cur_mag][i]; float z = worker_data.z[cur_mag][i]; printf("%8.4f, %8.4f, %8.4f\n", (double)x, (double)y, (double)z); } printf(">>>>>>>\n"); } printf("CALIBRATED DATA:\n--------------------\n"); for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) { printf("Calibrated: MAG %u with %u samples:\n", (unsigned)cur_mag, (unsigned)worker_data.calibration_counter_total[cur_mag]); for (size_t i = 0; i < worker_data.calibration_counter_total[cur_mag]; i++) { float x = worker_data.x[cur_mag][i] - sphere_x[cur_mag]; float y = worker_data.y[cur_mag][i] - sphere_y[cur_mag]; float z = worker_data.z[cur_mag][i] - sphere_z[cur_mag]; printf("%8.4f, %8.4f, %8.4f\n", (double)x, (double)y, (double)z); } printf("SPHERE RADIUS: %8.4f\n", (double)sphere_radius[cur_mag]); printf(">>>>>>>\n"); } } // Data points are no longer needed for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) { free(worker_data.x[cur_mag]); free(worker_data.y[cur_mag]); free(worker_data.z[cur_mag]); } if (result == calibrate_return_ok) { for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) { if (device_ids[cur_mag] != 0) { int fd_mag = -1; struct mag_scale mscale; // Set new scale (void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, cur_mag); fd_mag = px4_open(str, 0); if (fd_mag < 0) { mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: unable to open mag device #%u", cur_mag); result = calibrate_return_error; } if (result == calibrate_return_ok) { if (px4_ioctl(fd_mag, MAGIOCGSCALE, (long unsigned int)&mscale) != OK) { mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: failed to get current calibration #%u", cur_mag); result = calibrate_return_error; } } if (result == calibrate_return_ok) { mscale.x_offset = sphere_x[cur_mag]; mscale.y_offset = sphere_y[cur_mag]; mscale.z_offset = sphere_z[cur_mag]; if (px4_ioctl(fd_mag, MAGIOCSSCALE, (long unsigned int)&mscale) != OK) { mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_APPLY_CAL_MSG, cur_mag); result = calibrate_return_error; } } // Mag device no longer needed if (fd_mag >= 0) { px4_close(fd_mag); } if (result == calibrate_return_ok) { bool failed = false; /* set parameters */ (void)sprintf(str, "CAL_MAG%u_ID", cur_mag); failed |= (OK != param_set_no_notification(param_find(str), &(device_ids[cur_mag]))); (void)sprintf(str, "CAL_MAG%u_XOFF", cur_mag); failed |= (OK != param_set_no_notification(param_find(str), &(mscale.x_offset))); (void)sprintf(str, "CAL_MAG%u_YOFF", cur_mag); failed |= (OK != param_set_no_notification(param_find(str), &(mscale.y_offset))); (void)sprintf(str, "CAL_MAG%u_ZOFF", cur_mag); failed |= (OK != param_set_no_notification(param_find(str), &(mscale.z_offset))); (void)sprintf(str, "CAL_MAG%u_XSCALE", cur_mag); failed |= (OK != param_set_no_notification(param_find(str), &(mscale.x_scale))); (void)sprintf(str, "CAL_MAG%u_YSCALE", cur_mag); failed |= (OK != param_set_no_notification(param_find(str), &(mscale.y_scale))); (void)sprintf(str, "CAL_MAG%u_ZSCALE", cur_mag); failed |= (OK != param_set_no_notification(param_find(str), &(mscale.z_scale))); if (failed) { mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG, cur_mag); result = calibrate_return_error; } else { mavlink_and_console_log_info(mavlink_fd, "[cal] mag #%u off: x:%.2f y:%.2f z:%.2f Ga", cur_mag, (double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset); mavlink_and_console_log_info(mavlink_fd, "[cal] mag #%u scale: x:%.2f y:%.2f z:%.2f", cur_mag, (double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale); } } } } } return result; }
int sdlog_thread_main(int argc, char *argv[]) { warnx("starting\n"); if (file_exist(mountpoint) != OK) { errx(1, "logging mount point %s not present, exiting.", mountpoint); } char folder_path[64]; if (create_logfolder(folder_path)) errx(1, "unable to create logging folder, exiting."); /* create sensorfile */ int sensorfile = -1; int actuator_outputs_file = -1; int actuator_controls_file = -1; int sysvector_file = -1; FILE *gpsfile; FILE *blackbox_file; // FILE *vehiclefile; char path_buf[64] = ""; // string to hold the path to the sensorfile warnx("logging to directory %s\n", folder_path); /* set up file path: e.g. /mnt/sdcard/session0001/sensor_combined.bin */ sprintf(path_buf, "%s/%s.bin", folder_path, "sensor_combined"); if (0 == (sensorfile = open(path_buf, O_CREAT | O_WRONLY | O_DSYNC))) { errx(1, "opening %s failed.\n", path_buf); } // /* set up file path: e.g. /mnt/sdcard/session0001/actuator_outputs0.bin */ // sprintf(path_buf, "%s/%s.bin", folder_path, "actuator_outputs0"); // if (0 == (actuator_outputs_file = open(path_buf, O_CREAT | O_WRONLY | O_DSYNC))) { // errx(1, "opening %s failed.\n", path_buf); // } /* set up file path: e.g. /mnt/sdcard/session0001/actuator_controls0.bin */ sprintf(path_buf, "%s/%s.bin", folder_path, "sysvector"); if (0 == (sysvector_file = open(path_buf, O_CREAT | O_WRONLY | O_DSYNC))) { errx(1, "opening %s failed.\n", path_buf); } /* set up file path: e.g. /mnt/sdcard/session0001/actuator_controls0.bin */ sprintf(path_buf, "%s/%s.bin", folder_path, "actuator_controls0"); if (0 == (actuator_controls_file = open(path_buf, O_CREAT | O_WRONLY | O_DSYNC))) { errx(1, "opening %s failed.\n", path_buf); } /* set up file path: e.g. /mnt/sdcard/session0001/gps.txt */ sprintf(path_buf, "%s/%s.txt", folder_path, "gps"); if (NULL == (gpsfile = fopen(path_buf, "w"))) { errx(1, "opening %s failed.\n", path_buf); } int gpsfile_no = fileno(gpsfile); /* set up file path: e.g. /mnt/sdcard/session0001/blackbox.txt */ sprintf(path_buf, "%s/%s.txt", folder_path, "blackbox"); if (NULL == (blackbox_file = fopen(path_buf, "w"))) { errx(1, "opening %s failed.\n", path_buf); } int blackbox_file_no = fileno(blackbox_file); /* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */ /* number of messages */ const ssize_t fdsc = 25; /* Sanity check variable and index */ ssize_t fdsc_count = 0; /* file descriptors to wait for */ struct pollfd fds[fdsc]; struct { struct sensor_combined_s raw; struct vehicle_attitude_s att; struct vehicle_attitude_setpoint_s att_sp; struct actuator_outputs_s act_outputs; struct actuator_controls_s act_controls; struct vehicle_command_s cmd; struct vehicle_local_position_s local_pos; struct vehicle_global_position_s global_pos; struct vehicle_gps_position_s gps_pos; } buf; memset(&buf, 0, sizeof(buf)); struct { int cmd_sub; int sensor_sub; int att_sub; int spa_sub; int act_0_sub; int controls0_sub; int local_pos_sub; int global_pos_sub; int gps_pos_sub; } subs; /* --- MANAGEMENT - LOGGING COMMAND --- */ /* subscribe to ORB for sensors raw */ subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); fds[fdsc_count].fd = subs.cmd_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; /* --- SENSORS RAW VALUE --- */ /* subscribe to ORB for sensors raw */ subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); fds[fdsc_count].fd = subs.sensor_sub; /* rate-limit raw data updates to 200Hz */ orb_set_interval(subs.sensor_sub, 5); fds[fdsc_count].events = POLLIN; fdsc_count++; /* --- ATTITUDE VALUE --- */ /* subscribe to ORB for attitude */ subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); fds[fdsc_count].fd = subs.att_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; /* --- ATTITUDE SETPOINT VALUE --- */ /* subscribe to ORB for attitude setpoint */ /* struct already allocated */ subs.spa_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); fds[fdsc_count].fd = subs.spa_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; /** --- ACTUATOR OUTPUTS --- */ subs.act_0_sub = orb_subscribe(ORB_ID(actuator_outputs_0)); fds[fdsc_count].fd = subs.act_0_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; /* --- ACTUATOR CONTROL VALUE --- */ /* subscribe to ORB for actuator control */ subs.controls0_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); fds[fdsc_count].fd = subs.controls0_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; /* --- LOCAL POSITION --- */ /* subscribe to ORB for local position */ subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); fds[fdsc_count].fd = subs.local_pos_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; /* --- GLOBAL POSITION --- */ /* subscribe to ORB for global position */ subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); fds[fdsc_count].fd = subs.global_pos_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; /* --- GPS POSITION --- */ /* subscribe to ORB for global position */ subs.gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); fds[fdsc_count].fd = subs.gps_pos_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; /* WARNING: If you get the error message below, * then the number of registered messages (fdsc) * differs from the number of messages in the above list. */ if (fdsc_count > fdsc) { warn("WARNING: Not enough space for poll fds allocated. Check %s:%d.\n", __FILE__, __LINE__); fdsc_count = fdsc; } /* * set up poll to block for new data, * wait for a maximum of 1000 ms (1 second) */ // const int timeout = 1000; thread_running = true; int poll_count = 0; starttime = hrt_absolute_time(); while (!thread_should_exit) { // int poll_ret = poll(fds, fdsc_count, timeout); // /* handle the poll result */ // if (poll_ret == 0) { // /* XXX this means none of our providers is giving us data - might be an error? */ // } else if (poll_ret < 0) { // /* XXX this is seriously bad - should be an emergency */ // } else { // int ifds = 0; // if (poll_count % 5000 == 0) { // fsync(sensorfile); // fsync(actuator_outputs_file); // fsync(actuator_controls_file); // fsync(blackbox_file_no); // } // /* --- VEHICLE COMMAND VALUE --- */ // if (fds[ifds++].revents & POLLIN) { // /* copy command into local buffer */ // orb_copy(ORB_ID(vehicle_command), subs.cmd_sub, &buf.cmd); // blackbox_file_bytes += fprintf(blackbox_file, "[%10.4f\tVCMD] CMD #%d [%f\t%f\t%f\t%f\t%f\t%f\t%f]\n", hrt_absolute_time()/1000000.0d, // buf.cmd.command, (double)buf.cmd.param1, (double)buf.cmd.param2, (double)buf.cmd.param3, (double)buf.cmd.param4, // (double)buf.cmd.param5, (double)buf.cmd.param6, (double)buf.cmd.param7); // } // /* --- SENSORS RAW VALUE --- */ // if (fds[ifds++].revents & POLLIN) { // /* copy sensors raw data into local buffer */ // orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.raw); // /* write out */ // sensor_combined_bytes += write(sensorfile, (const char*)&(buf.raw), sizeof(buf.raw)); // } // /* --- ATTITUDE VALUE --- */ // if (fds[ifds++].revents & POLLIN) { // /* copy attitude data into local buffer */ // orb_copy(ORB_ID(vehicle_attitude), subs.att_sub, &buf.att); // } // /* --- VEHICLE ATTITUDE SETPOINT --- */ // if (fds[ifds++].revents & POLLIN) { // /* copy local position data into local buffer */ // orb_copy(ORB_ID(vehicle_attitude_setpoint), subs.spa_sub, &buf.att_sp); // } // /* --- ACTUATOR OUTPUTS 0 --- */ // if (fds[ifds++].revents & POLLIN) { // /* copy actuator data into local buffer */ // orb_copy(ORB_ID(actuator_outputs_0), subs.act_0_sub, &buf.act_outputs); // /* write out */ // // actuator_outputs_bytes += write(actuator_outputs_file, (const char*)&buf.act_outputs, sizeof(buf.act_outputs)); // } // /* --- ACTUATOR CONTROL --- */ // if (fds[ifds++].revents & POLLIN) { // orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs.controls0_sub, &buf.act_controls); // /* write out */ // actuator_controls_bytes += write(actuator_controls_file, (const char*)&buf.act_controls, sizeof(buf.act_controls)); // } // } if (poll_count % 100 == 0) { fsync(sysvector_file); } poll_count++; /* copy sensors raw data into local buffer */ orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.raw); orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs.controls0_sub, &buf.act_controls); /* copy actuator data into local buffer */ orb_copy(ORB_ID(actuator_outputs_0), subs.act_0_sub, &buf.act_outputs); orb_copy(ORB_ID(vehicle_attitude_setpoint), subs.spa_sub, &buf.att_sp); orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos); orb_copy(ORB_ID(vehicle_local_position), subs.local_pos_sub, &buf.local_pos); orb_copy(ORB_ID(vehicle_global_position), subs.global_pos_sub, &buf.global_pos); orb_copy(ORB_ID(vehicle_attitude), subs.att_sub, &buf.att); #pragma pack(push, 1) struct { uint64_t timestamp; //[us] float gyro[3]; //[rad/s] float accel[3]; //[m/s^2] float mag[3]; //[gauss] float baro; //pressure [millibar] float baro_alt; //altitude above MSL [meter] float baro_temp; //[degree celcius] float control[4]; //roll, pitch, yaw [-1..1], thrust [0..1] float actuators[8]; //motor 1-8, in motor units (PWM: 1000-2000,AR.Drone: 0-512) float vbat; //battery voltage in [volt] float adc[3]; //remaining auxiliary ADC ports [volt] float local_position[3]; //tangent plane mapping into x,y,z [m] int32_t gps_raw_position[3]; //latitude [degrees] north, longitude [degrees] east, altitude above MSL [millimeter] float attitude[3]; //pitch, roll, yaw [rad] float rotMatrix[9]; //unitvectors } sysvector = { .timestamp = buf.raw.timestamp, .gyro = {buf.raw.gyro_rad_s[0], buf.raw.gyro_rad_s[1], buf.raw.gyro_rad_s[2]}, .accel = {buf.raw.accelerometer_m_s2[0], buf.raw.accelerometer_m_s2[1], buf.raw.accelerometer_m_s2[2]}, .mag = {buf.raw.magnetometer_ga[0], buf.raw.magnetometer_ga[1], buf.raw.magnetometer_ga[2]}, .baro = buf.raw.baro_pres_mbar, .baro_alt = buf.raw.baro_alt_meter, .baro_temp = buf.raw.baro_temp_celcius, .control = {buf.act_controls.control[0], buf.act_controls.control[1], buf.act_controls.control[2], buf.act_controls.control[3]}, .actuators = {buf.act_outputs.output[0], buf.act_outputs.output[1], buf.act_outputs.output[2], buf.act_outputs.output[3], buf.act_outputs.output[4], buf.act_outputs.output[5], buf.act_outputs.output[6], buf.act_outputs.output[7]}, .vbat = buf.raw.battery_voltage_v, .adc = {buf.raw.adc_voltage_v[0], buf.raw.adc_voltage_v[1], buf.raw.adc_voltage_v[2]}, .local_position = {buf.local_pos.x, buf.local_pos.y, buf.local_pos.z}, .gps_raw_position = {buf.gps_pos.lat, buf.gps_pos.lon, buf.gps_pos.alt}, .attitude = {buf.att.pitch, buf.att.roll, buf.att.yaw}, .rotMatrix = {buf.att.R[0][0], buf.att.R[0][1], buf.att.R[0][2], buf.att.R[1][0], buf.att.R[1][1], buf.att.R[1][2], buf.att.R[2][0], buf.att.R[2][1], buf.att.R[2][2]} }; #pragma pack(pop) sysvector_bytes += write(sysvector_file, (const char*)&sysvector, sizeof(sysvector)); usleep(10000); //10000 corresponds in reality to ca. 76 Hz } fsync(sysvector_file); print_sdlog_status(); warnx("exiting.\n"); close(sensorfile); close(actuator_outputs_file); close(actuator_controls_file); fclose(gpsfile); fclose(blackbox_file); thread_running = false; return 0; } void print_sdlog_status() { unsigned bytes = sysvector_bytes + sensor_combined_bytes + actuator_outputs_bytes + blackbox_file_bytes + actuator_controls_bytes; float mebibytes = bytes / 1024.0f / 1024.0f; float seconds = ((float)(hrt_absolute_time() - starttime)) / 1000000.0f; warnx("wrote %4.2f MiB (average %5.3f MiB/s).\n", (double)mebibytes, (double)(mebibytes / seconds)); }
/* * Nonliner Exciplicit complementary filter (ECF), attitude estimator main function. * * Estimates the attitude once started. * * @param argc number of commandline arguments (plus command name) * @param argv strings containing the arguments */ int unibo_att_esti_ECF_s_thread_main(int argc, char *argv[]) { const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds //! Time constant float dt = 0.005f; /* output euler angles */ float euler[3] = {0.0f, 0.0f, 0.0f}; /* Initialization */ float Rot_matrix[9] = {1.f, 0.0f, 0.0f, 0.0f, 1.f, 0.0f, 0.0f, 0.0f, 1.f }; /**< init: identity matrix */ float acc[3] = {0.0f, 0.0f, 0.0f}; float gyro[3] = {0.0f, 0.0f, 0.0f}; float mag[3] = {0.0f, 0.0f, 0.0f}; warnx("main thread started"); struct sensor_combined_s raw; memset(&raw, 0, sizeof(raw)); //! Initialize attitude vehicle uORB message. struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); struct vehicle_control_mode_s control_mode; memset(&control_mode, 0, sizeof(control_mode)); uint64_t last_data = 0; uint64_t last_measurement = 0; /* subscribe to raw data */ int sub_raw = orb_subscribe(ORB_ID(sensor_combined)); /* rate-limit raw data updates to 333 Hz (sensors app publishes at 200, so this is just paranoid) */ orb_set_interval(sub_raw, 3); /* subscribe to param changes */ int sub_params = orb_subscribe(ORB_ID(parameter_update)); /* subscribe to control mode */ int sub_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode)); /* advertise attitude */ //orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); //orb_advert_t att_pub = -1; orb_advert_t att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att); int loopcounter = 0; int printcounter = 0; thread_running = true; float sensor_update_hz[3] = {0.0f, 0.0f, 0.0f}; // XXX write this out to perf regs /* keep track of sensor updates */ uint32_t sensor_last_count[3] = {0, 0, 0}; uint64_t sensor_last_timestamp[3] = {0, 0, 0}; struct unibo_att_esti_ECF_s_params so3_comp_params; struct unibo_att_esti_ECF_s_param_handles so3_comp_param_handles; /* initialize parameter handles */ parameters_init(&so3_comp_param_handles); parameters_update(&so3_comp_param_handles, &so3_comp_params); uint64_t start_time = hrt_absolute_time(); bool initialized = false; bool state_initialized = false; float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f }; unsigned offset_count = 0; /* register the perf counter */ perf_counter_t so3_comp_loop_perf = perf_alloc(PC_ELAPSED, "unibo_att_esti_ECF_s"); /*----------------------------------------------- * Main loop * ----------------------------------------------*/ while (!thread_should_exit) { struct pollfd fds[2]; fds[0].fd = sub_raw; fds[0].events = POLLIN; fds[1].fd = sub_params; fds[1].events = POLLIN; int ret = poll(fds, 2, 1000); if (ret < 0) { /* XXX this is seriously bad - should be an emergency */ } else if (ret == 0) { /* check if we're in HIL - not getting sensor data is fine then */ orb_copy(ORB_ID(vehicle_control_mode), sub_control_mode, &control_mode); if (!control_mode.flag_system_hil_enabled) { warnx("WARNING: Not getting sensors - sensor app running?"); } } else { /* only update parameters if they changed */ if (fds[1].revents & POLLIN) { /* read from param to clear updated flag */ struct parameter_update_s update; orb_copy(ORB_ID(parameter_update), sub_params, &update); /* update parameters */ parameters_update(&so3_comp_param_handles, &so3_comp_params); } /* only run filter if sensor values changed */ if (fds[0].revents & POLLIN) { /* get latest measurements */ orb_copy(ORB_ID(sensor_combined), sub_raw, &raw); if (!initialized) { gyro_offsets[0] += raw.gyro_rad_s[0]; gyro_offsets[1] += raw.gyro_rad_s[1]; gyro_offsets[2] += raw.gyro_rad_s[2]; offset_count++; if (hrt_absolute_time() > start_time + 3000000l) { initialized = true; gyro_offsets[0] /= offset_count; gyro_offsets[1] /= offset_count; gyro_offsets[2] /= offset_count; warnx("gyro initialized, offsets: %.5f %.5f %.5f", (double)gyro_offsets[0], (double)gyro_offsets[1], (double)gyro_offsets[2]); } } else { perf_begin(so3_comp_loop_perf); /* Calculate data time difference in seconds */ dt = (raw.timestamp - last_measurement) / 1000000.0f; last_measurement = raw.timestamp; uint8_t update_vect[3] = {0, 0, 0}; /* Fill in gyro measurements */ if (sensor_last_timestamp[0] != raw.timestamp) { update_vect[0] = 1; sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]); sensor_last_timestamp[0] = raw.timestamp; } gyro[0] = raw.gyro_rad_s[0] - gyro_offsets[0]; gyro[1] = raw.gyro_rad_s[1] - gyro_offsets[1]; gyro[2] = raw.gyro_rad_s[2] - gyro_offsets[2]; /* update accelerometer measurements */ if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) { update_vect[1] = 1; sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]); sensor_last_timestamp[1] = raw.accelerometer_timestamp; } acc[0] = raw.accelerometer_m_s2[0]; acc[1] = raw.accelerometer_m_s2[1]; acc[2] = raw.accelerometer_m_s2[2]; /* update magnetometer measurements */ if (sensor_last_timestamp[2] != raw.magnetometer_timestamp) { update_vect[2] = 1; sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]); sensor_last_timestamp[2] = raw.magnetometer_timestamp; } mag[0] = raw.magnetometer_ga[0]; mag[1] = raw.magnetometer_ga[1]; mag[2] = raw.magnetometer_ga[2]; /* initialize with good values once we have a reasonable dt estimate */ if (!state_initialized && dt < 0.05f && dt > 0.001f) { state_initialized = true; warnx("state initialized"); } /* do not execute the filter if not initialized */ if (!state_initialized) { continue; } uint64_t timing_start = hrt_absolute_time(); // NOTE : Accelerometer is reversed. // Because proper mount of PX4 will give you a reversed accelerometer readings. standardECFnew(gyro[0], gyro[1], gyro[2], -acc[0], -acc[1], -acc[2], mag[0], mag[1], mag[2], so3_comp_params.Ka, so3_comp_params.Km, so3_comp_params.Kp, so3_comp_params.Ki, dt); // direction cosine matrix of 1-2-3 sequence // Resulted Rotation matrix convert from Inertial frame to body frame Rot_matrix[0] = q0q0 + q1q1 - q2q2 - q3q3; // R11 Rot_matrix[1] = 2.f * (q1*q2 + q0*q3); // R12 Rot_matrix[2] = 2.f * (q1*q3 - q0*q2); // R13 Rot_matrix[3] = 2.f * (q1*q2 - q0*q3); // R21 Rot_matrix[4] = q0q0 - q1q1 + q2q2 - q3q3; // R22 Rot_matrix[5] = 2.f * (q2*q3 + q0*q1); // R23 Rot_matrix[6] = 2.f * (q1*q3 + q0*q2); // R31 Rot_matrix[7] = 2.f * (q2*q3 - q0*q1); // R32 Rot_matrix[8] = q0q0 - q1q1 - q2q2 + q3q3; // R33 // Euler Angles using Unit Quaternions; euler[0]= atan2f((2.0f*(q2q3+q0q1)),1.0f-2.0f*(q1q1+q2q2)); // Roll euler[1]= asinf(-2.0f*(q1q3-q0q2)); // Pitch euler[2]= atan2f((2.0f*(q1q2+q0q3)),1.0f-2.0f*(q2q2+q3q3)); // Yaw /* swap values for next iteration, check for fatal inputs */ if (isfinite(euler[0]) && isfinite(euler[1]) && isfinite(euler[2])) { // Publish only finite euler angles att.roll = euler[0] - so3_comp_params.roll_off; att.pitch = euler[1] - so3_comp_params.pitch_off; att.yaw = euler[2] - so3_comp_params.yaw_off; } else { /* due to inputs or numerical failure the output is invalid, skip it */ // Due to inputs or numerical failure the output is invalid warnx("infinite euler angles, rotation matrix:"); warnx("%.3f %.3f %.3f", (double)Rot_matrix[0], (double)Rot_matrix[1], (double)Rot_matrix[2]); warnx("%.3f %.3f %.3f", (double)Rot_matrix[3], (double)Rot_matrix[4], (double)Rot_matrix[5]); warnx("%.3f %.3f %.3f", (double)Rot_matrix[6], (double)Rot_matrix[7], (double)Rot_matrix[8]); // Don't publish anything continue; } if (last_data > 0 && raw.timestamp > last_data + 12000) { warnx("sensor data missed"); } last_data = raw.timestamp; /* send out */ att.timestamp = raw.timestamp; // Quaternion att.q[0] = q0; att.q[1] = q1; att.q[2] = q2; att.q[3] = q3; att.q_valid = true; // Euler angle rate. But it needs to be investigated again. /* att.rollspeed = 2.0f*(-q1*dq0 + q0*dq1 - q3*dq2 + q2*dq3); att.pitchspeed = 2.0f*(-q2*dq0 + q3*dq1 + q0*dq2 - q1*dq3); att.yawspeed = 2.0f*(-q3*dq0 -q2*dq1 + q1*dq2 + q0*dq3); */ att.rollspeed = gyro[0]; att.pitchspeed = gyro[1]; att.yawspeed = gyro[2]; att.rollacc = 0; att.pitchacc = 0; att.yawacc = 0; /* TODO: Bias estimation required */ memcpy(&att.rate_offsets, &(gyro_bias), sizeof(att.rate_offsets)); /* copy rotation matrix */ memcpy(&att.R, Rot_matrix, sizeof(float)*9); att.R_valid = true; // Publish if (att_pub > 0) { orb_publish(ORB_ID(vehicle_attitude), att_pub, &att); } else { warnx("NaN in roll/pitch/yaw estimate!"); orb_advertise(ORB_ID(vehicle_attitude), &att); } perf_end(so3_comp_loop_perf); } } } loopcounter++; } // here the main loop stop thread_running = false; return 0; }
void Navigator::task_main() { /* inform about start */ warnx("Initializing.."); _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); _geofence.setMavlinkFd(_mavlink_fd); /* Try to load the geofence: * if /fs/microsd/etc/geofence.txt load from this file * else clear geofence data in datamanager */ struct stat buffer; if (stat(GEOFENCE_FILENAME, &buffer) == 0) { warnx("Try to load geofence.txt"); _geofence.loadFromFile(GEOFENCE_FILENAME); } else { mavlink_log_critical(_mavlink_fd, "#audio: No geofence file"); if (_geofence.clearDm() > 0) warnx("Geofence cleared"); else warnx("Could not clear geofence"); } /* do subscriptions */ _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); _gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); _capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _home_pos_sub = orb_subscribe(ORB_ID(home_position)); _onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission)); _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission)); _param_update_sub = orb_subscribe(ORB_ID(parameter_update)); /* copy all topics first time */ vehicle_status_update(); vehicle_control_mode_update(); global_position_update(); gps_position_update(); sensor_combined_update(); home_position_update(); navigation_capabilities_update(); params_update(); /* rate limit position and sensor updates to 50 Hz */ orb_set_interval(_global_pos_sub, 20); orb_set_interval(_sensor_combined_sub, 20); hrt_abstime mavlink_open_time = 0; const hrt_abstime mavlink_open_interval = 500000; /* wakeup source(s) */ struct pollfd fds[8]; /* Setup of loop */ fds[0].fd = _global_pos_sub; fds[0].events = POLLIN; fds[1].fd = _home_pos_sub; fds[1].events = POLLIN; fds[2].fd = _capabilities_sub; fds[2].events = POLLIN; fds[3].fd = _vstatus_sub; fds[3].events = POLLIN; fds[4].fd = _control_mode_sub; fds[4].events = POLLIN; fds[5].fd = _param_update_sub; fds[5].events = POLLIN; fds[6].fd = _sensor_combined_sub; fds[6].events = POLLIN; fds[7].fd = _gps_pos_sub; fds[7].events = POLLIN; while (!_task_should_exit) { /* wait for up to 100ms for data */ int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); if (pret == 0) { /* timed out - periodic check for _task_should_exit, etc. */ continue; } else if (pret < 0) { /* this is undesirable but not much we can do - might want to flag unhappy status */ warn("poll error %d, %d", pret, errno); continue; } perf_begin(_loop_perf); if (_mavlink_fd < 0 && hrt_absolute_time() > mavlink_open_time) { /* try to reopen the mavlink log device with specified interval */ mavlink_open_time = hrt_abstime() + mavlink_open_interval; _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); } static bool have_geofence_position_data = false; /* gps updated */ if (fds[7].revents & POLLIN) { gps_position_update(); if (_geofence.getSource() == Geofence::GF_SOURCE_GPS) { have_geofence_position_data = true; } } /* sensors combined updated */ if (fds[6].revents & POLLIN) { sensor_combined_update(); } /* parameters updated */ if (fds[5].revents & POLLIN) { params_update(); updateParams(); } /* vehicle control mode updated */ if (fds[4].revents & POLLIN) { vehicle_control_mode_update(); } /* vehicle status updated */ if (fds[3].revents & POLLIN) { vehicle_status_update(); } /* navigation capabilities updated */ if (fds[2].revents & POLLIN) { navigation_capabilities_update(); } /* home position updated */ if (fds[1].revents & POLLIN) { home_position_update(); } /* global position updated */ if (fds[0].revents & POLLIN) { global_position_update(); if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS) { have_geofence_position_data = true; } } /* Check geofence violation */ static hrt_abstime last_geofence_check = 0; if (have_geofence_position_data && hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL) { bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter); last_geofence_check = hrt_absolute_time(); have_geofence_position_data = false; if (!inside) { /* inform other apps via the mission result */ _geofence_result.geofence_violated = true; publish_geofence_result(); /* Issue a warning about the geofence violation once */ if (!_geofence_violation_warning_sent) { mavlink_log_critical(_mavlink_fd, "#audio: Geofence violation"); _geofence_violation_warning_sent = true; } } else { /* inform other apps via the mission result */ _geofence_result.geofence_violated = false; publish_geofence_result(); /* Reset the _geofence_violation_warning_sent field */ _geofence_violation_warning_sent = false; } } /* Do stuff according to navigation state set by commander */ switch (_vstatus.nav_state) { case NAVIGATION_STATE_MANUAL: case NAVIGATION_STATE_ACRO: case NAVIGATION_STATE_ALTCTL: case NAVIGATION_STATE_POSCTL: case NAVIGATION_STATE_LAND: case NAVIGATION_STATE_TERMINATION: case NAVIGATION_STATE_OFFBOARD: _navigation_mode = nullptr; _can_loiter_at_sp = false; break; case NAVIGATION_STATE_AUTO_MISSION: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_mission; break; case NAVIGATION_STATE_AUTO_LOITER: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_loiter; break; case NAVIGATION_STATE_AUTO_RCRECOVER: _pos_sp_triplet_published_invalid_once = false; if (_param_rcloss_obc.get() != 0) { _navigation_mode = &_rcLoss; } else { _navigation_mode = &_rtl; } break; case NAVIGATION_STATE_AUTO_RTL: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_rtl; break; case NAVIGATION_STATE_AUTO_RTGS: /* Use complex data link loss mode only when enabled via param * otherwise use rtl */ _pos_sp_triplet_published_invalid_once = false; if (_param_datalinkloss_obc.get() != 0) { _navigation_mode = &_dataLinkLoss; } else { _navigation_mode = &_rtl; } break; case NAVIGATION_STATE_AUTO_LANDENGFAIL: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_engineFailure; break; case NAVIGATION_STATE_AUTO_LANDGPSFAIL: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_gpsFailure; break; default: _navigation_mode = nullptr; _can_loiter_at_sp = false; break; } /* iterate through navigation modes and set active/inactive for each */ for(unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) { _navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i]); } /* if nothing is running, set position setpoint triplet invalid once */ if (_navigation_mode == nullptr && !_pos_sp_triplet_published_invalid_once) { _pos_sp_triplet_published_invalid_once = true; _pos_sp_triplet.previous.valid = false; _pos_sp_triplet.current.valid = false; _pos_sp_triplet.next.valid = false; _pos_sp_triplet_updated = true; } if (_pos_sp_triplet_updated) { publish_position_setpoint_triplet(); _pos_sp_triplet_updated = false; } if (_mission_result_updated) { publish_mission_result(); _mission_result_updated = false; } perf_end(_loop_perf); } warnx("exiting."); _navigator_task = -1; _exit(0); }
void BlinkM::led() { static int vehicle_status_sub_fd; static int vehicle_gps_position_sub_fd; static int num_of_cells = 0; static int detected_cells_runcount = 0; static int t_led_color[8] = { 0, 0, 0, 0, 0, 0, 0, 0}; static int t_led_blink = 0; static int led_thread_runcount=0; static int led_interval = 1000; static int no_data_vehicle_status = 0; static int no_data_vehicle_gps_position = 0; static bool topic_initialized = false; static bool detected_cells_blinked = false; static bool led_thread_ready = true; int num_of_used_sats = 0; if(!topic_initialized) { vehicle_status_sub_fd = orb_subscribe(ORB_ID(vehicle_status)); orb_set_interval(vehicle_status_sub_fd, 1000); vehicle_gps_position_sub_fd = orb_subscribe(ORB_ID(vehicle_gps_position)); orb_set_interval(vehicle_gps_position_sub_fd, 1000); topic_initialized = true; } if(led_thread_ready == true) { if(!detected_cells_blinked) { if(num_of_cells > 0) { t_led_color[0] = LED_PURPLE; } if(num_of_cells > 1) { t_led_color[1] = LED_PURPLE; } if(num_of_cells > 2) { t_led_color[2] = LED_PURPLE; } if(num_of_cells > 3) { t_led_color[3] = LED_PURPLE; } if(num_of_cells > 4) { t_led_color[4] = LED_PURPLE; } t_led_color[5] = LED_OFF; t_led_color[6] = LED_OFF; t_led_color[7] = LED_OFF; t_led_blink = LED_BLINK; } else { t_led_color[0] = led_color_1; t_led_color[1] = led_color_2; t_led_color[2] = led_color_3; t_led_color[3] = led_color_4; t_led_color[4] = led_color_5; t_led_color[5] = led_color_6; t_led_color[6] = led_color_7; t_led_color[7] = led_color_8; t_led_blink = led_blink; } led_thread_ready = false; } if (led_thread_runcount & 1) { if (t_led_blink) setLEDColor(LED_OFF); led_interval = LED_OFFTIME; } else { setLEDColor(t_led_color[(led_thread_runcount / 2) % 8]); //led_interval = (led_thread_runcount & 1) : LED_ONTIME; led_interval = LED_ONTIME; } if (led_thread_runcount == 15) { /* obtained data for the first file descriptor */ struct vehicle_status_s vehicle_status_raw; struct vehicle_gps_position_s vehicle_gps_position_raw; memset(&vehicle_status_raw, 0, sizeof(vehicle_status_raw)); memset(&vehicle_gps_position_raw, 0, sizeof(vehicle_gps_position_raw)); bool new_data_vehicle_status; bool new_data_vehicle_gps_position; orb_check(vehicle_status_sub_fd, &new_data_vehicle_status); if (new_data_vehicle_status) { orb_copy(ORB_ID(vehicle_status), vehicle_status_sub_fd, &vehicle_status_raw); no_data_vehicle_status = 0; } else { no_data_vehicle_status++; if(no_data_vehicle_status >= 3) no_data_vehicle_status = 3; } orb_check(vehicle_gps_position_sub_fd, &new_data_vehicle_gps_position); if (new_data_vehicle_gps_position) { orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub_fd, &vehicle_gps_position_raw); no_data_vehicle_gps_position = 0; } else { no_data_vehicle_gps_position++; if(no_data_vehicle_gps_position >= 3) no_data_vehicle_gps_position = 3; } /* get number of used satellites in navigation */ num_of_used_sats = 0; //for(int satloop=0; satloop<20; satloop++) { for(int satloop=0; satloop<sizeof(vehicle_gps_position_raw.satellite_used); satloop++) { if(vehicle_gps_position_raw.satellite_used[satloop] == 1) { num_of_used_sats++; } } if(new_data_vehicle_status || no_data_vehicle_status < 3){ if(num_of_cells == 0) { /* looking for lipo cells that are connected */ printf("<blinkm> checking cells\n"); for(num_of_cells = 2; num_of_cells < 7; num_of_cells++) { if(vehicle_status_raw.voltage_battery < num_of_cells * MAX_CELL_VOLTAGE) break; } printf("<blinkm> cells found:%u\n", num_of_cells); } else { if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_WARNING) { /* LED Pattern for battery low warning */ led_color_1 = LED_YELLOW; led_color_2 = LED_YELLOW; led_color_3 = LED_YELLOW; led_color_4 = LED_YELLOW; led_color_5 = LED_YELLOW; led_color_6 = LED_YELLOW; led_color_7 = LED_YELLOW; led_color_8 = LED_YELLOW; led_blink = LED_BLINK; } else if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_ALERT) { /* LED Pattern for battery critical alerting */ led_color_1 = LED_RED; led_color_2 = LED_RED; led_color_3 = LED_RED; led_color_4 = LED_RED; led_color_5 = LED_RED; led_color_6 = LED_RED; led_color_7 = LED_RED; led_color_8 = LED_RED; led_blink = LED_BLINK; } else { /* no battery warnings here */ if(vehicle_status_raw.flag_system_armed == false) { /* system not armed */ led_color_1 = LED_RED; led_color_2 = LED_RED; led_color_3 = LED_RED; led_color_4 = LED_RED; led_color_5 = LED_RED; led_color_6 = LED_RED; led_color_7 = LED_RED; led_color_8 = LED_RED; led_blink = LED_NOBLINK; } else { /* armed system - initial led pattern */ led_color_1 = LED_RED; led_color_2 = LED_RED; led_color_3 = LED_RED; led_color_4 = LED_OFF; led_color_5 = LED_OFF; led_color_6 = LED_OFF; led_color_7 = LED_OFF; led_color_8 = LED_OFF; led_blink = LED_BLINK; /* handle 4th led - flightmode indicator */ switch((int)vehicle_status_raw.flight_mode) { case VEHICLE_FLIGHT_MODE_MANUAL: led_color_4 = LED_AMBER; break; case VEHICLE_FLIGHT_MODE_STAB: led_color_4 = LED_YELLOW; break; case VEHICLE_FLIGHT_MODE_HOLD: led_color_4 = LED_BLUE; break; case VEHICLE_FLIGHT_MODE_AUTO: led_color_4 = LED_GREEN; break; } if(new_data_vehicle_gps_position || no_data_vehicle_gps_position < 3) { /* handling used sat´s */ if(num_of_used_sats >= 7) { led_color_1 = LED_OFF; led_color_2 = LED_OFF; led_color_3 = LED_OFF; } else if(num_of_used_sats == 6) { led_color_2 = LED_OFF; led_color_3 = LED_OFF; } else if(num_of_used_sats == 5) { led_color_3 = LED_OFF; } } else { /* no vehicle_gps_position data */ led_color_1 = LED_WHITE; led_color_2 = LED_WHITE; led_color_3 = LED_WHITE; } } } } } else { /* LED Pattern for general Error - no vehicle_status can retrieved */ led_color_1 = LED_WHITE; led_color_2 = LED_WHITE; led_color_3 = LED_WHITE; led_color_4 = LED_WHITE; led_color_5 = LED_WHITE; led_color_6 = LED_WHITE; led_color_7 = LED_WHITE; led_color_8 = LED_WHITE; led_blink = LED_BLINK; } /* printf( "<blinkm> Volt:%8.4f\tArmed:%4u\tMode:%4u\tCells:%4u\tNDVS:%4u\tNDSAT:%4u\tSats:%4u\tFix:%4u\tVisible:%4u\n", vehicle_status_raw.voltage_battery, vehicle_status_raw.flag_system_armed, vehicle_status_raw.flight_mode, num_of_cells, no_data_vehicle_status, no_data_vehicle_gps_position, num_of_used_sats, vehicle_gps_position_raw.fix_type, vehicle_gps_position_raw.satellites_visible); */ led_thread_runcount=0; led_thread_ready = true; led_interval = LED_OFFTIME; if(detected_cells_runcount < 4){ detected_cells_runcount++; } else { detected_cells_blinked = true; } } else { led_thread_runcount++; } if(systemstate_run == true) { /* re-queue ourselves to run again later */ work_queue(LPWORK, &_work, (worker_t)&BlinkM::led_trampoline, this, led_interval); } else { stop_script(); set_rgb(0,0,0); } }
/* * EKF Attitude Estimator main function. * * Estimates the attitude recursively once started. * * @param argc number of commandline arguments (plus command name) * @param argv strings containing the arguments */ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) { const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds float dt = 0.005f; /* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */ float z_k[9] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 9.81f, 0.2f, -0.2f, 0.2f}; /**< Measurement vector */ float x_aposteriori_k[12]; /**< states */ float P_aposteriori_k[144] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 100.0f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 100.0f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 0, 100.0f, }; /**< init: diagonal matrix with big values */ float x_aposteriori[12]; float P_aposteriori[144]; /* output euler angles */ float euler[3] = {0.0f, 0.0f, 0.0f}; float Rot_matrix[9] = {1.f, 0, 0, 0, 1.f, 0, 0, 0, 1.f }; /**< init: identity matrix */ // print text printf("Extended Kalman Filter Attitude Estimator initialized..\n\n"); fflush(stdout); int overloadcounter = 19; /* Initialize filter */ attitudeKalmanfilter_initialize(); /* store start time to guard against too slow update rates */ uint64_t last_run = hrt_absolute_time(); struct sensor_combined_s raw; memset(&raw, 0, sizeof(raw)); struct vehicle_gps_position_s gps; memset(&gps, 0, sizeof(gps)); struct vehicle_global_position_s global_pos; memset(&global_pos, 0, sizeof(global_pos)); struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); struct vehicle_control_mode_s control_mode; memset(&control_mode, 0, sizeof(control_mode)); struct vehicle_vicon_position_s vicon_pos; // Added by Ross Allen memset(&vicon_pos, 0, sizeof(vicon_pos)); uint64_t last_data = 0; uint64_t last_measurement = 0; uint64_t last_vel_t = 0; /* Vicon parameters - Added by Ross Allen */ bool vicon_valid = false; //~ static const hrt_abstime vicon_topic_timeout = 500000; // vicon topic timeout = 0.25s /* current velocity */ math::Vector<3> vel; vel.zero(); /* previous velocity */ math::Vector<3> vel_prev; vel_prev.zero(); /* actual acceleration (by GPS velocity) in body frame */ math::Vector<3> acc; acc.zero(); /* rotation matrix */ math::Matrix<3, 3> R; R.identity(); /* subscribe to raw data */ int sub_raw = orb_subscribe(ORB_ID(sensor_combined)); /* rate-limit raw data updates to 333 Hz (sensors app publishes at 200, so this is just paranoid) */ orb_set_interval(sub_raw, 3); /* subscribe to GPS */ int sub_gps = orb_subscribe(ORB_ID(vehicle_gps_position)); /* subscribe to GPS */ int sub_global_pos = orb_subscribe(ORB_ID(vehicle_global_position)); /* subscribe to param changes */ int sub_params = orb_subscribe(ORB_ID(parameter_update)); /* subscribe to control mode*/ int sub_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode)); /* subscribe to vicon position */ int vehicle_vicon_position_sub = orb_subscribe(ORB_ID(vehicle_vicon_position)); // Added by Ross Allen /* advertise attitude */ orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); int loopcounter = 0; thread_running = true; /* advertise debug value */ // struct debug_key_value_s dbg = { .key = "", .value = 0.0f }; // orb_advert_t pub_dbg = -1; /* keep track of sensor updates */ uint64_t sensor_last_timestamp[3] = {0, 0, 0}; struct attitude_estimator_ekf_params ekf_params; struct attitude_estimator_ekf_param_handles ekf_param_handles; /* initialize parameter handles */ parameters_init(&ekf_param_handles); bool initialized = false; float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f }; /* magnetic declination, in radians */ float mag_decl = 0.0f; /* rotation matrix for magnetic declination */ math::Matrix<3, 3> R_decl; R_decl.identity(); /* register the perf counter */ perf_counter_t ekf_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_ekf"); /* Main loop*/ while (!thread_should_exit) { struct pollfd fds[2]; fds[0].fd = sub_raw; fds[0].events = POLLIN; fds[1].fd = sub_params; fds[1].events = POLLIN; int ret = poll(fds, 2, 1000); /* Added by Ross Allen */ //~ hrt_abstime t = hrt_absolute_time(); bool updated; if (ret < 0) { /* XXX this is seriously bad - should be an emergency */ } else if (ret == 0) { /* check if we're in HIL - not getting sensor data is fine then */ orb_copy(ORB_ID(vehicle_control_mode), sub_control_mode, &control_mode); if (!control_mode.flag_system_hil_enabled) { fprintf(stderr, "[att ekf] WARNING: Not getting sensors - sensor app running?\n"); } } else { /* only update parameters if they changed */ if (fds[1].revents & POLLIN) { /* read from param to clear updated flag */ struct parameter_update_s update; orb_copy(ORB_ID(parameter_update), sub_params, &update); /* update parameters */ parameters_update(&ekf_param_handles, &ekf_params); } /* only run filter if sensor values changed */ if (fds[0].revents & POLLIN) { /* get latest measurements */ orb_copy(ORB_ID(sensor_combined), sub_raw, &raw); bool gps_updated; orb_check(sub_gps, &gps_updated); if (gps_updated) { orb_copy(ORB_ID(vehicle_gps_position), sub_gps, &gps); if (gps.eph < 20.0f && hrt_elapsed_time(&gps.timestamp_position) < 1000000) { mag_decl = math::radians(get_mag_declination(gps.lat / 1e7f, gps.lon / 1e7f)); /* update mag declination rotation matrix */ R_decl.from_euler(0.0f, 0.0f, mag_decl); } } bool global_pos_updated; orb_check(sub_global_pos, &global_pos_updated); if (global_pos_updated) { orb_copy(ORB_ID(vehicle_global_position), sub_global_pos, &global_pos); } if (!initialized) { // XXX disabling init for now initialized = true; // gyro_offsets[0] += raw.gyro_rad_s[0]; // gyro_offsets[1] += raw.gyro_rad_s[1]; // gyro_offsets[2] += raw.gyro_rad_s[2]; // offset_count++; // if (hrt_absolute_time() - start_time > 3000000LL) { // initialized = true; // gyro_offsets[0] /= offset_count; // gyro_offsets[1] /= offset_count; // gyro_offsets[2] /= offset_count; // } } else { perf_begin(ekf_loop_perf); /* Calculate data time difference in seconds */ dt = (raw.timestamp - last_measurement) / 1000000.0f; last_measurement = raw.timestamp; uint8_t update_vect[3] = {0, 0, 0}; /* Fill in gyro measurements */ if (sensor_last_timestamp[0] != raw.timestamp) { update_vect[0] = 1; // sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]); sensor_last_timestamp[0] = raw.timestamp; } z_k[0] = raw.gyro_rad_s[0] - gyro_offsets[0]; z_k[1] = raw.gyro_rad_s[1] - gyro_offsets[1]; z_k[2] = raw.gyro_rad_s[2] - gyro_offsets[2]; /* update accelerometer measurements */ if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) { update_vect[1] = 1; // sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]); sensor_last_timestamp[1] = raw.accelerometer_timestamp; } hrt_abstime vel_t = 0; bool vel_valid = false; if (ekf_params.acc_comp == 1 && gps.fix_type >= 3 && gps.eph < 10.0f && gps.vel_ned_valid && hrt_absolute_time() < gps.timestamp_velocity + 500000) { vel_valid = true; if (gps_updated) { vel_t = gps.timestamp_velocity; vel(0) = gps.vel_n_m_s; vel(1) = gps.vel_e_m_s; vel(2) = gps.vel_d_m_s; } } else if (ekf_params.acc_comp == 2 && gps.eph < 5.0f && global_pos.timestamp != 0 && hrt_absolute_time() < global_pos.timestamp + 20000) { vel_valid = true; if (global_pos_updated) { vel_t = global_pos.timestamp; vel(0) = global_pos.vel_n; vel(1) = global_pos.vel_e; vel(2) = global_pos.vel_d; } } if (vel_valid) { /* velocity is valid */ if (vel_t != 0) { /* velocity updated */ if (last_vel_t != 0 && vel_t != last_vel_t) { float vel_dt = (vel_t - last_vel_t) / 1000000.0f; /* calculate acceleration in body frame */ acc = R.transposed() * ((vel - vel_prev) / vel_dt); } last_vel_t = vel_t; vel_prev = vel; } } else { /* velocity is valid, reset acceleration */ acc.zero(); vel_prev.zero(); last_vel_t = 0; } z_k[3] = raw.accelerometer_m_s2[0] - acc(0); z_k[4] = raw.accelerometer_m_s2[1] - acc(1); z_k[5] = raw.accelerometer_m_s2[2] - acc(2); /* update magnetometer measurements */ if (sensor_last_timestamp[2] != raw.magnetometer_timestamp) { update_vect[2] = 1; // sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]); sensor_last_timestamp[2] = raw.magnetometer_timestamp; } z_k[6] = raw.magnetometer_ga[0]; z_k[7] = raw.magnetometer_ga[1]; z_k[8] = raw.magnetometer_ga[2]; uint64_t now = hrt_absolute_time(); unsigned int time_elapsed = now - last_run; last_run = now; if (time_elapsed > loop_interval_alarm) { //TODO: add warning, cpu overload here // if (overloadcounter == 20) { // printf("CPU OVERLOAD DETECTED IN ATTITUDE ESTIMATOR EKF (%lu > %lu)\n", time_elapsed, loop_interval_alarm); // overloadcounter = 0; // } overloadcounter++; } static bool const_initialized = false; /* initialize with good values once we have a reasonable dt estimate */ if (!const_initialized && dt < 0.05f && dt > 0.001f) { dt = 0.005f; parameters_update(&ekf_param_handles, &ekf_params); /* update mag declination rotation matrix */ if (gps.eph < 20.0f && hrt_elapsed_time(&gps.timestamp_position) < 1000000) { mag_decl = math::radians(get_mag_declination(gps.lat / 1e7f, gps.lon / 1e7f)); } else { mag_decl = ekf_params.mag_decl; } /* update mag declination rotation matrix */ R_decl.from_euler(0.0f, 0.0f, mag_decl); x_aposteriori_k[0] = z_k[0]; x_aposteriori_k[1] = z_k[1]; x_aposteriori_k[2] = z_k[2]; x_aposteriori_k[3] = 0.0f; x_aposteriori_k[4] = 0.0f; x_aposteriori_k[5] = 0.0f; x_aposteriori_k[6] = z_k[3]; x_aposteriori_k[7] = z_k[4]; x_aposteriori_k[8] = z_k[5]; x_aposteriori_k[9] = z_k[6]; x_aposteriori_k[10] = z_k[7]; x_aposteriori_k[11] = z_k[8]; const_initialized = true; } /* do not execute the filter if not initialized */ if (!const_initialized) { continue; } attitudeKalmanfilter(update_vect, dt, z_k, x_aposteriori_k, P_aposteriori_k, ekf_params.q, ekf_params.r, euler, Rot_matrix, x_aposteriori, P_aposteriori); /* swap values for next iteration, check for fatal inputs */ if (isfinite(euler[0]) && isfinite(euler[1]) && isfinite(euler[2])) { memcpy(P_aposteriori_k, P_aposteriori, sizeof(P_aposteriori_k)); memcpy(x_aposteriori_k, x_aposteriori, sizeof(x_aposteriori_k)); } else { /* due to inputs or numerical failure the output is invalid, skip it */ continue; } if (last_data > 0 && raw.timestamp - last_data > 30000) printf("[attitude estimator ekf] sensor data missed! (%llu)\n", raw.timestamp - last_data); last_data = raw.timestamp; /* Check Vicon for attitude data*/ /* Added by Ross Allen */ orb_check(vehicle_vicon_position_sub, &updated); if (updated) { orb_copy(ORB_ID(vehicle_vicon_position), vehicle_vicon_position_sub, &vicon_pos); vicon_valid = vicon_pos.valid; } //~ if (vicon_valid && (t > (vicon_pos.timestamp + vicon_topic_timeout))) { //~ vicon_valid = false; //~ warnx("VICON timeout"); //~ } /* send out */ att.timestamp = raw.timestamp; att.roll = euler[0]; att.pitch = euler[1]; att.yaw = euler[2] + mag_decl; /* Use vicon yaw if valid and just overwrite*/ /* Added by Ross Allen */ if(vicon_valid) { att.yaw = vicon_pos.yaw; } att.rollspeed = x_aposteriori[0]; att.pitchspeed = x_aposteriori[1]; att.yawspeed = x_aposteriori[2]; att.rollacc = x_aposteriori[3]; att.pitchacc = x_aposteriori[4]; att.yawacc = x_aposteriori[5]; att.g_comp[0] = raw.accelerometer_m_s2[0] - acc(0); att.g_comp[1] = raw.accelerometer_m_s2[1] - acc(1); att.g_comp[2] = raw.accelerometer_m_s2[2] - acc(2); /* copy offsets */ memcpy(&att.rate_offsets, &(x_aposteriori[3]), sizeof(att.rate_offsets)); /* magnetic declination */ math::Matrix<3, 3> R_body = (&Rot_matrix[0]); R = R_decl * R_body; /* copy rotation matrix */ memcpy(&att.R[0][0], &R.data[0][0], sizeof(att.R)); att.R_valid = true; if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) { // Broadcast orb_publish(ORB_ID(vehicle_attitude), pub_att, &att); } else { warnx("NaN in roll/pitch/yaw estimate!"); } perf_end(ekf_loop_perf); } } } loopcounter++; } thread_running = false; return 0; }
void PX4FMU::task_main() { /* force a reset of the update rate */ _current_update_rate = 0; _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); _param_sub = orb_subscribe(ORB_ID(parameter_update)); /* advertise the mixed control outputs */ actuator_outputs_s outputs; memset(&outputs, 0, sizeof(outputs)); #ifdef HRT_PPM_CHANNEL // rc input, published to ORB struct rc_input_values rc_in; orb_advert_t to_input_rc = 0; memset(&rc_in, 0, sizeof(rc_in)); rc_in.input_source = RC_INPUT_SOURCE_PX4FMU_PPM; #endif /* initialize PWM limit lib */ pwm_limit_init(&_pwm_limit); update_pwm_rev_mask(); /* loop until killed */ while (!_task_should_exit) { if (_groups_subscribed != _groups_required) { subscribe(); _groups_subscribed = _groups_required; /* force setting update rate */ _current_update_rate = 0; } /* * Adjust actuator topic update rate to keep up with * the highest servo update rate configured. * * We always mix at max rate; some channels may update slower. */ unsigned max_rate = (_pwm_default_rate > _pwm_alt_rate) ? _pwm_default_rate : _pwm_alt_rate; if (_current_update_rate != max_rate) { _current_update_rate = max_rate; int update_rate_in_ms = int(1000 / _current_update_rate); /* reject faster than 500 Hz updates */ if (update_rate_in_ms < 2) { update_rate_in_ms = 2; } /* reject slower than 10 Hz updates */ if (update_rate_in_ms > 100) { update_rate_in_ms = 100; } debug("adjusted actuator update interval to %ums", update_rate_in_ms); for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (_control_subs[i] > 0) { orb_set_interval(_control_subs[i], update_rate_in_ms); } } // set to current max rate, even if we are actually checking slower/faster _current_update_rate = max_rate; } /* sleep waiting for data, stopping to check for PPM * input at 50Hz */ int ret = ::poll(_poll_fds, _poll_fds_num, CONTROL_INPUT_DROP_LIMIT_MS); /* this would be bad... */ if (ret < 0) { log("poll error %d", errno); continue; } else if (ret == 0) { /* timeout: no control data, switch to failsafe values */ // warnx("no PWM: failsafe"); } else { /* get controls for required topics */ unsigned poll_id = 0; for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (_control_subs[i] > 0) { if (_poll_fds[poll_id].revents & POLLIN) { orb_copy(_control_topics[i], _control_subs[i], &_controls[i]); } poll_id++; } } /* can we mix? */ if (_mixers != nullptr) { unsigned num_outputs; switch (_mode) { case MODE_2PWM: num_outputs = 2; break; case MODE_4PWM: num_outputs = 4; break; case MODE_6PWM: num_outputs = 6; break; case MODE_8PWM: num_outputs = 8; break; default: num_outputs = 0; break; } /* do mixing */ outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs, NULL); outputs.timestamp = hrt_absolute_time(); /* iterate actuators */ for (unsigned i = 0; i < num_outputs; i++) { /* last resort: catch NaN and INF */ if ((i >= outputs.noutputs) || !isfinite(outputs.output[i])) { /* * Value is NaN, INF or out of band - set to the minimum value. * This will be clearly visible on the servo status and will limit the risk of accidentally * spinning motors. It would be deadly in flight. */ outputs.output[i] = -1.0f; } } uint16_t pwm_limited[num_outputs]; /* the PWM limit call takes care of out of band errors and constrains */ pwm_limit_calc(_servo_armed, num_outputs, _reverse_pwm_mask, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit); /* output to the servos */ for (unsigned i = 0; i < num_outputs; i++) { up_pwm_servo_set(i, pwm_limited[i]); } /* publish mixed control outputs */ if (_outputs_pub != nullptr) { _outputs_pub = orb_advertise_multi(ORB_ID(actuator_outputs), &outputs, &_actuator_output_topic_instance, ORB_PRIO_DEFAULT); } else { orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &outputs); } } } /* check arming state */ bool updated = false; orb_check(_armed_sub, &updated); if (updated) { orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); /* update the armed status and check that we're not locked down */ bool set_armed = _armed.armed && !_armed.lockdown; if (_servo_armed != set_armed) _servo_armed = set_armed; /* update PWM status if armed or if disarmed PWM values are set */ bool pwm_on = (_armed.armed || _num_disarmed_set > 0); if (_pwm_on != pwm_on) { _pwm_on = pwm_on; up_pwm_servo_arm(pwm_on); } } orb_check(_param_sub, &updated); if (updated) { parameter_update_s pupdate; orb_copy(ORB_ID(parameter_update), _param_sub, &pupdate); update_pwm_rev_mask(); } #ifdef HRT_PPM_CHANNEL // see if we have new PPM input data if (ppm_last_valid_decode != rc_in.timestamp_last_signal) { // we have a new PPM frame. Publish it. rc_in.channel_count = ppm_decoded_channels; if (rc_in.channel_count > RC_INPUT_MAX_CHANNELS) { rc_in.channel_count = RC_INPUT_MAX_CHANNELS; } for (uint8_t i = 0; i < rc_in.channel_count; i++) { rc_in.values[i] = ppm_buffer[i]; } rc_in.timestamp_publication = ppm_last_valid_decode; rc_in.timestamp_last_signal = ppm_last_valid_decode; rc_in.rc_ppm_frame_length = ppm_frame_length; rc_in.rssi = RC_INPUT_RSSI_MAX; rc_in.rc_failsafe = false; rc_in.rc_lost = false; rc_in.rc_lost_frame_count = 0; rc_in.rc_total_frame_count = 0; /* lazily advertise on first publication */ if (to_input_rc == 0) { to_input_rc = orb_advertise(ORB_ID(input_rc), &rc_in); } else { orb_publish(ORB_ID(input_rc), to_input_rc, &rc_in); } } #endif } for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (_control_subs[i] > 0) { ::close(_control_subs[i]); _control_subs[i] = -1; } } ::close(_armed_sub); ::close(_param_sub); /* make sure servos are off */ up_pwm_servo_deinit(); log("stopping"); /* note - someone else is responsible for restoring the GPIO config */ /* tell the dtor that we are exiting */ _task = -1; _exit(0); }
calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub) { calibrate_return result = calibrate_return_ok; mag_worker_data_t worker_data; worker_data.mavlink_log_pub = mavlink_log_pub; worker_data.done_count = 0; worker_data.calibration_points_perside = calibration_total_points / calibration_sides; worker_data.calibration_interval_perside_seconds = calibraton_duration_seconds / calibration_sides; worker_data.calibration_interval_perside_useconds = worker_data.calibration_interval_perside_seconds * 1000 * 1000; // Collect: Right-side up, Left Side, Nose down worker_data.side_data_collected[DETECT_ORIENTATION_RIGHTSIDE_UP] = false; worker_data.side_data_collected[DETECT_ORIENTATION_LEFT] = false; worker_data.side_data_collected[DETECT_ORIENTATION_NOSE_DOWN] = false; worker_data.side_data_collected[DETECT_ORIENTATION_TAIL_DOWN] = true; worker_data.side_data_collected[DETECT_ORIENTATION_UPSIDE_DOWN] = true; worker_data.side_data_collected[DETECT_ORIENTATION_RIGHT] = true; calibration_log_info(mavlink_log_pub, "[cal] %s side done, rotate to a different side", detect_orientation_str(DETECT_ORIENTATION_TAIL_DOWN)); usleep(100000); calibration_log_info(mavlink_log_pub, "[cal] %s side done, rotate to a different side", detect_orientation_str(DETECT_ORIENTATION_TAIL_DOWN)); usleep(100000); calibration_log_info(mavlink_log_pub, "[cal] %s side done, rotate to a different side", detect_orientation_str(DETECT_ORIENTATION_UPSIDE_DOWN)); usleep(100000); calibration_log_info(mavlink_log_pub, "[cal] %s side done, rotate to a different side", detect_orientation_str(DETECT_ORIENTATION_UPSIDE_DOWN)); usleep(100000); calibration_log_info(mavlink_log_pub, "[cal] %s side done, rotate to a different side", detect_orientation_str(DETECT_ORIENTATION_RIGHT)); usleep(100000); calibration_log_info(mavlink_log_pub, "[cal] %s side done, rotate to a different side", detect_orientation_str(DETECT_ORIENTATION_RIGHT)); usleep(100000); for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) { // Initialize to no subscription worker_data.sub_mag[cur_mag] = -1; // Initialize to no memory allocated worker_data.x[cur_mag] = NULL; worker_data.y[cur_mag] = NULL; worker_data.z[cur_mag] = NULL; worker_data.calibration_counter_total[cur_mag] = 0; } const unsigned int calibration_points_maxcount = calibration_sides * worker_data.calibration_points_perside; char str[30]; for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) { worker_data.x[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount)); worker_data.y[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount)); worker_data.z[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount)); if (worker_data.x[cur_mag] == NULL || worker_data.y[cur_mag] == NULL || worker_data.z[cur_mag] == NULL) { calibration_log_critical(mavlink_log_pub, "[cal] ERROR: out of memory"); result = calibrate_return_error; } } // Setup subscriptions to mag sensors if (result == calibrate_return_ok) { // We should not try to subscribe if the topic doesn't actually exist and can be counted. const unsigned mag_count = orb_group_count(ORB_ID(sensor_mag)); for (unsigned cur_mag = 0; cur_mag < mag_count; cur_mag++) { // Mag in this slot is available worker_data.sub_mag[cur_mag] = orb_subscribe_multi(ORB_ID(sensor_mag), cur_mag); #ifdef __PX4_QURT // For QURT respectively the driver framework, we need to get the device ID by copying one report. struct mag_report mag_report; orb_copy(ORB_ID(sensor_mag), worker_data.sub_mag[cur_mag], &mag_report); device_ids[cur_mag] = mag_report.device_id; #endif if (worker_data.sub_mag[cur_mag] < 0) { calibration_log_critical(mavlink_log_pub, "[cal] Mag #%u not found, abort", cur_mag); result = calibrate_return_error; break; } if (device_ids[cur_mag] != 0) { // Get priority int32_t prio; orb_priority(worker_data.sub_mag[cur_mag], &prio); if (prio > device_prio_max) { device_prio_max = prio; device_id_primary = device_ids[cur_mag]; } } else { calibration_log_critical(mavlink_log_pub, "[cal] Mag #%u no device id, abort", cur_mag); result = calibrate_return_error; break; } } } // Limit update rate to get equally spaced measurements over time (in ms) if (result == calibrate_return_ok) { for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) { if (device_ids[cur_mag] != 0) { // Mag in this slot is available unsigned int orb_interval_msecs = (worker_data.calibration_interval_perside_useconds / 1000) / worker_data.calibration_points_perside; //calibration_log_info(mavlink_log_pub, "Orb interval %u msecs", orb_interval_msecs); orb_set_interval(worker_data.sub_mag[cur_mag], orb_interval_msecs); } } } if (result == calibrate_return_ok) { int cancel_sub = calibrate_cancel_subscribe(); result = calibrate_from_orientation(mavlink_log_pub, // uORB handle to write output cancel_sub, // Subscription to vehicle_command for cancel support worker_data.side_data_collected, // Sides to calibrate mag_calibration_worker, // Calibration worker &worker_data, // Opaque data for calibration worked true); // true: lenient still detection calibrate_cancel_unsubscribe(cancel_sub); } // Close subscriptions for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) { if (worker_data.sub_mag[cur_mag] >= 0) { px4_close(worker_data.sub_mag[cur_mag]); } } // Calculate calibration values for each mag float sphere_x[max_mags]; float sphere_y[max_mags]; float sphere_z[max_mags]; float sphere_radius[max_mags]; // Sphere fit the data to get calibration values if (result == calibrate_return_ok) { for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) { if (device_ids[cur_mag] != 0) { // Mag in this slot is available and we should have values for it to calibrate sphere_fit_least_squares(worker_data.x[cur_mag], worker_data.y[cur_mag], worker_data.z[cur_mag], worker_data.calibration_counter_total[cur_mag], 100, 0.0f, &sphere_x[cur_mag], &sphere_y[cur_mag], &sphere_z[cur_mag], &sphere_radius[cur_mag]); if (!PX4_ISFINITE(sphere_x[cur_mag]) || !PX4_ISFINITE(sphere_y[cur_mag]) || !PX4_ISFINITE(sphere_z[cur_mag])) { calibration_log_emergency(mavlink_log_pub, "ERROR: Retry calibration (sphere NaN, #%u)", cur_mag); result = calibrate_return_error; } if (fabsf(sphere_x[cur_mag]) > MAG_MAX_OFFSET_LEN || fabsf(sphere_y[cur_mag]) > MAG_MAX_OFFSET_LEN || fabsf(sphere_z[cur_mag]) > MAG_MAX_OFFSET_LEN) { calibration_log_emergency(mavlink_log_pub, "ERROR: Replace %s mag fault", (internal[cur_mag]) ? "autopilot, internal" : "GPS unit, external"); calibration_log_info(mavlink_log_pub, "Excessive offsets: %8.4f, %8.4f, %8.4f, #%u", (double)sphere_x[cur_mag], (double)sphere_y[cur_mag], (double)sphere_z[cur_mag], cur_mag); result = calibrate_return_ok; } } } } // Print uncalibrated data points if (result == calibrate_return_ok) { // DO NOT REMOVE! Critical validation data! // printf("RAW DATA:\n--------------------\n"); // for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) { // if (worker_data.calibration_counter_total[cur_mag] == 0) { // continue; // } // printf("RAW: MAG %u with %u samples:\n", (unsigned)cur_mag, (unsigned)worker_data.calibration_counter_total[cur_mag]); // for (size_t i = 0; i < worker_data.calibration_counter_total[cur_mag]; i++) { // float x = worker_data.x[cur_mag][i]; // float y = worker_data.y[cur_mag][i]; // float z = worker_data.z[cur_mag][i]; // printf("%8.4f, %8.4f, %8.4f\n", (double)x, (double)y, (double)z); // } // printf(">>>>>>>\n"); // } // printf("CALIBRATED DATA:\n--------------------\n"); // for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) { // if (worker_data.calibration_counter_total[cur_mag] == 0) { // continue; // } // printf("Calibrated: MAG %u with %u samples:\n", (unsigned)cur_mag, (unsigned)worker_data.calibration_counter_total[cur_mag]); // for (size_t i = 0; i < worker_data.calibration_counter_total[cur_mag]; i++) { // float x = worker_data.x[cur_mag][i] - sphere_x[cur_mag]; // float y = worker_data.y[cur_mag][i] - sphere_y[cur_mag]; // float z = worker_data.z[cur_mag][i] - sphere_z[cur_mag]; // printf("%8.4f, %8.4f, %8.4f\n", (double)x, (double)y, (double)z); // } // printf("SPHERE RADIUS: %8.4f\n", (double)sphere_radius[cur_mag]); // printf(">>>>>>>\n"); // } } // Data points are no longer needed for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) { free(worker_data.x[cur_mag]); free(worker_data.y[cur_mag]); free(worker_data.z[cur_mag]); } if (result == calibrate_return_ok) { (void)param_set_no_notification(param_find("CAL_MAG_PRIME"), &(device_id_primary)); for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) { if (device_ids[cur_mag] != 0) { struct mag_calibration_s mscale; #ifndef __PX4_QURT int fd_mag = -1; // Set new scale (void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, cur_mag); fd_mag = px4_open(str, 0); if (fd_mag < 0) { calibration_log_critical(mavlink_log_pub, "[cal] ERROR: unable to open mag device #%u", cur_mag); result = calibrate_return_error; } if (result == calibrate_return_ok) { if (px4_ioctl(fd_mag, MAGIOCGSCALE, (long unsigned int)&mscale) != OK) { calibration_log_critical(mavlink_log_pub, "[cal] ERROR: failed to get current calibration #%u", cur_mag); result = calibrate_return_error; } } #endif if (result == calibrate_return_ok) { mscale.x_offset = sphere_x[cur_mag]; mscale.y_offset = sphere_y[cur_mag]; mscale.z_offset = sphere_z[cur_mag]; #ifndef __PX4_QURT if (px4_ioctl(fd_mag, MAGIOCSSCALE, (long unsigned int)&mscale) != OK) { calibration_log_critical(mavlink_log_pub, CAL_ERROR_APPLY_CAL_MSG, cur_mag); result = calibrate_return_error; } #endif } #ifndef __PX4_QURT // Mag device no longer needed if (fd_mag >= 0) { px4_close(fd_mag); } #endif if (result == calibrate_return_ok) { bool failed = false; /* set parameters */ (void)sprintf(str, "CAL_MAG%u_ID", cur_mag); failed |= (OK != param_set_no_notification(param_find(str), &(device_ids[cur_mag]))); (void)sprintf(str, "CAL_MAG%u_XOFF", cur_mag); failed |= (OK != param_set_no_notification(param_find(str), &(mscale.x_offset))); (void)sprintf(str, "CAL_MAG%u_YOFF", cur_mag); failed |= (OK != param_set_no_notification(param_find(str), &(mscale.y_offset))); (void)sprintf(str, "CAL_MAG%u_ZOFF", cur_mag); failed |= (OK != param_set_no_notification(param_find(str), &(mscale.z_offset))); (void)sprintf(str, "CAL_MAG%u_XSCALE", cur_mag); // FIXME: scaling is not used right now on QURT #ifndef __PX4_QURT failed |= (OK != param_set_no_notification(param_find(str), &(mscale.x_scale))); (void)sprintf(str, "CAL_MAG%u_YSCALE", cur_mag); failed |= (OK != param_set_no_notification(param_find(str), &(mscale.y_scale))); (void)sprintf(str, "CAL_MAG%u_ZSCALE", cur_mag); failed |= (OK != param_set_no_notification(param_find(str), &(mscale.z_scale))); #endif if (failed) { calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG, cur_mag); result = calibrate_return_error; } else { calibration_log_info(mavlink_log_pub, "[cal] mag #%u off: x:%.2f y:%.2f z:%.2f Ga", cur_mag, (double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset); #ifndef __PX4_QURT calibration_log_info(mavlink_log_pub, "[cal] mag #%u scale: x:%.2f y:%.2f z:%.2f", cur_mag, (double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale); #endif usleep(200000); } } } } } return result; }
void FixedwingAttitudeControl::task_main() { /* inform about start */ warnx("Initializing.."); fflush(stdout); /* * do subscriptions */ _att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); _accel_sub = orb_subscribe(ORB_ID(sensor_accel)); _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_vcontrol_mode_sub, 200); /* rate limit attitude control to 50 Hz (with some margin, so 17 ms) */ orb_set_interval(_att_sub, 17); parameters_update(); /* get an initial update for all sensor and status data */ vehicle_airspeed_poll(); vehicle_setpoint_poll(); vehicle_accel_poll(); vehicle_control_mode_poll(); vehicle_manual_poll(); /* wakeup source(s) */ struct pollfd fds[2]; /* Setup of loop */ fds[0].fd = _params_sub; fds[0].events = POLLIN; fds[1].fd = _att_sub; fds[1].events = POLLIN; while (!_task_should_exit) { static int loop_counter = 0; /* wait for up to 500ms for data */ int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); /* timed out - periodic check for _task_should_exit, etc. */ if (pret == 0) continue; /* this is undesirable but not much we can do - might want to flag unhappy status */ if (pret < 0) { warn("poll error %d, %d", pret, errno); continue; } perf_begin(_loop_perf); /* only update parameters if they changed */ if (fds[0].revents & POLLIN) { /* read from param to clear updated flag */ struct parameter_update_s update; orb_copy(ORB_ID(parameter_update), _params_sub, &update); /* update parameters from storage */ parameters_update(); } /* only run controller if attitude changed */ if (fds[1].revents & POLLIN) { static uint64_t last_run = 0; float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; last_run = hrt_absolute_time(); /* guard against too large deltaT's */ if (deltaT > 1.0f) deltaT = 0.01f; /* load local copies */ orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); vehicle_airspeed_poll(); vehicle_setpoint_poll(); vehicle_accel_poll(); vehicle_control_mode_poll(); vehicle_manual_poll(); global_pos_poll(); /* lock integrator until control is started */ bool lock_integrator; if (_vcontrol_mode.flag_control_attitude_enabled) { lock_integrator = false; } else { lock_integrator = true; } /* Simple handling of failsafe: deploy parachute if failsafe is on */ if (_vcontrol_mode.flag_control_termination_enabled) { _actuators_airframe.control[1] = 1.0f; // warnx("_actuators_airframe.control[1] = 1.0f;"); } else { _actuators_airframe.control[1] = 0.0f; // warnx("_actuators_airframe.control[1] = -1.0f;"); } /* decide if in stabilized or full manual control */ if (_vcontrol_mode.flag_control_attitude_enabled) { /* scale around tuning airspeed */ float airspeed; /* if airspeed is not updating, we assume the normal average speed */ if (bool nonfinite = !isfinite(_airspeed.true_airspeed_m_s) || hrt_elapsed_time(&_airspeed.timestamp) > 1e6) { airspeed = _parameters.airspeed_trim; if (nonfinite) { perf_count(_nonfinite_input_perf); } } else { airspeed = _airspeed.true_airspeed_m_s; } /* * For scaling our actuators using anything less than the min (close to stall) * speed doesn't make any sense - its the strongest reasonable deflection we * want to do in flight and its the baseline a human pilot would choose. * * Forcing the scaling to this value allows reasonable handheld tests. */ float airspeed_scaling = _parameters.airspeed_trim / ((airspeed < _parameters.airspeed_min) ? _parameters.airspeed_min : airspeed); float roll_sp = _parameters.rollsp_offset_rad; float pitch_sp = _parameters.pitchsp_offset_rad; float throttle_sp = 0.0f; if (_vcontrol_mode.flag_control_velocity_enabled || _vcontrol_mode.flag_control_position_enabled) { /* read in attitude setpoint from attitude setpoint uorb topic */ roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad; pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad; throttle_sp = _att_sp.thrust; /* reset integrals where needed */ if (_att_sp.roll_reset_integral) { _roll_ctrl.reset_integrator(); } if (_att_sp.pitch_reset_integral) { _pitch_ctrl.reset_integrator(); } if (_att_sp.yaw_reset_integral) { _yaw_ctrl.reset_integrator(); } } else { /* * Scale down roll and pitch as the setpoints are radians * and a typical remote can only do around 45 degrees, the mapping is * -1..+1 to -man_roll_max rad..+man_roll_max rad (equivalent for pitch) * * With this mapping the stick angle is a 1:1 representation of * the commanded attitude. * * The trim gets subtracted here from the manual setpoint to get * the intended attitude setpoint. Later, after the rate control step the * trim is added again to the control signal. */ roll_sp = (_manual.y * _parameters.man_roll_max - _parameters.trim_roll) + _parameters.rollsp_offset_rad; pitch_sp = -(_manual.x * _parameters.man_pitch_max - _parameters.trim_pitch) + _parameters.pitchsp_offset_rad; throttle_sp = _manual.z; _actuators.control[4] = _manual.flaps; /* * in manual mode no external source should / does emit attitude setpoints. * emit the manual setpoint here to allow attitude controller tuning * in attitude control mode. */ struct vehicle_attitude_setpoint_s att_sp; att_sp.timestamp = hrt_absolute_time(); att_sp.roll_body = roll_sp; att_sp.pitch_body = pitch_sp; att_sp.yaw_body = 0.0f - _parameters.trim_yaw; att_sp.thrust = throttle_sp; /* lazily publish the setpoint only once available */ if (_attitude_sp_pub > 0) { /* publish the attitude setpoint */ orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &att_sp); } else { /* advertise and publish */ _attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); } } /* Prepare speed_body_u and speed_body_w */ float speed_body_u = 0.0f; float speed_body_v = 0.0f; float speed_body_w = 0.0f; if(_att.R_valid) { speed_body_u = _att.R[0][0] * _global_pos.vel_n + _att.R[1][0] * _global_pos.vel_e + _att.R[2][0] * _global_pos.vel_d; speed_body_v = _att.R[0][1] * _global_pos.vel_n + _att.R[1][1] * _global_pos.vel_e + _att.R[2][1] * _global_pos.vel_d; speed_body_w = _att.R[0][2] * _global_pos.vel_n + _att.R[1][2] * _global_pos.vel_e + _att.R[2][2] * _global_pos.vel_d; } else { if (loop_counter % 10 == 0) { warnx("Did not get a valid R\n"); } } /* Run attitude controllers */ if (isfinite(roll_sp) && isfinite(pitch_sp)) { _roll_ctrl.control_attitude(roll_sp, _att.roll); _pitch_ctrl.control_attitude(pitch_sp, _att.roll, _att.pitch, airspeed); _yaw_ctrl.control_attitude(_att.roll, _att.pitch, speed_body_u, speed_body_v, speed_body_w, _roll_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate()); //runs last, because is depending on output of roll and pitch attitude /* Run attitude RATE controllers which need the desired attitudes from above, add trim */ float roll_u = _roll_ctrl.control_bodyrate(_att.pitch, _att.rollspeed, _att.yawspeed, _yaw_ctrl.get_desired_rate(), _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); _actuators.control[0] = (isfinite(roll_u)) ? roll_u + _parameters.trim_roll : _parameters.trim_roll; if (!isfinite(roll_u)) { _roll_ctrl.reset_integrator(); perf_count(_nonfinite_output_perf); if (loop_counter % 10 == 0) { warnx("roll_u %.4f", (double)roll_u); } } float pitch_u = _pitch_ctrl.control_bodyrate(_att.roll, _att.pitch, _att.pitchspeed, _att.yawspeed, _yaw_ctrl.get_desired_rate(), _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); _actuators.control[1] = (isfinite(pitch_u)) ? pitch_u + _parameters.trim_pitch : _parameters.trim_pitch; if (!isfinite(pitch_u)) { _pitch_ctrl.reset_integrator(); perf_count(_nonfinite_output_perf); if (loop_counter % 10 == 0) { warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f," " airspeed %.4f, airspeed_scaling %.4f," " roll_sp %.4f, pitch_sp %.4f," " _roll_ctrl.get_desired_rate() %.4f," " _pitch_ctrl.get_desired_rate() %.4f" " att_sp.roll_body %.4f", (double)pitch_u, (double)_yaw_ctrl.get_desired_rate(), (double)airspeed, (double)airspeed_scaling, (double)roll_sp, (double)pitch_sp, (double)_roll_ctrl.get_desired_rate(), (double)_pitch_ctrl.get_desired_rate(), (double)_att_sp.roll_body); } } float yaw_u = _yaw_ctrl.control_bodyrate(_att.roll, _att.pitch, _att.pitchspeed, _att.yawspeed, _pitch_ctrl.get_desired_rate(), _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); _actuators.control[2] = (isfinite(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw; if (!isfinite(yaw_u)) { _yaw_ctrl.reset_integrator(); perf_count(_nonfinite_output_perf); if (loop_counter % 10 == 0) { warnx("yaw_u %.4f", (double)yaw_u); } } /* throttle passed through */ _actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f; if (!isfinite(throttle_sp)) { if (loop_counter % 10 == 0) { warnx("throttle_sp %.4f", (double)throttle_sp); } } } else { perf_count(_nonfinite_input_perf); if (loop_counter % 10 == 0) { warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", (double)roll_sp, (double)pitch_sp); } } /* * Lazily publish the rate setpoint (for analysis, the actuators are published below) * only once available */ vehicle_rates_setpoint_s rates_sp; rates_sp.roll = _roll_ctrl.get_desired_rate(); rates_sp.pitch = _pitch_ctrl.get_desired_rate(); rates_sp.yaw = _yaw_ctrl.get_desired_rate(); rates_sp.timestamp = hrt_absolute_time(); if (_rate_sp_pub > 0) { /* publish the attitude setpoint */ orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &rates_sp); } else { /* advertise and publish */ _rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); } } else { /* manual/direct control */ _actuators.control[0] = _manual.y; _actuators.control[1] = -_manual.x; _actuators.control[2] = _manual.r; _actuators.control[3] = _manual.z; _actuators.control[4] = _manual.flaps; } _actuators.control[5] = _manual.aux1; _actuators.control[6] = _manual.aux2; _actuators.control[7] = _manual.aux3; /* lazily publish the setpoint only once available */ _actuators.timestamp = hrt_absolute_time(); _actuators_airframe.timestamp = hrt_absolute_time(); if (_actuators_0_pub > 0) { /* publish the attitude setpoint */ orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators); } else { /* advertise and publish */ _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); } if (_actuators_1_pub > 0) { /* publish the attitude setpoint */ orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_airframe); // warnx("%.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f", // (double)_actuators_airframe.control[0], (double)_actuators_airframe.control[1], (double)_actuators_airframe.control[2], // (double)_actuators_airframe.control[3], (double)_actuators_airframe.control[4], (double)_actuators_airframe.control[5], // (double)_actuators_airframe.control[6], (double)_actuators_airframe.control[7]); } else { /* advertise and publish */ _actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_airframe); } } loop_counter++; perf_end(_loop_perf); } warnx("exiting.\n"); _control_task = -1; _exit(0); }
void Sensors::task_main() { /* start individual sensors */ accel_init(); gyro_init(); mag_init(); baro_init(); adc_init(); /* * do subscriptions */ _gyro_sub = orb_subscribe(ORB_ID(sensor_gyro)); _accel_sub = orb_subscribe(ORB_ID(sensor_accel)); _mag_sub = orb_subscribe(ORB_ID(sensor_mag)); _rc_sub = orb_subscribe(ORB_ID(input_rc)); _baro_sub = orb_subscribe(ORB_ID(sensor_baro)); _diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); _vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_vcontrol_mode_sub, 200); /* rate limit gyro to 250 Hz (the gyro signal is lowpassed accordingly earlier) */ orb_set_interval(_gyro_sub, 4); /* * do advertisements */ struct sensor_combined_s raw; memset(&raw, 0, sizeof(raw)); raw.timestamp = hrt_absolute_time(); raw.adc_voltage_v[0] = 0.0f; raw.adc_voltage_v[1] = 0.0f; raw.adc_voltage_v[2] = 0.0f; raw.adc_voltage_v[3] = 0.0f; memset(&_battery_status, 0, sizeof(_battery_status)); _battery_status.voltage_v = -1.0f; _battery_status.voltage_filtered_v = -1.0f; _battery_status.current_a = -1.0f; _battery_status.discharged_mah = -1.0f; /* get a set of initial values */ accel_poll(raw); gyro_poll(raw); mag_poll(raw); baro_poll(raw); diff_pres_poll(raw); parameter_update_poll(true /* forced */); /* advertise the sensor_combined topic and make the initial publication */ _sensor_pub = orb_advertise(ORB_ID(sensor_combined), &raw); /* wakeup source(s) */ struct pollfd fds[1]; /* use the gyro to pace output - XXX BROKEN if we are using the L3GD20 */ fds[0].fd = _gyro_sub; fds[0].events = POLLIN; while (!_task_should_exit) { /* wait for up to 50ms for data */ int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 50); /* if pret == 0 it timed out - periodic check for _task_should_exit, etc. */ /* this is undesirable but not much we can do - might want to flag unhappy status */ if (pret < 0) { warn("poll error %d, %d", pret, errno); continue; } perf_begin(_loop_perf); /* check vehicle status for changes to publication state */ vehicle_control_mode_poll(); /* check parameters for updates */ parameter_update_poll(); /* the timestamp of the raw struct is updated by the gyro_poll() method */ /* copy most recent sensor data */ gyro_poll(raw); accel_poll(raw); mag_poll(raw); baro_poll(raw); /* check battery voltage */ adc_poll(raw); diff_pres_poll(raw); /* Inform other processes that new data is available to copy */ if (_publishing) { orb_publish(ORB_ID(sensor_combined), _sensor_pub, &raw); } /* Look for new r/c input data */ rc_poll(); perf_end(_loop_perf); } warnx("[sensors] exiting."); _sensors_task = -1; _exit(0); }