xiaoxiang781216 commented on code in PR #2113: URL: https://github.com/apache/nuttx-apps/pull/2113#discussion_r1363280347
########## examples/sensor_fusion/sensor_fusion_main.c: ########## @@ -0,0 +1,162 @@ +/**************************************************************************** + * apps/examples/sensor_fusion/sensor_fusion_main.c + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> +#include <stdio.h> +#include <fcntl.h> +#include <unistd.h> +#include <nuttx/sensors/sensor.h> +#include "Fusion/Fusion.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +#define REG_LOW_MASK 0xFF00 +#define REG_HIGH_MASK 0x00FF +#define MPU6050_FS_SEL 32.8f +#define MPU6050_AFS_SEL 4096.0f + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +struct mpu6050_imu_msg +{ + int16_t acc_x; + int16_t acc_y; + int16_t acc_z; + int16_t temp; + int16_t gyro_x; + int16_t gyro_y; + int16_t gyro_z; +}; + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +void read_mpu6050(int fd, struct sensor_accel *acc_data, + struct sensor_gyro *gyro_data); + +/**************************************************************************** + * sensor_fusion_main + ****************************************************************************/ + +int main(int argc, FAR char *argv[]) +{ + int fd; + int iterations = CONFIG_EXAMPLES_SENSOR_FUSION_SAMPLES; + float acq_period = CONFIG_EXAMPLES_SENSOR_FUSION_SAMPLE_RATE / 1000.0f; + + /* Minimal required data for Fusion library. Use of magnetometer data + * is optional and can improve performance. + */ + + struct sensor_accel imu_acc_data; + struct sensor_gyro imu_gyro_data; + FusionVector accelerometer; + FusionVector gyroscope; + FusionEuler euler; + FusionAhrs ahrs; + + FusionAhrsInitialise(&ahrs); + + printf("Sensor Fusion example\n"); + printf("Sample Rate: %.2f Hz\n", 1.0 / acq_period); + + fd = open("/dev/imu0", O_RDONLY); + if (fd < 0) + { + printf("Failed to open imu0\n"); + return EXIT_FAILURE; + } + + for (int i = 0; i < iterations; i++) + { + read_mpu6050(fd, &imu_acc_data, &imu_gyro_data); + + accelerometer.axis.x = imu_acc_data.x; + accelerometer.axis.y = imu_acc_data.y; + accelerometer.axis.z = imu_acc_data.z; + + gyroscope.axis.x = imu_gyro_data.x; + gyroscope.axis.y = imu_gyro_data.y; + gyroscope.axis.z = imu_gyro_data.z; + + FusionAhrsUpdateNoMagnetometer(&ahrs, + gyroscope, + accelerometer, + acq_period); + euler = FusionQuaternionToEuler(FusionAhrsGetQuaternion(&ahrs)); + + printf("Yaw: %.3f | Pitch: %.3f | Roll: %.3f\n", + euler.angle.yaw, euler.angle.pitch, euler.angle.roll); + usleep(CONFIG_EXAMPLES_SENSOR_FUSION_SAMPLE_RATE * 1000); + } + + close(fd); + + return EXIT_SUCCESS; +} + +void read_mpu6050(int fd, + struct sensor_accel *acc_data, + struct sensor_gyro *gyro_data) +{ + struct mpu6050_imu_msg raw_imu; + memset(&raw_imu, 0, sizeof(raw_imu)); Review Comment: move after line 131 ########## examples/sensor_fusion/sensor_fusion_main.c: ########## @@ -0,0 +1,162 @@ +/**************************************************************************** + * apps/examples/sensor_fusion/sensor_fusion_main.c + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> +#include <stdio.h> +#include <fcntl.h> +#include <unistd.h> +#include <nuttx/sensors/sensor.h> +#include "Fusion/Fusion.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +#define REG_LOW_MASK 0xFF00 +#define REG_HIGH_MASK 0x00FF +#define MPU6050_FS_SEL 32.8f +#define MPU6050_AFS_SEL 4096.0f + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +struct mpu6050_imu_msg +{ + int16_t acc_x; + int16_t acc_y; + int16_t acc_z; + int16_t temp; + int16_t gyro_x; + int16_t gyro_y; + int16_t gyro_z; +}; + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +void read_mpu6050(int fd, struct sensor_accel *acc_data, + struct sensor_gyro *gyro_data); + +/**************************************************************************** + * sensor_fusion_main + ****************************************************************************/ + +int main(int argc, FAR char *argv[]) Review Comment: remove FAR, or add FAR to ALL pointer type -- This is an automated message from the Apache Git Service. To respond to the message, please log on to GitHub and use the URL above to go to the specific comment. To unsubscribe, e-mail: commits-unsubscr...@nuttx.apache.org For queries about this service, please contact Infrastructure at: us...@infra.apache.org
