错误挺多，还未改正，请见谅。
1.setup();loop();
由HAL_PX4_Class.cpp可知，g_callbacks->setup();运行一次，g_callbacks->loop();在循环里
static int main_loop(int argc, char **argv)
{
hal.uartA->begin(115200);
hal.uartB->begin(38400);
hal.uartC->begin(57600);
hal.uartD->begin(57600);
hal.uartE->begin(57600);
hal.scheduler->init();
/*
run setup() at low priority to ensure CLI doesn't hang the
system, and to allow initial sensor read loops to run
*/
hal_px4_set_priority(APM_STARTUP_PRIORITY);
schedulerInstance.hal_initialized();
g_callbacks->setup();
hal.scheduler->system_initialized();
perf_counter_t perf_loop = perf_alloc(PC_ELAPSED, "APM_loop");
perf_counter_t perf_overrun = perf_alloc(PC_COUNT, "APM_overrun");
struct hrt_call loop_overtime_call;
thread_running = true;
/*
switch to high priority for main loop
*/
hal_px4_set_priority(APM_MAIN_PRIORITY);
while (!_px4_thread_should_exit) {
perf_begin(perf_loop);
/*
this ensures a tight loop waiting on a lower priority driver
will eventually give up some time for the driver to run. It
will only ever be called if a loop() call runs for more than
0.1 second
*/
hrt_call_after(&loop_overtime_call, 100000, (hrt_callout)loop_overtime, nullptr);
g_callbacks->loop();
if (px4_ran_overtime) {
/*
we ran over 1s in loop(), and our priority was lowered
to let a driver run. Set it back to high priority now.
*/
hal_px4_set_priority(APM_MAIN_PRIORITY);
perf_count(perf_overrun);
px4_ran_overtime = false;
}
perf_end(perf_loop);
/*
give up 250 microseconds of time, to ensure drivers get a
chance to run. This relies on the accurate semaphore wait
using hrt in semaphore.cpp
*/
hal.scheduler->delay_microseconds(250);
}
thread_running = false;
return 0;
}对应到ArduCoptor.cpp就是Copter::setup()运行一次(也就是初始化)，Copter::loop()循环运行(程序主体)。
2.init_ardupilot()注册传感器
请注意初始化里的init_ardupilot();
void Copter::setup()
{
cliSerial = hal.console;
// Load the default values of variables listed in var_info[]s
AP_Param::setup_sketch_defaults();
// setup storage layout for copter
StorageManager::set_layout_copter();
init_ardupilot();
// initialise the main loop scheduler
scheduler.init(&scheduler_tasks[0], ARRAY_SIZE(scheduler_tasks));
// setup initial performance counters
perf_info_reset();
fast_loopTimer = AP_HAL::micros();
}请注意BoardConfig.init();
void Copter::init_ardupilot()
{
if (!hal.gpio->usb_connected()) {
// USB is not connected, this means UART0 may be a Xbee, with
// its darned bricking problem. We can't write to it for at
// least one second after powering up. Simplest solution for
// now is to delay for 1 second. Something more elegant may be
// added later
delay(1000);
}
// initialise serial port
serial_manager.init_console();
// init vehicle capabilties
init_capabilities();
cliSerial->printf("\n\nInit " FIRMWARE_STRING
"\n\nFree RAM: %u\n",
(unsigned)hal.util->available_memory());
//
// Report firmware version code expect on console (check of actual EEPROM format version is done in load_parameters function)
//
report_version();
// load parameters from EEPROM
load_parameters();
// initialise stats module
g2.stats.init();
GCS_MAVLINK::set_dataflash(&DataFlash);
// identify ourselves correctly with the ground station
mavlink_system.sysid = g.sysid_this_mav;
// initialise serial ports
serial_manager.init();
// setup first port early to allow BoardConfig to report errors
gcs[0].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, 0);
// Register mavlink_delay_cb, which will run anytime you have
// more than 5ms remaining in your call to hal.scheduler->delay
hal.scheduler->register_delay_callback(mavlink_delay_cb_static, 5);
BoardConfig.init();
// init cargo gripper
#if GRIPPER_ENABLED == ENABLED
g2.gripper.init();
#endif
// initialise notify system
// disable external leds if epm is enabled because of pin conflict on the APM
notify.init(true);
// initialise battery monitor
battery.init();
// Init RSSI
rssi.init();
barometer.init();
// we start by assuming USB connected, as we initialed the serial
// port with SERIAL0_BAUD. check_usb_mux() fixes this if need be.
ap.usb_connected = true;
check_usb_mux();
// setup telem slots with serial ports
for (uint8_t i = 1; i < MAVLINK_COMM_NUM_BUFFERS; i++) {
gcs[i].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, i);
}
#if FRSKY_TELEM_ENABLED == ENABLED
// setup frsky, and pass a number of parameters to the library
frsky_telemetry.init(serial_manager, FIRMWARE_STRING " " FRAME_CONFIG_STRING,
FRAME_MAV_TYPE,
&g.fs_batt_voltage, &g.fs_batt_mah, &ap.value);
#endif
#if LOGGING_ENABLED == ENABLED
log_init();
#endif
// update motor interlock state
update_using_interlock();
#if FRAME_CONFIG == HELI_FRAME
// trad heli specific initialisation
heli_init();
#endif
init_rc_in(); // sets up rc channels from radio
init_rc_out(); // sets up motors and output to escs
// initialise which outputs Servo and Relay events can use
ServoRelayEvents.set_channel_mask(~motors.get_motor_mask());
relay.init();
/*
* setup the 'main loop is dead' check. Note that this relies on
* the RC library being initialised.
*/
hal.scheduler->register_timer_failsafe(failsafe_check_static, 1000);
// give AHRS the rnage beacon sensor
ahrs.set_beacon(&g2.beacon);
// Do GPS init
gps.init(&DataFlash, serial_manager);
if(g.compass_enabled)
init_compass();
#if OPTFLOW == ENABLED
// make optflow available to AHRS
ahrs.set_optflow(&optflow);
#endif
// init Location class
Location_Class::set_ahrs(&ahrs);
#if AP_TERRAIN_AVAILABLE && AC_TERRAIN
Location_Class::set_terrain(&terrain);
wp_nav.set_terrain(&terrain);
#endif
wp_nav.set_avoidance(&avoid);
attitude_control.parameter_sanity_check();
pos_control.set_dt(MAIN_LOOP_SECONDS);
// init the optical flow sensor
init_optflow();
#if MOUNT == ENABLED
// initialise camera mount
camera_mount.init(&DataFlash, serial_manager);
#endif
#if PRECISION_LANDING == ENABLED
// initialise precision landing
init_precland();
#endif
#ifdef USERHOOK_INIT
USERHOOK_INIT
#endif
#if CLI_ENABLED == ENABLED
if (g.cli_enabled) {
const char *msg = "\nPress ENTER 3 times to start interactive setup\n";
cliSerial->println(msg);
if (gcs[1].initialised && (gcs[1].get_uart() != nullptr)) {
gcs[1].get_uart()->println(msg);
}
if (num_gcs > 2 && gcs[2].initialised && (gcs[2].get_uart() != nullptr)) {
gcs[2].get_uart()->println(msg);
}
}
#endif // CLI_ENABLED
#if HIL_MODE != HIL_MODE_DISABLED
while (barometer.get_last_update() == 0) {
// the barometer begins updating when we get the first
// HIL_STATE message
gcs_send_text(MAV_SEVERITY_WARNING, "Waiting for first HIL_STATE message");
delay(1000);
}
// set INS to HIL mode
ins.set_hil_mode();
#endif
// read Baro pressure at ground
//-----------------------------
init_barometer(true);
// initialise rangefinder
init_rangefinder();
// init proximity sensor
init_proximity();
// init beacons used for non-gps position estimation
init_beacon();
// initialise AP_RPM library
rpm_sensor.init();
// initialise mission library
mission.init();
// initialise the flight mode and aux switch
// ---------------------------
reset_control_switch();
init_aux_switches();
startup_INS_ground();
// set landed flags
set_land_complete(true);
set_land_complete_maybe(true);
// we don't want writes to the serial port to cause us to pause
// mid-flight, so set the serial ports non-blocking once we are
// ready to fly
serial_manager.set_blocking_writes_all(false);
// enable CPU failsafe
failsafe_enable();
ins.set_raw_logging(should_log(MASK_LOG_IMU_RAW));
ins.set_dataflash(&DataFlash);
cliSerial->print("\nReady to FLY ");
// flag that initialisation has completed
ap.initialised = true;
}请注意px4_setup();
void AP_BoardConfig::init()
{
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
px4_setup();
#endif
#if HAL_HAVE_IMU_HEATER
// let the HAL know the target temperature. We pass a pointer as
// we want the user to be able to change the parameter without
// rebooting
hal.util->set_imu_target_temp((int8_t *)&_imu_target_temperature);
#endif
}请注意px4_setup_drivers();
/*
setup px4 peripherals and drivers
*/
void AP_BoardConfig::px4_setup()
{
px4_setup_peripherals();
px4_setup_pwm();
px4_setup_safety();
px4_setup_uart();
px4_setup_sbus();
px4_setup_drivers();
px4_setup_canbus();
}请注意
px4_start_driver(fmu_main, “fmu”, “sensor_reset 20”)
px4_start_common_sensors();
px4_start_fmuv2_sensors();
void AP_BoardConfig::px4_setup_drivers(void)
{
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V4)
/*
this works around an issue with some FMUv4 hardware (eg. copies
of the Pixracer) which have incorrect components leading to
sensor brownout on boot
*/
if (px4_start_driver(fmu_main, "fmu", "sensor_reset 20")) {
printf("FMUv4 sensor reset complete\n");
}
#endif
// run board auto-detection
px4_autodetect();
if (px4.board_type == PX4_BOARD_PH2SLIM ||
px4.board_type == PX4_BOARD_PIXHAWK2) {
_imu_target_temperature.set_default(60);
}
if (px4.board_type == PX4_BOARD_PX4V1 ||
px4.board_type == PX4_BOARD_PHMINI ||
px4.board_type == PX4_BOARD_PH2SLIM ||
px4.board_type == PX4_BOARD_PIXRACER ||
px4.board_type == PX4_BOARD_PIXHAWK ||
px4.board_type == PX4_BOARD_PIXHAWK2) {
// use in-tree drivers
printf("Using in-tree drivers\n");
px4_configured_board = (enum px4_board_type)px4.board_type.get();
return;
}
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
px4_start_common_sensors();
switch ((px4_board_type)px4.board_type.get()) {
case PX4_BOARD_AUTO:
default:
px4_start_fmuv1_sensors();
px4_start_fmuv2_sensors();
break;
}
#elif CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
vrx_start_common_sensors();
switch ((px4_board_type)px4.board_type.get()) {
case VRX_BOARD_BRAIN51:
vrx_start_brain51_sensors();
break;
case VRX_BOARD_BRAIN52:
vrx_start_brain52_sensors();
break;
case VRX_BOARD_UBRAIN51:
vrx_start_ubrain51_sensors();
break;
case VRX_BOARD_UBRAIN52:
vrx_start_ubrain52_sensors();
break;
case VRX_BOARD_CORE10:
vrx_start_core10_sensors();
break;
case VRX_BOARD_BRAIN54:
vrx_start_brain54_sensors();
break;
default:
break;
}
#endif // HAL_BOARD_PX4
px4_configured_board = (enum px4_board_type)px4.board_type.get();
// delay for 1 second to give time for drivers to initialise
hal.scheduler->delay(1000);
}请注意px4_start_driver(hmc5883_main, “hmc5883”, “-C -T -I -R 4 start”)等px4_start_driver()函数
void AP_BoardConfig::px4_start_fmuv2_sensors(void)
{
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
bool have_FMUV3 = false;
printf("Starting FMUv2 sensors\n");
if (px4_start_driver(hmc5883_main, "hmc5883", "-C -T -I -R 4 start")) {
printf("Have internal hmc5883\n");
} else {
printf("No internal hmc5883\n");
}
// external MPU6000 is rotated YAW_180 from standard
if (px4_start_driver(mpu6000_main, "mpu6000", "-X -R 4 start")) {
printf("Found MPU6000 external\n");
have_FMUV3 = true;
} else {
if (px4_start_driver(mpu9250_main, "mpu9250", "-X -R 4 start")) {
printf("Found MPU9250 external\n");
have_FMUV3 = true;
} else {
printf("No MPU6000 or MPU9250 external\n");
}
}
if (have_FMUV3) {
// external L3GD20 is rotated YAW_180 from standard
if (px4_start_driver(l3gd20_main, "l3gd20", "-X -R 4 start")) {
printf("l3gd20 external started OK\n");
} else {
px4_sensor_error("No l3gd20");
}
// external LSM303D is rotated YAW_270 from standard
if (px4_start_driver(lsm303d_main, "lsm303d", "-a 16 -X -R 6 start")) {
printf("lsm303d external started OK\n");
} else {
px4_sensor_error("No lsm303d");
}
// internal MPU6000 is rotated ROLL_180_YAW_270 from standard
if (px4_start_driver(mpu6000_main, "mpu6000", "-R 14 start")) {
printf("Found MPU6000 internal\n");
} else {
if (px4_start_driver(mpu9250_main, "mpu9250", "-R 14 start")) {
printf("Found MPU9250 internal\n");
} else {
px4_sensor_error("No MPU6000 or MPU9250");
}
}
if (px4_start_driver(hmc5883_main, "hmc5883", "-C -T -S -R 8 start")) {
printf("Found SPI hmc5883\n");
}
} else {
// not FMUV3 (ie. not a pixhawk2)
if (px4_start_driver(mpu6000_main, "mpu6000", "start")) {
printf("Found MPU6000\n");
} else {
if (px4_start_driver(mpu9250_main, "mpu9250", "start")) {
printf("Found MPU9250\n");
} else {
printf("No MPU6000 or MPU9250\n");
}
}
if (px4_start_driver(l3gd20_main, "l3gd20", "start")) {
printf("l3gd20 started OK\n");
} else {
px4_sensor_error("no l3gd20 found");
}
if (px4_start_driver(lsm303d_main, "lsm303d", "-a 16 start")) {
printf("lsm303d started OK\n");
} else {
px4_sensor_error("no lsm303d found");
}
}
if (have_FMUV3) {
// on Pixhawk2 default IMU temperature to 60
_imu_target_temperature.set_default(60);
}
printf("FMUv2 sensors started\n");
#endif // CONFIG_ARCH_BOARD_PX4FMU_V2
}px4_start_driver()函数原型
/*
start one px4 driver
*/
bool AP_BoardConfig::px4_start_driver(main_fn_t main_function, const char *name, const char *arguments)
{
char *s = strdup(arguments);
char *args[10];
uint8_t nargs = 0;
char *saveptr = nullptr;
// parse into separate arguments
for (char *tok=strtok_r(s, " ", &saveptr); tok; tok=strtok_r(nullptr, " ", &saveptr)) {
args[nargs++] = tok;
if (nargs == ARRAY_SIZE(args)-1) {
break;
}
}
args[nargs++] = nullptr;
printf("Starting driver %s %s\n", name, arguments);
pid_t pid;
if (task_spawn(&pid, name, main_function, nullptr, nullptr,
args, nullptr) != 0) {
free(s);
printf("Failed to spawn %s\n", name);
return false;
}
// wait for task to exit and gather status
int status = -1;
if (waitpid(pid, &status, 0) != pid) {
printf("waitpid failed for %s\n", name);
free(s);
return false;
}
free(s);
return (status >> 8) == 0;
}其中task_spawn(&pid, name, main_function, nullptr, nullptr, args, nullptr) != 0)创建了新的线程
比如px4_start_driver(hmc5883_main, “hmc5883”, “-C -T -I -R 4 start”)就映射到了px4的hmc5883驱动
int
hmc5883_main(int argc, char *argv[])
{
int ch;
enum HMC5883_BUS busid = HMC5883_BUS_ALL;
enum Rotation rotation = ROTATION_NONE;
bool calibrate = false;
bool temp_compensation = false;
while ((ch = getopt(argc, argv, "XISR:CT")) != EOF) {
switch (ch) {
case 'R':
rotation = (enum Rotation)atoi(optarg);
break;
#if (PX4_I2C_BUS_ONBOARD || PX4_SPIDEV_HMC)
case 'I':
busid = HMC5883_BUS_I2C_INTERNAL;
break;
#endif
case 'X':
busid = HMC5883_BUS_I2C_EXTERNAL;
break;
case 'S':
busid = HMC5883_BUS_SPI;
break;
case 'C':
calibrate = true;
break;
case 'T':
temp_compensation = true;
break;
default:
hmc5883::usage();
exit(0);
}
}
const char *verb = argv[optind];
/*
* Start/load the driver.
*/
if (!strcmp(verb, "start")) {
hmc5883::start(busid, rotation);
if (calibrate && hmc5883::calibrate(busid) != 0) {
errx(1, "calibration failed");
}
if (temp_compensation) {
// we consider failing to setup temperature
// compensation as non-fatal
hmc5883::temp_enable(busid, true);
}
exit(0);
}
/*
* Test the driver/device.
*/
if (!strcmp(verb, "test")) {
hmc5883::test(busid);
}
/*
* Reset the driver.
*/
if (!strcmp(verb, "reset")) {
hmc5883::reset(busid);
}
/*
* enable/disable temperature compensation
*/
if (!strcmp(verb, "tempoff")) {
hmc5883::temp_enable(busid, false);
}
if (!strcmp(verb, "tempon")) {
hmc5883::temp_enable(busid, true);
}
/*
* Print driver information.
*/
if (!strcmp(verb, "info") || !strcmp(verb, "status")) {
hmc5883::info(busid);
}
/*
* Autocalibrate the scaling
*/
if (!strcmp(verb, "calibrate")) {
if (hmc5883::calibrate(busid) == 0) {
errx(0, "calibration successful");
} else {
errx(1, "calibration failed");
}
}
errx(1, "unrecognized command, try 'start', 'test', 'reset' 'calibrate', 'tempoff', 'tempon' or 'info'");
}请注意hmc5883::start(busid, rotation);
/**
* Start the driver.
*
* This function call only returns once the driver
* is either successfully up and running or failed to start.
*/
void
start(enum HMC5883_BUS busid, enum Rotation rotation)
{
bool started = false;
for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) {
if (busid == HMC5883_BUS_ALL && bus_options[i].dev != NULL) {
// this device is already started
continue;
}
if (busid != HMC5883_BUS_ALL && bus_options[i].busid != busid) {
// not the one that is asked for
continue;
}
started |= start_bus(bus_options[i], rotation);
}
if (!started) {
exit(1);
}
}请注意started |= start_bus(bus_options[i], rotation);
/**
* start driver for a specific bus option
*/
bool
start_bus(struct hmc5883_bus_option &bus, enum Rotation rotation)
{
if (bus.dev != nullptr) {
errx(1, "bus option already started");
}
device::Device *interface = bus.interface_constructor(bus.busnum);
if (interface->init() != OK) {
delete interface;
warnx("no device on bus %u", (unsigned)bus.busid);
return false;
}
bus.dev = new HMC5883(interface, bus.devpath, rotation);
if (bus.dev != nullptr && OK != bus.dev->init()) {
delete bus.dev;
bus.dev = NULL;
return false;
}
int fd = open(bus.devpath, O_RDONLY);
if (fd < 0) {
return false;
}
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
close(fd);
errx(1, "Failed to setup poll rate");
}
close(fd);
return true;
}请注意bus.dev->init()
int
HMC5883::init()
{
int ret = ERROR;
ret = CDev::init();
if (ret != OK) {
DEVICE_DEBUG("CDev init failed");
goto out;
}
/* allocate basic report buffers */
_reports = new ringbuffer::RingBuffer(2, sizeof(mag_report));
if (_reports == nullptr) {
goto out;
}
/* reset the device configuration */
reset();
_class_instance = register_class_devname(MAG_BASE_DEVICE_PATH);
ret = OK;
/* sensor is ok, but not calibrated */
_sensor_ok = true;
out:
return ret;
}请注意_class_instance = register_class_devname(MAG_BASE_DEVICE_PATH);注册了设备路径。
返回bool start_bus(struct hmc5883_bus_option &bus, enum Rotation rotation)
int fd = open(bus.devpath, O_RDONLY);
打开设备。此时就可以直接用read、ioctl等操作了。
此处有疑问了，是ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)读取的数据，还是ardupilot里面::read()读取数据
3.读取传感器数据
请注意fast_loop();
void Copter::loop()
{
// wait for an INS sample
ins.wait_for_sample();
uint32_t timer = micros();
// check loop time
perf_info_check_loop_time(timer - fast_loopTimer);
// used by PI Loops
G_Dt = (float)(timer - fast_loopTimer) / 1000000.0f;
fast_loopTimer = timer;
// for mainloop failure monitoring
mainLoop_count++;
// Execute the fast loop
// ---------------------
fast_loop();
// tell the scheduler one tick has passed
scheduler.tick();
// run all the tasks that are due to run. Note that we only
// have to call this once per loop, as the tasks are scheduled
// in multiples of the main loop tick. So if they don't run on
// the first call to the scheduler they won't run on a later
// call until scheduler.tick() is called again
uint32_t time_available = (timer + MAIN_LOOP_MICROS) - micros();
scheduler.run(time_available > MAIN_LOOP_MICROS ? 0u : time_available);
}请注意read_AHRS();
// Main loop - 400hz
void Copter::fast_loop()
{
// IMU DCM Algorithm
// --------------------
read_AHRS();
// run low level rate controllers that only require IMU data
attitude_control.rate_controller_run();
#if FRAME_CONFIG == HELI_FRAME
update_heli_control_dynamics();
#endif //HELI_FRAME
// send outputs to the motors library
motors_output();
// Inertial Nav
// --------------------
read_inertia();
// check if ekf has reset target heading or position
check_ekf_reset();
// run the attitude controllers
update_flight_mode();
// update home from EKF if necessary
update_home_from_EKF();
// check if we've landed or crashed
update_land_and_crash_detectors();
#if MOUNT == ENABLED
// camera mount's fast update
camera_mount.update_fast();
#endif
// log sensor health
if (should_log(MASK_LOG_ANY)) {
Log_Sensor_Health();
}
}请注意ahrs.update();
void Copter::read_AHRS(void)
{
// Perform IMU calculations and get attitude info
//-----------------------------------------------
#if HIL_MODE != HIL_MODE_DISABLED
// update hil before ahrs update
gcs_check_input();
#endif
ahrs.update();
}这个地方调用了AP_AHRS_NavEKF 中的update,而不是AP_AHRS_DCM 的updata
void AP_AHRS_NavEKF::update(void)
{
#if !AP_AHRS_WITH_EKF1
if (_ekf_type == 1) {
_ekf_type.set(2);
}
#endif
update_DCM();
#if AP_AHRS_WITH_EKF1
update_EKF1();
#endif
update_EKF2();
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
update_SITL();
#endif
// call AHRS_update hook if any
AP_Module::call_hook_AHRS_update(*this);
}这里update_DCM();直接调用了AP_AHRS_DCM 的updata
void AP_AHRS_NavEKF::update_DCM(void)
{
// we need to restore the old DCM attitude values as these are
// used internally in DCM to calculate error values for gyro drift
// correction
roll = _dcm_attitude.x;
pitch = _dcm_attitude.y;
yaw = _dcm_attitude.z;
update_cd_values();
AP_AHRS_DCM::update();
// keep DCM attitude available for get_secondary_attitude()
_dcm_attitude(roll, pitch, yaw);
}接着AP_AHRS_DCM::update();
// run a full DCM update round
void
AP_AHRS_DCM::update(void)
{
float delta_t;
if (_last_startup_ms == 0) {
_last_startup_ms = AP_HAL::millis();
}
// tell the IMU to grab some data
_ins.update();
// ask the IMU how much time this sensor reading represents
delta_t = _ins.get_delta_time();
// if the update call took more than 0.2 seconds then discard it,
// otherwise we may move too far. This happens when arming motors
// in ArduCopter
if (delta_t > 0.2f) {
memset(&_ra_sum[0], 0, sizeof(_ra_sum));
_ra_deltat = 0;
return;
}
// Integrate the DCM matrix using gyro inputs
matrix_update(delta_t);
// Normalize the DCM matrix
normalize();
// Perform drift correction
drift_correction(delta_t);
// paranoid check for bad values in the DCM matrix
check_matrix();
// Calculate pitch, roll, yaw for stabilization and navigation
euler_angles();
// update trig values including _cos_roll, cos_pitch
update_trig();
}请注意
// tell the IMU to grab some data
_ins.update();/*
update gyro and accel values from backends
*/
void AP_InertialSensor::update(void)
{
// during initialisation update() may be called without
// wait_for_sample(), and a wait is implied
wait_for_sample();
if (!_hil_mode) {
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
// mark sensors unhealthy and let update() in each backend
// mark them healthy via _publish_gyro() and
// _publish_accel()
_gyro_healthy[i] = false;
_accel_healthy[i] = false;
_delta_velocity_valid[i] = false;
_delta_angle_valid[i] = false;
}
for (uint8_t i=0; i<_backend_count; i++) {
_backends[i]->update();
}
// clear accumulators
for (uint8_t i = 0; i < INS_MAX_INSTANCES; i++) {
_delta_velocity_acc[i].zero();
_delta_velocity_acc_dt[i] = 0;
_delta_angle_acc[i].zero();
_delta_angle_acc_dt[i] = 0;
}
if (!_startup_error_counts_set) {
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
_accel_startup_error_count[i] = _accel_error_count[i];
_gyro_startup_error_count[i] = _gyro_error_count[i];
}
if (_startup_ms == 0) {
_startup_ms = AP_HAL::millis();
} else if (AP_HAL::millis()-_startup_ms > 2000) {
_startup_error_counts_set = true;
}
}
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
if (_accel_error_count[i] < _accel_startup_error_count[i]) {
_accel_startup_error_count[i] = _accel_error_count[i];
}
if (_gyro_error_count[i] < _gyro_startup_error_count[i]) {
_gyro_startup_error_count[i] = _gyro_error_count[i];
}
}
// adjust health status if a sensor has a non-zero error count
// but another sensor doesn't.
bool have_zero_accel_error_count = false;
bool have_zero_gyro_error_count = false;
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
if (_accel_healthy[i] && _accel_error_count[i] <= _accel_startup_error_count[i]) {
have_zero_accel_error_count = true;
}
if (_gyro_healthy[i] && _gyro_error_count[i] <= _gyro_startup_error_count[i]) {
have_zero_gyro_error_count = true;
}
}
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
if (_gyro_healthy[i] && _gyro_error_count[i] > _gyro_startup_error_count[i] && have_zero_gyro_error_count) {
// we prefer not to use a gyro that has had errors
_gyro_healthy[i] = false;
}
if (_accel_healthy[i] && _accel_error_count[i] > _accel_startup_error_count[i] && have_zero_accel_error_count) {
// we prefer not to use a accel that has had errors
_accel_healthy[i] = false;
}
}
// set primary to first healthy accel and gyro
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
if (_gyro_healthy[i] && _use[i]) {
_primary_gyro = i;
break;
}
}
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
if (_accel_healthy[i] && _use[i]) {
_primary_accel = i;
break;
}
}
}
_have_sample = false;
}请注意_backends[i]->update();
update()是虚函数
那么需要找update()所在类的继承类，搜索public AP_InertialSensor_Backend
有很多继承类，比如MPU6000
/*
publish any pending data
*/
bool AP_InertialSensor_MPU6000::update()
{
update_accel(_accel_instance);
update_gyro(_gyro_instance);
_publish_temperature(_accel_instance, _temp_filtered);
return true;
}px4
bool AP_InertialSensor_PX4::update(void)
{
// get the latest sample from the sensor drivers
_get_sample();
for (uint8_t k=0; k<_num_accel_instances; k++) {
update_accel(_accel_instance[k]);
}
for (uint8_t k=0; k<_num_gyro_instances; k++) {
update_gyro(_gyro_instance[k]);
}
return true;
}那么需要找谁给_backends[i]赋了值
搜索_backends可找到_add_backend()
bool AP_InertialSensor::_add_backend(AP_InertialSensor_Backend *backend)
{
if (!backend)
return false;
if (_backend_count == INS_MAX_BACKENDS)
AP_HAL::panic("Too many INS backends");
_backends[_backend_count++] = backend;
return true;
}那么看哪里调用了_add_backend()
/*
detect available backends for this board
*/
void
AP_InertialSensor::detect_backends(void)
{
if (_backends_detected)
return;
_backends_detected = true;
if (_hil_mode) {
_add_backend(AP_InertialSensor_HIL::detect(*this));
return;
}
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
_add_backend(AP_InertialSensor_SITL::detect(*this));
#elif HAL_INS_DEFAULT == HAL_INS_HIL
_add_backend(AP_InertialSensor_HIL::detect(*this));
#elif HAL_INS_DEFAULT == HAL_INS_MPU60XX_SPI && defined(HAL_INS_DEFAULT_ROTATION)
_add_backend(AP_InertialSensor_MPU6000::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME),
HAL_INS_DEFAULT_ROTATION));
#elif HAL_INS_DEFAULT == HAL_INS_MPU60XX_SPI
_add_backend(AP_InertialSensor_MPU6000::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME)));
#elif HAL_INS_DEFAULT == HAL_INS_MPU60XX_I2C && defined(HAL_INS_DEFAULT_ROTATION)
_add_backend(AP_InertialSensor_MPU6000::probe(*this, hal.i2c_mgr->get_device(HAL_INS_MPU60x0_I2C_BUS, HAL_INS_MPU60x0_I2C_ADDR),
HAL_INS_DEFAULT_ROTATION));
#elif HAL_INS_DEFAULT == HAL_INS_MPU60XX_I2C
_add_backend(AP_InertialSensor_MPU6000::probe(*this, hal.i2c_mgr->get_device(HAL_INS_MPU60x0_I2C_BUS, HAL_INS_MPU60x0_I2C_ADDR)));
#elif HAL_INS_DEFAULT == HAL_INS_BH
_add_backend(AP_InertialSensor_MPU6000::probe(*this, hal.i2c_mgr->get_device(HAL_INS_MPU60x0_I2C_BUS, HAL_INS_MPU60x0_I2C_ADDR)));
_add_backend(AP_InertialSensor_MPU9250::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME)));
#elif HAL_INS_DEFAULT == HAL_INS_PX4 || HAL_INS_DEFAULT == HAL_INS_VRBRAIN
if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PX4V1) {
_add_backend(AP_InertialSensor_MPU6000::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME)));
} else if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PIXHAWK) {
if (!_add_backend(AP_InertialSensor_MPU6000::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME), ROTATION_ROLL_180))) {
// handle pixfalcon with mpu9250 instead of mpu6000
_fast_sampling_mask.set_default(1);
_add_backend(AP_InertialSensor_MPU9250::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), ROTATION_ROLL_180));
}
_add_backend(AP_InertialSensor_LSM9DS0::probe(*this,
hal.spi->get_device(HAL_INS_LSM9DS0_G_NAME),
hal.spi->get_device(HAL_INS_LSM9DS0_A_NAME),
ROTATION_ROLL_180, ROTATION_ROLL_180_YAW_270));
} else if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PIXHAWK2) {
// older Pixhawk2 boards have the MPU6000 instead of MPU9250
_fast_sampling_mask.set_default(1);
if (!_add_backend(AP_InertialSensor_MPU9250::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_EXT_NAME), ROTATION_PITCH_180))) {
_add_backend(AP_InertialSensor_MPU6000::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_EXT_NAME), ROTATION_PITCH_180));
}
_add_backend(AP_InertialSensor_LSM9DS0::probe(*this,
hal.spi->get_device(HAL_INS_LSM9DS0_EXT_G_NAME),
hal.spi->get_device(HAL_INS_LSM9DS0_EXT_A_NAME),
ROTATION_ROLL_180_YAW_270, ROTATION_ROLL_180_YAW_90));
if (!_add_backend(AP_InertialSensor_MPU9250::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), ROTATION_YAW_270))) {
_add_backend(AP_InertialSensor_MPU6000::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME), ROTATION_YAW_270));
}
}
......
可以找到else if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PIXHAWK2)判断
AP_InertialSensor_MPU6000::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME), ROTATION_YAW_270)返回AP_InertialSensor_Backend类的对象，也就是操作传感器的后端对象
我们使用的是px4，所以可以找到update()的实例化
(先岔开一会，想看看哪里调用了void AP_InertialSensor::detect_backends(void)，请看最下面第4点)
AP_InertialSensor_PX4::update(void) 是update()的实现
bool AP_InertialSensor_PX4::update(void)
{
// get the latest sample from the sensor drivers
_get_sample();
for (uint8_t k=0; k<_num_accel_instances; k++) {
update_accel(_accel_instance[k]);
}
for (uint8_t k=0; k<_num_gyro_instances; k++) {
update_gyro(_gyro_instance[k]);
}
return true;
}请注意_get_sample();
void AP_InertialSensor_PX4::_get_sample()
{
for (uint8_t i=0; i<MAX(_num_accel_instances,_num_gyro_instances);i++) {
struct accel_report accel_report;
struct gyro_report gyro_report;
bool gyro_valid = _get_gyro_sample(i,gyro_report);
bool accel_valid = _get_accel_sample(i,accel_report);
while(gyro_valid || accel_valid) {
// interleave accel and gyro samples by time - this will allow sculling corrections later
// check the next gyro measurement to see if it needs to be integrated first
if(gyro_valid && accel_valid && gyro_report.timestamp <= accel_report.timestamp) {
_new_gyro_sample(i,gyro_report);
gyro_valid = _get_gyro_sample(i,gyro_report);
continue;
}
// if not, try to integrate an accelerometer sample
if(accel_valid) {
_new_accel_sample(i,accel_report);
accel_valid = _get_accel_sample(i,accel_report);
continue;
}
// if not, we've only got gyro samples left in the buffer
if(gyro_valid) {
_new_gyro_sample(i,gyro_report);
gyro_valid = _get_gyro_sample(i,gyro_report);
}
}
}
}请注意
bool gyro_valid = _get_gyro_sample(i,gyro_report);
bool accel_valid = _get_accel_sample(i,accel_report);
bool AP_InertialSensor_PX4::_get_gyro_sample(uint8_t i, struct gyro_report &gyro_report)
{
if (i<_num_gyro_instances &&
_gyro_fd[i] != -1 &&
::read(_gyro_fd[i], &gyro_report, sizeof(gyro_report)) == sizeof(gyro_report) &&
gyro_report.timestamp != _last_gyro_timestamp[i]) {
return true;
}
return false;
}请注意 ::read(_gyro_fd[i], &gyro_report, sizeof(gyro_report))
根据2中_class_instance = register_class_devname(PATH);注册设备路径，可以根据_gyro_fd描述符直接调用
ssize_t
MPU6000::read(struct file *filp, char *buffer, size_t buflen)
{
unsigned count = buflen / sizeof(accel_report);
/* buffer must be large enough */
if (count < 1) {
return -ENOSPC;
}
/* if automatic measurement is not enabled, get a fresh measurement into the buffer */
if (_call_interval == 0) {
_accel_reports->flush();
measure();
}
/* if no data, error (we could block here) */
if (_accel_reports->empty()) {
return -EAGAIN;
}
perf_count(_accel_reads);
/* copy reports out of our buffer to the caller */
accel_report *arp = reinterpret_cast<accel_report *>(buffer);
int transferred = 0;
while (count--) {
if (!_accel_reports->get(arp)) {
break;
}
transferred++;
arp++;
}
/* return the number of bytes transferred */
return (transferred * sizeof(accel_report));
}于是读出数据。
疑问：是ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)读取的数据，还是ardupilot里面::read()读取数据？还是他们都可以读取数据？
4.startup_INS_ground()
void Copter::setup()
|
|
init_ardupilot();
\
\__void Copter::init_ardupilot()
|
|
startup_INS_ground();
|
|
void Copter::startup_INS_ground()
|
\
\_____ins.init(scheduler.get_loop_rate_hz());
|
|
void AP_InertialSensor::init(uint16_t sample_rate)
|
|
void AP_InertialSensor::_start_backends()
|
|
void AP_InertialSensor::detect_backends(void)
|
|
_add_backend(AP_InertialSensor_PX4::detect(*this));
|
|
\
\_____AP_InertialSensor_Backend *AP_InertialSensor_PX4::detect(AP_InertialSensor &_imu)
|
|
bool AP_InertialSensor_PX4::_init_sensor(void) 您的鼓励将是笔者书写高质量文章的最大动力^_^!!
4774
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
