void Navigator::task_main() { bool have_geofence_position_data = false; /* 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) { PX4_INFO("Try to load geofence.txt"); _geofence.loadFromFile(GEOFENCE_FILENAME); } else { if (_geofence.clearDm() != OK) { mavlink_log_critical(&_mavlink_log_pub, "failed clearing 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)); _fw_pos_ctrl_status_sub = orb_subscribe(ORB_ID(fw_pos_ctrl_status)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected)); _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)); _vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command)); /* copy all topics first time */ vehicle_status_update(); vehicle_land_detected_update(); vehicle_control_mode_update(); global_position_update(); gps_position_update(); sensor_combined_update(); home_position_update(true); fw_pos_ctrl_status_update(); params_update(); /* wakeup source(s) */ px4_pollfd_struct_t fds[1] = {}; /* Setup of loop */ fds[0].fd = _global_pos_sub; fds[0].events = POLLIN; bool global_pos_available_once = false; while (!_task_should_exit) { /* wait for up to 200ms for data */ int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 1000); if (pret == 0) { /* timed out - periodic check for _task_should_exit, etc. */ if (global_pos_available_once) { global_pos_available_once = false; PX4_WARN("global position timeout"); } /* Let the loop run anyway, don't do `continue` here. */ } else if (pret < 0) { /* this is undesirable but not much we can do - might want to flag unhappy status */ PX4_ERR("nav: poll error %d, %d", pret, errno); usleep(10000); continue; } else { if (fds[0].revents & POLLIN) { /* success, global pos is available */ global_position_update(); if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS) { have_geofence_position_data = true; } global_pos_available_once = true; } } perf_begin(_loop_perf); bool updated; /* gps updated */ orb_check(_gps_pos_sub, &updated); if (updated) { gps_position_update(); if (_geofence.getSource() == Geofence::GF_SOURCE_GPS) { have_geofence_position_data = true; } } /* sensors combined updated */ orb_check(_sensor_combined_sub, &updated); if (updated) { sensor_combined_update(); } /* parameters updated */ orb_check(_param_update_sub, &updated); if (updated) { params_update(); updateParams(); } /* vehicle control mode updated */ orb_check(_control_mode_sub, &updated); if (updated) { vehicle_control_mode_update(); } /* vehicle status updated */ orb_check(_vstatus_sub, &updated); if (updated) { vehicle_status_update(); } /* vehicle land detected updated */ orb_check(_land_detected_sub, &updated); if (updated) { vehicle_land_detected_update(); } /* navigation capabilities updated */ orb_check(_fw_pos_ctrl_status_sub, &updated); if (updated) { fw_pos_ctrl_status_update(); } /* home position updated */ orb_check(_home_pos_sub, &updated); if (updated) { home_position_update(); } orb_check(_vehicle_command_sub, &updated); if (updated) { vehicle_command_s cmd; orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd); if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_REPOSITION) { struct position_setpoint_triplet_s *rep = get_reposition_triplet(); // store current position as previous position and goal as next rep->previous.yaw = get_global_position()->yaw; rep->previous.lat = get_global_position()->lat; rep->previous.lon = get_global_position()->lon; rep->previous.alt = get_global_position()->alt; rep->current.loiter_radius = get_loiter_radius(); rep->current.loiter_direction = 1; rep->current.type = position_setpoint_s::SETPOINT_TYPE_LOITER; // Go on and check which changes had been requested if (PX4_ISFINITE(cmd.param4)) { rep->current.yaw = cmd.param4; } else { rep->current.yaw = NAN; } if (PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6)) { rep->current.lat = (cmd.param5 < 1000) ? cmd.param5 : cmd.param5 / (double)1e7; rep->current.lon = (cmd.param6 < 1000) ? cmd.param6 : cmd.param6 / (double)1e7; } else { rep->current.lat = get_global_position()->lat; rep->current.lon = get_global_position()->lon; } if (PX4_ISFINITE(cmd.param7)) { rep->current.alt = cmd.param7; } else { rep->current.alt = get_global_position()->alt; } rep->previous.valid = true; rep->current.valid = true; rep->next.valid = false; } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF) { struct position_setpoint_triplet_s *rep = get_takeoff_triplet(); // store current position as previous position and goal as next rep->previous.yaw = get_global_position()->yaw; rep->previous.lat = get_global_position()->lat; rep->previous.lon = get_global_position()->lon; rep->previous.alt = get_global_position()->alt; rep->current.loiter_radius = get_loiter_radius(); rep->current.loiter_direction = 1; rep->current.type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF; rep->current.yaw = cmd.param4; if (PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6)) { rep->current.lat = (cmd.param5 < 1000) ? cmd.param5 : cmd.param5 / (double)1e7; rep->current.lon = (cmd.param6 < 1000) ? cmd.param6 : cmd.param6 / (double)1e7; } else { // If one of them is non-finite, reset both rep->current.lat = NAN; rep->current.lon = NAN; } rep->current.alt = cmd.param7; rep->previous.valid = true; rep->current.valid = true; rep->next.valid = false; } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_LAND_START) { /* find NAV_CMD_DO_LAND_START in the mission and * use MAV_CMD_MISSION_START to start the mission there */ unsigned land_start = _mission.find_offboard_land_start(); if (land_start != -1) { vehicle_command_s vcmd = {}; vcmd.target_system = get_vstatus()->system_id; vcmd.target_component = get_vstatus()->component_id; vcmd.command = vehicle_command_s::VEHICLE_CMD_MISSION_START; vcmd.param1 = land_start; vcmd.param2 = 0; orb_advert_t pub = orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH); (void)orb_unadvertise(pub); } } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_MISSION_START) { if (get_mission_result()->valid && PX4_ISFINITE(cmd.param1) && (cmd.param1 >= 0) && (cmd.param1 < _mission_result.seq_total)) { _mission.set_current_offboard_mission_index(cmd.param1); } } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_PAUSE_CONTINUE) { PX4_INFO("got pause/continue command"); } } /* Check geofence violation */ static hrt_abstime last_geofence_check = 0; if (have_geofence_position_data && (_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_NONE) && (hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL)) { bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter, _home_pos, home_position_valid()); last_geofence_check = hrt_absolute_time(); have_geofence_position_data = false; _geofence_result.geofence_action = _geofence.getGeofenceAction(); 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_log_pub, "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 vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_mission; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_loiter; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER: _pos_sp_triplet_published_invalid_once = false; if (_param_rcloss_act.get() == 1) { _navigation_mode = &_loiter; } else if (_param_rcloss_act.get() == 3) { _navigation_mode = &_land; } else if (_param_rcloss_act.get() == 4) { _navigation_mode = &_rcLoss; } else { /* if == 2 or unknown, RTL */ _navigation_mode = &_rtl; } break; case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_rtl; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_takeoff; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_land; break; case vehicle_status_s::NAVIGATION_STATE_DESCEND: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_land; break; case vehicle_status_s::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_act.get() == 1) { _navigation_mode = &_loiter; } else if (_param_datalinkloss_act.get() == 3) { _navigation_mode = &_land; } else if (_param_datalinkloss_act.get() == 4) { _navigation_mode = &_dataLinkLoss; } else { /* if == 2 or unknown, RTL */ _navigation_mode = &_rtl; } break; case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_engineFailure; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_gpsFailure; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_follow_target; break; case vehicle_status_s::NAVIGATION_STATE_MANUAL: case vehicle_status_s::NAVIGATION_STATE_ACRO: case vehicle_status_s::NAVIGATION_STATE_ALTCTL: case vehicle_status_s::NAVIGATION_STATE_POSCTL: case vehicle_status_s::NAVIGATION_STATE_TERMINATION: case vehicle_status_s::NAVIGATION_STATE_OFFBOARD: case vehicle_status_s::NAVIGATION_STATE_STAB: default: _pos_sp_triplet_published_invalid_once = false; _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) { _pos_sp_triplet.timestamp = hrt_absolute_time(); publish_position_setpoint_triplet(); _pos_sp_triplet_updated = false; } if (_mission_result_updated) { publish_mission_result(); _mission_result_updated = false; } perf_end(_loop_perf); } PX4_INFO("exiting"); _navigator_task = -1; return; }
void controls_tick() { /* * Gather R/C control inputs from supported sources. * * Note that if you're silly enough to connect more than * one control input source, they're going to fight each * other. Don't do that. */ /* receive signal strenght indicator (RSSI). 0 = no connection, 255: perfect connection */ uint16_t rssi = 0; #ifdef ADC_RSSI if (r_setup_features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) { unsigned counts = adc_measure(ADC_RSSI); if (counts != 0xffff) { /* low pass*/ _rssi_adc_counts = (_rssi_adc_counts * 0.998f) + (counts * 0.002f); /* use 1:1 scaling on 3.3V, 12-Bit ADC input */ unsigned mV = _rssi_adc_counts * 3300 / 4095; /* scale to 0..100 (RC_INPUT_RSSI_MAX == 100) */ rssi = (mV * RC_INPUT_RSSI_MAX / 3300); if (rssi > RC_INPUT_RSSI_MAX) { rssi = RC_INPUT_RSSI_MAX; } } } #endif /* zero RSSI if signal is lost */ if (!(r_raw_rc_flags & (PX4IO_P_RAW_RC_FLAGS_RC_OK))) { rssi = 0; } perf_begin(c_gather_dsm); bool dsm_updated, st24_updated, sumd_updated; (void)dsm_port_input(&rssi, &dsm_updated, &st24_updated, &sumd_updated); if (dsm_updated) { r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM; } if (st24_updated) { r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_ST24; } if (sumd_updated) { r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SUMD; } perf_end(c_gather_dsm); perf_begin(c_gather_sbus); bool sbus_failsafe, sbus_frame_drop; bool sbus_updated = sbus_input(_sbus_fd, r_raw_rc_values, &r_raw_rc_count, &sbus_failsafe, &sbus_frame_drop, PX4IO_RC_INPUT_CHANNELS); if (sbus_updated) { r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS; unsigned sbus_rssi = RC_INPUT_RSSI_MAX; if (sbus_frame_drop) { r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FRAME_DROP; sbus_rssi = RC_INPUT_RSSI_MAX / 2; } else { r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); } if (sbus_failsafe) { r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE; } else { r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); } /* set RSSI to an emulated value if ADC RSSI is off */ if (!(r_setup_features & PX4IO_P_SETUP_FEATURES_ADC_RSSI)) { rssi = sbus_rssi; } } perf_end(c_gather_sbus); /* * XXX each S.bus frame will cause a PPM decoder interrupt * storm (lots of edges). It might be sensible to actually * disable the PPM decoder completely if we have S.bus signal. */ perf_begin(c_gather_ppm); bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count, &r_page_raw_rc_input[PX4IO_P_RAW_RC_DATA]); if (ppm_updated) { r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM; r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); } perf_end(c_gather_ppm); /* limit number of channels to allowable data size */ if (r_raw_rc_count > PX4IO_RC_INPUT_CHANNELS) { r_raw_rc_count = PX4IO_RC_INPUT_CHANNELS; } /* store RSSI */ r_page_raw_rc_input[PX4IO_P_RAW_RC_NRSSI] = rssi; /* * In some cases we may have received a frame, but input has still * been lost. */ bool rc_input_lost = false; /* * If we received a new frame from any of the RC sources, process it. */ if (dsm_updated || sbus_updated || ppm_updated || st24_updated || sumd_updated) { /* record a bitmask of channels assigned */ unsigned assigned_channels = 0; /* update RC-received timestamp */ system_state.rc_channels_timestamp_received = hrt_absolute_time(); /* update RC-received timestamp */ system_state.rc_channels_timestamp_valid = system_state.rc_channels_timestamp_received; /* map raw inputs to mapped inputs */ /* XXX mapping should be atomic relative to protocol */ for (unsigned i = 0; i < r_raw_rc_count; i++) { /* map the input channel */ uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE]; if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) { uint16_t raw = r_raw_rc_values[i]; int16_t scaled; /* * 1) Constrain to min/max values, as later processing depends on bounds. */ if (raw < conf[PX4IO_P_RC_CONFIG_MIN]) { raw = conf[PX4IO_P_RC_CONFIG_MIN]; } if (raw > conf[PX4IO_P_RC_CONFIG_MAX]) { raw = conf[PX4IO_P_RC_CONFIG_MAX]; } /* * 2) Scale around the mid point differently for lower and upper range. * * This is necessary as they don't share the same endpoints and slope. * * First normalize to 0..1 range with correct sign (below or above center), * then scale to 20000 range (if center is an actual center, -10000..10000, * if parameters only support half range, scale to 10000 range, e.g. if * center == min 0..10000, if center == max -10000..0). * * As the min and max bounds were enforced in step 1), division by zero * cannot occur, as for the case of center == min or center == max the if * statement is mutually exclusive with the arithmetic NaN case. * * DO NOT REMOVE OR ALTER STEP 1! */ if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) { scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)( conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])); } else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) { scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)( conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN])); } else { /* in the configured dead zone, output zero */ scaled = 0; } /* invert channel if requested */ if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) { scaled = -scaled; } /* and update the scaled/mapped version */ unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]; if (mapped < PX4IO_CONTROL_CHANNELS) { /* invert channel if pitch - pulling the lever down means pitching up by convention */ if (mapped == 1) { /* roll, pitch, yaw, throttle, override is the standard order */ scaled = -scaled; } if (mapped == 3 && r_setup_rc_thr_failsafe) { /* throttle failsafe detection */ if (((raw < conf[PX4IO_P_RC_CONFIG_MIN]) && (raw < r_setup_rc_thr_failsafe)) || ((raw > conf[PX4IO_P_RC_CONFIG_MAX]) && (raw > r_setup_rc_thr_failsafe))) { r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE; } else { r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); } } r_rc_values[mapped] = SIGNED_TO_REG(scaled); assigned_channels |= (1 << mapped); } else if (mapped == PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH) { /* pick out override channel, indicated by special mapping */ rc_value_override = SIGNED_TO_REG(scaled); } } } /* set un-assigned controls to zero */ for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) { if (!(assigned_channels & (1 << i))) { r_rc_values[i] = 0; } } /* set RC OK flag, as we got an update */ r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK; r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_OK; /* if we have enough channels (5) to control the vehicle, the mapping is ok */ if (assigned_channels > 4) { r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_MAPPING_OK; } else { r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_MAPPING_OK); } /* * Export the valid channel bitmap */ r_rc_valid = assigned_channels; } /* * If we haven't seen any new control data in 200ms, assume we * have lost input. */ if (hrt_elapsed_time(&system_state.rc_channels_timestamp_received) > 200000) { rc_input_lost = true; /* clear the input-kind flags here */ r_status_flags &= ~( PX4IO_P_STATUS_FLAGS_RC_PPM | PX4IO_P_STATUS_FLAGS_RC_DSM | PX4IO_P_STATUS_FLAGS_RC_SBUS); } /* * Handle losing RC input */ /* if we are in failsafe, clear the override flag */ if (r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE) { r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE); } /* this kicks in if the receiver is gone, but there is not on failsafe (indicated by separate flag) */ if (rc_input_lost) { /* Clear the RC input status flag, clear manual override flag */ r_status_flags &= ~( PX4IO_P_STATUS_FLAGS_OVERRIDE | PX4IO_P_STATUS_FLAGS_RC_OK); /* flag raw RC as lost */ r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_RC_OK); /* Mark all channels as invalid, as we just lost the RX */ r_rc_valid = 0; /* Set raw channel count to zero */ r_raw_rc_count = 0; /* Set the RC_LOST alarm */ r_status_alarms |= PX4IO_P_STATUS_ALARMS_RC_LOST; } /* * Check for manual override. * * The PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK flag must be set, and we * must have R/C input (NO FAILSAFE!). * Override is enabled if either the hardcoded channel / value combination * is selected, or the AP has requested it. */ if ((!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) || (r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK)) && (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && !(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) { bool override = false; /* * Check mapped channel 5 (can be any remote channel, * depends on RC_MAP_OVER parameter); * If the value is 'high' then the pilot has * requested override. * */ if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(rc_value_override) < RC_CHANNEL_LOW_THRESH)) { override = true; }
bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool *sumd_updated) { perf_begin(c_gather_dsm); uint16_t temp_count = r_raw_rc_count; uint8_t n_bytes = 0; uint8_t *bytes; *dsm_updated = dsm_input(r_raw_rc_values, &temp_count, &n_bytes, &bytes, PX4IO_RC_INPUT_CHANNELS); if (*dsm_updated) { r_raw_rc_count = temp_count & 0x7fff; if (temp_count & 0x8000) { r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_DSM11; } else { r_raw_rc_flags &= ~PX4IO_P_RAW_RC_FLAGS_RC_DSM11; } r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); } perf_end(c_gather_dsm); /* get data from FD and attempt to parse with DSM and ST24 libs */ uint8_t st24_rssi, rx_count; uint16_t st24_channel_count = 0; *st24_updated = false; for (unsigned i = 0; i < n_bytes; i++) { /* set updated flag if one complete packet was parsed */ st24_rssi = RC_INPUT_RSSI_MAX; *st24_updated |= (OK == st24_decode(bytes[i], &st24_rssi, &rx_count, &st24_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS)); } if (*st24_updated) { /* ensure ADC RSSI is disabled */ r_setup_features &= ~(PX4IO_P_SETUP_FEATURES_ADC_RSSI); *rssi = st24_rssi; r_raw_rc_count = st24_channel_count; r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_ST24; r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); } /* get data from FD and attempt to parse with SUMD libs */ uint8_t sumd_rssi, sumd_rx_count; uint16_t sumd_channel_count = 0; *sumd_updated = false; for (unsigned i = 0; i < n_bytes; i++) { /* set updated flag if one complete packet was parsed */ sumd_rssi = RC_INPUT_RSSI_MAX; *sumd_updated |= (OK == sumd_decode(bytes[i], &sumd_rssi, &sumd_rx_count, &sumd_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS)); } if (*sumd_updated) { /* not setting RSSI since SUMD does not provide one */ r_raw_rc_count = sumd_channel_count; r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SUMD; r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); } return (*dsm_updated | *st24_updated | *sumd_updated); }
void BMI055_gyro::measure() { if (hrt_absolute_time() < _reset_wait) { // we're waiting for a reset to complete return; } struct BMI_GyroReport bmi_gyroreport; struct Report { int16_t temp; int16_t gyro_x; int16_t gyro_y; int16_t gyro_z; } report; /* start measuring */ perf_begin(_sample_perf); /* * Fetch the full set of measurements from the BMI055 gyro in one pass. */ bmi_gyroreport.cmd = BMI055_GYR_X_L | DIR_READ; if (OK != transfer((uint8_t *)&bmi_gyroreport, ((uint8_t *)&bmi_gyroreport), sizeof(bmi_gyroreport))) { return; } check_registers(); uint8_t temp = read_reg(BMI055_ACC_TEMP); report.temp = temp; report.gyro_x = bmi_gyroreport.gyro_x; report.gyro_y = bmi_gyroreport.gyro_y; report.gyro_z = bmi_gyroreport.gyro_z; if (report.temp == 0 && report.gyro_x == 0 && report.gyro_y == 0 && report.gyro_z == 0) { // all zero data - probably a SPI bus error perf_count(_bad_transfers); perf_end(_sample_perf); // note that we don't call reset() here as a reset() // costs 20ms with interrupts disabled. That means if // the bmi055 does go bad it would cause a FMU failure, // regardless of whether another sensor is available, return; } perf_count(_good_transfers); if (_register_wait != 0) { // we are waiting for some good transfers before using // the sensor again. We still increment // _good_transfers, but don't return any data yet _register_wait--; return; } /* * Report buffers. */ gyro_report grb; grb.timestamp = hrt_absolute_time(); // report the error count as the sum of the number of bad // transfers and bad register reads. This allows the higher // level code to decide if it should use this sensor based on // whether it has had failures grb.error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers); /* * 1) Scale raw value to SI units using scaling from datasheet. * 2) Subtract static offset (in SI units) * 3) Scale the statically calibrated values with a linear * dynamically obtained factor * * Note: the static sensor offset is the number the sensor outputs * at a nominally 'zero' input. Therefore the offset has to * be subtracted. * * Example: A gyro outputs a value of 74 at zero angular rate * the offset is 74 from the origin and subtracting * 74 from all measurements centers them around zero. */ grb.x_raw = report.gyro_x; grb.y_raw = report.gyro_y; grb.z_raw = report.gyro_z; float xraw_f = report.gyro_x; float yraw_f = report.gyro_y; float zraw_f = report.gyro_z; // apply user specified rotation rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); float x_gyro_in_new = ((xraw_f * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; float y_gyro_in_new = ((yraw_f * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; float z_gyro_in_new = ((zraw_f * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; grb.x = _gyro_filter_x.apply(x_gyro_in_new); grb.y = _gyro_filter_y.apply(y_gyro_in_new); grb.z = _gyro_filter_z.apply(z_gyro_in_new); matrix::Vector3f gval(x_gyro_in_new, y_gyro_in_new, z_gyro_in_new); matrix::Vector3f gval_integrated; bool gyro_notify = _gyro_int.put(grb.timestamp, gval, gval_integrated, grb.integral_dt); grb.x_integral = gval_integrated(0); grb.y_integral = gval_integrated(1); grb.z_integral = gval_integrated(2); grb.scaling = _gyro_range_scale; grb.range_rad_s = _gyro_range_rad_s; grb.temperature_raw = report.temp; grb.temperature = _last_temperature; grb.device_id = _device_id.devid; _gyro_reports->force(&grb); /* notify anyone waiting for data */ if (gyro_notify) { poll_notify(POLLIN); } if (gyro_notify && !(_pub_blocked)) { /* publish it */ orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &grb); } /* stop measuring */ perf_end(_sample_perf); }
int SF0X::collect() { int ret; perf_begin(_sample_perf); /* clear buffer if last read was too long ago */ uint64_t read_elapsed = hrt_elapsed_time(&_last_read); /* the buffer for read chars is buflen minus null termination */ char readbuf[sizeof(_linebuf)]; unsigned readlen = sizeof(readbuf) - 1; /* read from the sensor (uart buffer) */ ret = ::read(_fd, &readbuf[0], readlen); if (ret < 0) { DEVICE_DEBUG("read err: %d", ret); perf_count(_comms_errors); perf_end(_sample_perf); /* only throw an error if we time out */ if (read_elapsed > (SF0X_CONVERSION_INTERVAL * 2)) { return ret; } else { return -EAGAIN; } } else if (ret == 0) { return -EAGAIN; } _last_read = hrt_absolute_time(); float distance_m = -1.0f; bool valid = false; for (int i = 0; i < ret; i++) { if (OK == sf0x_parser(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, &distance_m)) { valid = true; } } if (!valid) { return -EAGAIN; } DEVICE_DEBUG("val (float): %8.4f, raw: %s, valid: %s", (double)distance_m, _linebuf, ((valid) ? "OK" : "NO")); struct distance_sensor_s report; report.timestamp = hrt_absolute_time(); report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER; report.orientation = 8; report.current_distance = distance_m; report.min_distance = get_minimum_distance(); report.max_distance = get_maximum_distance(); report.covariance = 0.0f; /* TODO: set proper ID */ report.id = 0; /* publish it */ orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report); if (_reports->force(&report)) { perf_count(_buffer_overflows); } /* notify anyone waiting for data */ poll_notify(POLLIN); ret = OK; perf_end(_sample_perf); return ret; }
int SF0X::collect() { int ret; perf_begin(_sample_perf); /* clear buffer if last read was too long ago */ uint64_t read_elapsed = hrt_elapsed_time(&_last_read); /* the buffer for read chars is buflen minus null termination */ char readbuf[sizeof(_linebuf)]; unsigned readlen = sizeof(readbuf) - 1; /* read from the sensor (uart buffer) */ ret = ::read(_fd, &readbuf[0], readlen); if (ret < 0) { debug("read err: %d", ret); perf_count(_comms_errors); perf_end(_sample_perf); /* only throw an error if we time out */ if (read_elapsed > (SF0X_CONVERSION_INTERVAL * 2)) { return ret; } else { return -EAGAIN; } } else if (ret == 0) { return -EAGAIN; } _last_read = hrt_absolute_time(); float si_units; bool valid = false; for (int i = 0; i < ret; i++) { if (OK == sf0x_parser(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, &si_units)) { valid = true; } } if (!valid) { return -EAGAIN; } debug("val (float): %8.4f, raw: %s, valid: %s", (double)si_units, _linebuf, ((valid) ? "OK" : "NO")); struct range_finder_report report; /* this should be fairly close to the end of the measurement, so the best approximation of the time */ report.timestamp = hrt_absolute_time(); report.error_count = perf_event_count(_comms_errors); report.distance = si_units; report.minimum_distance = get_minimum_distance(); report.maximum_distance = get_maximum_distance(); report.valid = valid && (si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0); /* publish it */ orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report); if (_reports->force(&report)) { perf_count(_buffer_overflows); } /* notify anyone waiting for data */ poll_notify(POLLIN); ret = OK; perf_end(_sample_perf); return ret; }
int LIS3MDL::collect() { #pragma pack(push, 1) struct { uint8_t x[2]; uint8_t y[2]; uint8_t z[2]; } lis_report; struct { int16_t x; int16_t y; int16_t z; int16_t t; } report; #pragma pack(pop) int ret = 0; uint8_t buf_rx[2] = {0}; float xraw_f; float yraw_f; float zraw_f; struct mag_report new_mag_report; bool sensor_is_onboard = false; perf_begin(_sample_perf); new_mag_report.timestamp = hrt_absolute_time(); new_mag_report.error_count = perf_event_count(_comms_errors); new_mag_report.scaling = _range_scale; new_mag_report.device_id = _device_id.devid; ret = _interface->read(ADDR_OUT_X_L, (uint8_t *)&lis_report, sizeof(lis_report)); /** * Weird behavior: the X axis will be read instead of the temperature registers if you use a pointer to a packed struct...not sure why. * This works now, but further investigation to determine why this happens would be good (I am guessing a type error somewhere) */ ret = _interface->read(ADDR_OUT_T_L, (uint8_t *)&buf_rx, sizeof(buf_rx)); if (ret != OK) { perf_count(_comms_errors); PX4_WARN("Register read error."); return ret; } report.x = (int16_t)((lis_report.x[1] << 8) | lis_report.x[0]); report.y = (int16_t)((lis_report.y[1] << 8) | lis_report.y[0]); report.z = (int16_t)((lis_report.z[1] << 8) | lis_report.z[0]); report.t = (int16_t)((buf_rx[1] << 8) | buf_rx[0]); float temperature = report.t; new_mag_report.temperature = 25.0f + (temperature / 8.0f); // XXX revisit for SPI part, might require a bus type IOCTL unsigned dummy = 0; sensor_is_onboard = !_interface->ioctl(MAGIOCGEXTERNAL, dummy); new_mag_report.is_external = !sensor_is_onboard; /** * RAW outputs */ new_mag_report.x_raw = report.x; new_mag_report.y_raw = report.y; new_mag_report.z_raw = report.z; xraw_f = report.x; yraw_f = report.y; zraw_f = report.z; // apply user specified rotation rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); new_mag_report.x = ((xraw_f * _range_scale) - _scale.x_offset) * _scale.x_scale; /* flip axes and negate value for y */ new_mag_report.y = ((yraw_f * _range_scale) - _scale.y_offset) * _scale.y_scale; /* z remains z */ new_mag_report.z = ((zraw_f * _range_scale) - _scale.z_offset) * _scale.z_scale; if (!(_pub_blocked)) { if (_mag_topic != nullptr) { /* publish it */ orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_mag_report); } else { _mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &new_mag_report, &_orb_class_instance, (sensor_is_onboard) ? ORB_PRIO_HIGH : ORB_PRIO_MAX); if (_mag_topic == nullptr) { DEVICE_DEBUG("ADVERT FAIL"); } } } _last_report = new_mag_report; /* post a report to the ring */ _reports->force(&new_mag_report); /* notify anyone waiting for data */ poll_notify(POLLIN); ret = OK; perf_end(_sample_perf); return ret; }
void FXAS21002C::measure() { /* status register and data as read back from the device */ #pragma pack(push, 1) struct { uint8_t cmd; uint8_t status; int16_t x; int16_t y; int16_t z; } raw_gyro_report; #pragma pack(pop) struct gyro_report gyro_report; /* start the performance counter */ perf_begin(_sample_perf); check_registers(); if (_register_wait != 0) { // we are waiting for some good transfers before using // the sensor again. _register_wait--; perf_end(_sample_perf); return; } /* fetch data from the sensor */ memset(&raw_gyro_report, 0, sizeof(raw_gyro_report)); raw_gyro_report.cmd = DIR_READ(FXAS21002C_STATUS); transfer((uint8_t *)&raw_gyro_report, (uint8_t *)&raw_gyro_report, sizeof(raw_gyro_report)); if (!(raw_gyro_report.status & DR_STATUS_ZYXDR)) { perf_end(_sample_perf); perf_count(_duplicates); return; } /* * The TEMP register contains an 8-bit 2's complement temperature value with a range * of –128 °C to +127 °C and a scaling of 1 °C/LSB. The temperature data is only * compensated (factory trim values applied) when the device is operating in the Active * mode and actively measuring the angular rate. */ if ((_read % _current_rate) == 0) { _last_temperature = read_reg(FXAS21002C_TEMP) * 1.0f; gyro_report.temperature = _last_temperature; } /* * 1) Scale raw value to SI units using scaling from datasheet. * 2) Subtract static offset (in SI units) * 3) Scale the statically calibrated values with a linear * dynamically obtained factor * * Note: the static sensor offset is the number the sensor outputs * at a nominally 'zero' input. Therefore the offset has to * be subtracted. * * Example: A gyro outputs a value of 74 at zero angular rate * the offset is 74 from the origin and subtracting * 74 from all measurements centers them around zero. */ gyro_report.timestamp = hrt_absolute_time(); // report the error count as the number of bad // register reads. This allows the higher level // code to decide if it should use this sensor based on // whether it has had failures gyro_report.error_count = perf_event_count(_bad_registers); gyro_report.x_raw = swap16(raw_gyro_report.x); gyro_report.y_raw = swap16(raw_gyro_report.y); gyro_report.z_raw = swap16(raw_gyro_report.z); float xraw_f = gyro_report.x_raw; float yraw_f = gyro_report.y_raw; float zraw_f = gyro_report.z_raw; // apply user specified rotation rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); float x_in_new = ((xraw_f * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; float y_in_new = ((yraw_f * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; float z_in_new = ((zraw_f * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; gyro_report.x = _gyro_filter_x.apply(x_in_new); gyro_report.y = _gyro_filter_y.apply(y_in_new); gyro_report.z = _gyro_filter_z.apply(z_in_new); matrix::Vector3f gval(x_in_new, y_in_new, z_in_new); matrix::Vector3f gval_integrated; bool gyro_notify = _gyro_int.put(gyro_report.timestamp, gval, gval_integrated, gyro_report.integral_dt); gyro_report.x_integral = gval_integrated(0); gyro_report.y_integral = gval_integrated(1); gyro_report.z_integral = gval_integrated(2); gyro_report.scaling = _gyro_range_scale; gyro_report.range_rad_s = _gyro_range_rad_s; /* return device ID */ gyro_report.device_id = _device_id.devid; _reports->force(&gyro_report); /* notify anyone waiting for data */ if (gyro_notify) { poll_notify(POLLIN); if (!(_pub_blocked)) { /* publish it */ orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &gyro_report); } } _read++; /* stop the perf counter */ perf_end(_sample_perf); }
int user_start(int argc, char *argv[]) { /* run C++ ctors before we go any further */ up_cxxinitialize(); /* reset all to zero */ memset(&system_state, 0, sizeof(system_state)); /* configure the high-resolution time/callout interface */ hrt_init(); /* calculate our fw CRC so FMU can decide if we need to update */ calculate_fw_crc(); /* * Poll at 1ms intervals for received bytes that have not triggered * a DMA event. */ #ifdef CONFIG_ARCH_DMA hrt_call_every(&serial_dma_call, 1000, 1000, (hrt_callout)stm32_serial_dma_poll, NULL); #endif /* print some startup info */ lowsyslog("\nPX4IO: starting\n"); /* default all the LEDs to off while we start */ LED_AMBER(false); LED_BLUE(false); LED_SAFETY(false); #ifdef GPIO_LED4 LED_RING(false); #endif /* turn on servo power (if supported) */ #ifdef POWER_SERVO POWER_SERVO(true); #endif /* turn off S.Bus out (if supported) */ #ifdef ENABLE_SBUS_OUT ENABLE_SBUS_OUT(false); #endif /* start the safety switch handler */ safety_init(); /* configure the first 8 PWM outputs (i.e. all of them) */ up_pwm_servo_init(0xff); /* initialise the control inputs */ controls_init(); /* set up the ADC */ adc_init(); /* start the FMU interface */ interface_init(); #ifdef CONFIG_ARCH_BOARD_RASPILOTIO_V1 spiuart_init(57600U); #endif /* add a performance counter for mixing */ perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix"); /* add a performance counter for controls */ perf_counter_t controls_perf = perf_alloc(PC_ELAPSED, "controls"); /* and one for measuring the loop rate */ perf_counter_t loop_perf = perf_alloc(PC_INTERVAL, "loop"); struct mallinfo minfo = mallinfo(); lowsyslog("MEM: free %u, largest %u\n", minfo.mxordblk, minfo.fordblks); /* initialize PWM limit lib */ pwm_limit_init(&pwm_limit); /* * P O L I C E L I G H T S * * Not enough memory, lock down. * * We might need to allocate mixers later, and this will * ensure that a developer doing a change will notice * that he just burned the remaining RAM with static * allocations. We don't want him to be able to * get past that point. This needs to be clearly * documented in the dev guide. * */ if (minfo.mxordblk < 600) { lowsyslog("ERR: not enough MEM"); bool phase = false; while (true) { if (phase) { LED_AMBER(true); LED_BLUE(false); } else { LED_AMBER(false); LED_BLUE(true); } up_udelay(250000); phase = !phase; } } /* Start the failsafe led init */ failsafe_led_init(); /* * Run everything in a tight loop. */ uint64_t last_debug_time = 0; uint64_t last_heartbeat_time = 0; for (;;) { /* track the rate at which the loop is running */ perf_count(loop_perf); /* kick the mixer */ perf_begin(mixer_perf); mixer_tick(); perf_end(mixer_perf); /* kick the control inputs */ perf_begin(controls_perf); controls_tick(); perf_end(controls_perf); if ((hrt_absolute_time() - last_heartbeat_time) > 250*1000) { last_heartbeat_time = hrt_absolute_time(); heartbeat_blink(); } ring_blink(); check_reboot(); /* check for debug activity (default: none) */ show_debug_messages(); /* post debug state at ~1Hz - this is via an auxiliary serial port * DEFAULTS TO OFF! */ if (hrt_absolute_time() - last_debug_time > (1000 * 1000)) { isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x m=%u", (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG], (unsigned)r_status_flags, (unsigned)r_setup_arming, (unsigned)r_setup_features, (unsigned)mallinfo().mxordblk); last_debug_time = hrt_absolute_time(); } } }
void MPU9250::measure() { if (hrt_absolute_time() < _reset_wait) { // we're waiting for a reset to complete return; } struct MPUReport mpu_report; struct Report { int16_t accel_x; int16_t accel_y; int16_t accel_z; int16_t temp; int16_t gyro_x; int16_t gyro_y; int16_t gyro_z; } report; /* start measuring */ perf_begin(_sample_perf); /* * Fetch the full set of measurements from the MPU9250 in one pass. */ mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS; // sensor transfer at high clock speed set_frequency(MPU9250_HIGH_BUS_SPEED); if (OK != transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report))) { return; } check_registers(); if (check_duplicate(&mpu_report.accel_x[0])) { return; } _mag->measure(mpu_report.mag); /* * Convert from big to little endian */ report.accel_x = int16_t_from_bytes(mpu_report.accel_x); report.accel_y = int16_t_from_bytes(mpu_report.accel_y); report.accel_z = int16_t_from_bytes(mpu_report.accel_z); report.temp = int16_t_from_bytes(mpu_report.temp); report.gyro_x = int16_t_from_bytes(mpu_report.gyro_x); report.gyro_y = int16_t_from_bytes(mpu_report.gyro_y); report.gyro_z = int16_t_from_bytes(mpu_report.gyro_z); if (check_null_data((uint32_t *)&report, sizeof(report) / 4)) { return; } if (_register_wait != 0) { // we are waiting for some good transfers before using the sensor again // We still increment _good_transfers, but don't return any data yet _register_wait--; return; } /* * Swap axes and negate y */ int16_t accel_xt = report.accel_y; int16_t accel_yt = ((report.accel_x == -32768) ? 32767 : -report.accel_x); int16_t gyro_xt = report.gyro_y; int16_t gyro_yt = ((report.gyro_x == -32768) ? 32767 : -report.gyro_x); /* * Apply the swap */ report.accel_x = accel_xt; report.accel_y = accel_yt; report.gyro_x = gyro_xt; report.gyro_y = gyro_yt; #if defined(CONFIG_ARCH_BOARD_LUCI_V1) int16_t gyt = -report.gyro_y; int16_t gzt = -report.gyro_z; int16_t ayt = -report.accel_y; int16_t azt = -report.accel_z; report.gyro_z = gzt; report.gyro_y = gyt; report.accel_z = azt; report.accel_y = ayt; #endif /* * Report buffers. */ accel_report arb; gyro_report grb; /* * Adjust and scale results to m/s^2. */ grb.timestamp = arb.timestamp = hrt_absolute_time(); // report the error count as the sum of the number of bad // transfers and bad register reads. This allows the higher // level code to decide if it should use this sensor based on // whether it has had failures grb.error_count = arb.error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers); /* * 1) Scale raw value to SI units using scaling from datasheet. * 2) Subtract static offset (in SI units) * 3) Scale the statically calibrated values with a linear * dynamically obtained factor * * Note: the static sensor offset is the number the sensor outputs * at a nominally 'zero' input. Therefore the offset has to * be subtracted. * * Example: A gyro outputs a value of 74 at zero angular rate * the offset is 74 from the origin and subtracting * 74 from all measurements centers them around zero. */ /* NOTE: Axes have been swapped to match the board a few lines above. */ arb.x_raw = report.accel_x; arb.y_raw = report.accel_y; arb.z_raw = report.accel_z; float xraw_f = report.accel_x; float yraw_f = report.accel_y; float zraw_f = report.accel_z; // apply user specified rotation rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); float x_in_new = ((xraw_f * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; float y_in_new = ((yraw_f * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; float z_in_new = ((zraw_f * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; arb.x = _accel_filter_x.apply(x_in_new); arb.y = _accel_filter_y.apply(y_in_new); arb.z = _accel_filter_z.apply(z_in_new); math::Vector<3> aval(x_in_new, y_in_new, z_in_new); math::Vector<3> aval_integrated; bool accel_notify = _accel_int.put(arb.timestamp, aval, aval_integrated, arb.integral_dt); arb.x_integral = aval_integrated(0); arb.y_integral = aval_integrated(1); arb.z_integral = aval_integrated(2); arb.scaling = _accel_range_scale; arb.range_m_s2 = _accel_range_m_s2; _last_temperature = (report.temp) / 361.0f + 35.0f; arb.temperature_raw = report.temp; arb.temperature = _last_temperature; grb.x_raw = report.gyro_x; grb.y_raw = report.gyro_y; grb.z_raw = report.gyro_z; xraw_f = report.gyro_x; yraw_f = report.gyro_y; zraw_f = report.gyro_z; // apply user specified rotation rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); float x_gyro_in_new = ((xraw_f * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; float y_gyro_in_new = ((yraw_f * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; float z_gyro_in_new = ((zraw_f * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; grb.x = _gyro_filter_x.apply(x_gyro_in_new); grb.y = _gyro_filter_y.apply(y_gyro_in_new); grb.z = _gyro_filter_z.apply(z_gyro_in_new); math::Vector<3> gval(x_gyro_in_new, y_gyro_in_new, z_gyro_in_new); math::Vector<3> gval_integrated; bool gyro_notify = _gyro_int.put(arb.timestamp, gval, gval_integrated, grb.integral_dt); grb.x_integral = gval_integrated(0); grb.y_integral = gval_integrated(1); grb.z_integral = gval_integrated(2); grb.scaling = _gyro_range_scale; grb.range_rad_s = _gyro_range_rad_s; grb.temperature_raw = report.temp; grb.temperature = _last_temperature; _accel_reports->force(&arb); _gyro_reports->force(&grb); /* notify anyone waiting for data */ if (accel_notify) { poll_notify(POLLIN); } if (gyro_notify) { _gyro->parent_poll_notify(); } if (accel_notify && !(_pub_blocked)) { /* log the time of this report */ perf_begin(_controller_latency_perf); /* publish it */ orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb); } if (gyro_notify && !(_pub_blocked)) { /* publish it */ orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb); } /* stop measuring */ perf_end(_sample_perf); }
void MPU9250::measure() { if (hrt_absolute_time() < _reset_wait) { // we're waiting for a reset to complete return; } struct MPUReport mpu_report; struct ICMReport icm_report; struct Report { int16_t accel_x; int16_t accel_y; int16_t accel_z; int16_t temp; int16_t gyro_x; int16_t gyro_y; int16_t gyro_z; } report; /* start measuring */ perf_begin(_sample_perf); /* * Fetch the full set of measurements from the MPU9250 in one pass */ if ((!_magnetometer_only || _mag->is_passthrough()) && _register_wait == 0) { if (_whoami == MPU_WHOAMI_9250 || _whoami == MPU_WHOAMI_6500) { if (OK != read_reg_range(MPUREG_INT_STATUS, MPU9250_HIGH_BUS_SPEED, (uint8_t *)&mpu_report, sizeof(mpu_report))) { perf_end(_sample_perf); return; } } else { // ICM20948 select_register_bank(REG_BANK(ICMREG_20948_ACCEL_XOUT_H)); if (OK != read_reg_range(ICMREG_20948_ACCEL_XOUT_H, MPU9250_HIGH_BUS_SPEED, (uint8_t *)&icm_report, sizeof(icm_report))) { perf_end(_sample_perf); return; } } check_registers(); if (check_duplicate(MPU_OR_ICM(&mpu_report.accel_x[0], &icm_report.accel_x[0]))) { return; } } /* * In case of a mag passthrough read, hand the magnetometer data over to _mag. Else, * try to read a magnetometer report. */ # ifdef USE_I2C if (_mag->is_passthrough()) { # endif _mag->_measure(mpu_report.mag); # ifdef USE_I2C } else { _mag->measure(); } # endif /* * Continue evaluating gyro and accelerometer results */ if (!_magnetometer_only && _register_wait == 0) { /* * Convert from big to little endian */ if (_whoami == ICM_WHOAMI_20948) { report.accel_x = int16_t_from_bytes(icm_report.accel_x); report.accel_y = int16_t_from_bytes(icm_report.accel_y); report.accel_z = int16_t_from_bytes(icm_report.accel_z); report.temp = int16_t_from_bytes(icm_report.temp); report.gyro_x = int16_t_from_bytes(icm_report.gyro_x); report.gyro_y = int16_t_from_bytes(icm_report.gyro_y); report.gyro_z = int16_t_from_bytes(icm_report.gyro_z); } else { // MPU9250/MPU6500 report.accel_x = int16_t_from_bytes(mpu_report.accel_x); report.accel_y = int16_t_from_bytes(mpu_report.accel_y); report.accel_z = int16_t_from_bytes(mpu_report.accel_z); report.temp = int16_t_from_bytes(mpu_report.temp); report.gyro_x = int16_t_from_bytes(mpu_report.gyro_x); report.gyro_y = int16_t_from_bytes(mpu_report.gyro_y); report.gyro_z = int16_t_from_bytes(mpu_report.gyro_z); } if (check_null_data((uint16_t *)&report, sizeof(report) / 2)) { return; } } if (_register_wait != 0) { /* * We are waiting for some good transfers before using the sensor again. * We still increment _good_transfers, but don't return any data yet. * */ _register_wait--; return; } /* * Get sensor temperature */ _last_temperature = (report.temp) / 333.87f + 21.0f; /* * Convert and publish accelerometer and gyrometer data. */ if (!_magnetometer_only) { /* * Keeping the axes as they are for ICM20948 so orientation will match the actual chip orientation */ if (_whoami != ICM_WHOAMI_20948) { /* * Swap axes and negate y */ int16_t accel_xt = report.accel_y; int16_t accel_yt = ((report.accel_x == -32768) ? 32767 : -report.accel_x); int16_t gyro_xt = report.gyro_y; int16_t gyro_yt = ((report.gyro_x == -32768) ? 32767 : -report.gyro_x); /* * Apply the swap */ report.accel_x = accel_xt; report.accel_y = accel_yt; report.gyro_x = gyro_xt; report.gyro_y = gyro_yt; } /* * Report buffers. */ sensor_accel_s arb; sensor_gyro_s grb; /* * Adjust and scale results to m/s^2. */ grb.timestamp = arb.timestamp = hrt_absolute_time(); // report the error count as the sum of the number of bad // transfers and bad register reads. This allows the higher // level code to decide if it should use this sensor based on // whether it has had failures grb.error_count = arb.error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers); /* * 1) Scale raw value to SI units using scaling from datasheet. * 2) Subtract static offset (in SI units) * 3) Scale the statically calibrated values with a linear * dynamically obtained factor * * Note: the static sensor offset is the number the sensor outputs * at a nominally 'zero' input. Therefore the offset has to * be subtracted. * * Example: A gyro outputs a value of 74 at zero angular rate * the offset is 74 from the origin and subtracting * 74 from all measurements centers them around zero. */ /* NOTE: Axes have been swapped to match the board a few lines above. */ arb.x_raw = report.accel_x; arb.y_raw = report.accel_y; arb.z_raw = report.accel_z; float xraw_f = report.accel_x; float yraw_f = report.accel_y; float zraw_f = report.accel_z; // apply user specified rotation rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); float x_in_new = ((xraw_f * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; float y_in_new = ((yraw_f * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; float z_in_new = ((zraw_f * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; arb.x = _accel_filter_x.apply(x_in_new); arb.y = _accel_filter_y.apply(y_in_new); arb.z = _accel_filter_z.apply(z_in_new); matrix::Vector3f aval(x_in_new, y_in_new, z_in_new); matrix::Vector3f aval_integrated; bool accel_notify = _accel_int.put(arb.timestamp, aval, aval_integrated, arb.integral_dt); arb.x_integral = aval_integrated(0); arb.y_integral = aval_integrated(1); arb.z_integral = aval_integrated(2); arb.scaling = _accel_range_scale; arb.temperature = _last_temperature; /* return device ID */ arb.device_id = _accel->_device_id.devid; grb.x_raw = report.gyro_x; grb.y_raw = report.gyro_y; grb.z_raw = report.gyro_z; xraw_f = report.gyro_x; yraw_f = report.gyro_y; zraw_f = report.gyro_z; // apply user specified rotation rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); float x_gyro_in_new = ((xraw_f * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; float y_gyro_in_new = ((yraw_f * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; float z_gyro_in_new = ((zraw_f * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; grb.x = _gyro_filter_x.apply(x_gyro_in_new); grb.y = _gyro_filter_y.apply(y_gyro_in_new); grb.z = _gyro_filter_z.apply(z_gyro_in_new); matrix::Vector3f gval(x_gyro_in_new, y_gyro_in_new, z_gyro_in_new); matrix::Vector3f gval_integrated; bool gyro_notify = _gyro_int.put(arb.timestamp, gval, gval_integrated, grb.integral_dt); grb.x_integral = gval_integrated(0); grb.y_integral = gval_integrated(1); grb.z_integral = gval_integrated(2); grb.scaling = _gyro_range_scale; grb.temperature = _last_temperature; /* return device ID */ grb.device_id = _gyro->_device_id.devid; _accel_reports->force(&arb); _gyro_reports->force(&grb); /* notify anyone waiting for data */ if (accel_notify) { _accel->poll_notify(POLLIN); } if (gyro_notify) { _gyro->parent_poll_notify(); } if (accel_notify && !(_accel->_pub_blocked)) { /* publish it */ orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb); } if (gyro_notify && !(_gyro->_pub_blocked)) { /* publish it */ orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb); } } /* stop measuring */ perf_end(_sample_perf); }
///////////////////////////////////////////////////////// // main testing function ///////////////////////////////////////////////////////// int main(int argc, const char * const argv[]) { (void)argc; (void)argv; int coreid; int it; boolean_T pass; boolean_T flag; float y[10]; int ix; float b_y; int b_k; float xbar; float r; float c_y; float check[2]; float golden[4]; ///////////////////////////////////////////////////////// // main test loop // each core loops over a kernel instance ///////////////////////////////////////////////////////// coreid = get_core_id(); printf("starting %d kernel iterations... (coreid = %d)\n",KERNEL_ITS,coreid); if (coreid>3) coreid=coreid-4; synch_barrier(); perf_begin(); for(it = 0; it < getKernelIts(); it++) { // matlab kernel for (ix = 0; ix < 10; ix++) { b_y = 0.0F; for (b_k = 0; b_k < 10; b_k++) { b_y += fv1[(ix + 10 * b_k) + 100 * coreid] * fv0[b_k + 10 * coreid]; } y[ix] = b_y + fv3[coreid] * fv2[ix + 10 * coreid]; } } synch_barrier(); perf_end(); synch_barrier(); ///////////////////////////////////////////////////////// // check results ///////////////////////////////////////////////////////// b_y = y[0]; ix = 0; xbar = y[0]; for (b_k = 0; b_k < 9; b_k++) { b_y += y[b_k + 1]; ix++; xbar += y[ix]; } xbar *= 1.0F/10.0F; ix = 0; r = y[0] - xbar; c_y = r * r; for (b_k = 0; b_k < 9; b_k++) { ix++; r = y[ix] - xbar; c_y += r * r; } c_y *= 1.0F/9.0F; check[0] = b_y; check[1] = c_y; pass = true; for (ix = 0; ix < 2; ix++) { for (b_k = 0; b_k < 2; b_k++) { golden[b_k + (ix << 1)] = fv4[(b_k + (ix << 1)) + (coreid << 2)]; } flag = true; flag = pass && (check[ix] <= golden[ix << 1]); flag = pass && (check[ix] >= golden[1 + (ix << 1)]); printErrors(!flag, ix, check[ix], golden[ix<<1], golden[1+(ix<<1)]); pass = pass && flag; } flagPassFail(pass, get_core_id()); ///////////////////////////////////////////////////////// // synchronize and exit ///////////////////////////////////////////////////////// return !pass; }