Skip to content

Commit

Permalink
feat(estimator): 添加ekf2_fake模块的支持
Browse files Browse the repository at this point in the history 
- 在Kconfig中添加了INS_USING_EKF2_FAKE的配置选项
- 引入了ekf2_fake.cpp文件，包含了ekf2_fake模块的实现
- 更新了SConscript文件，以编译新增的ekf2_fake模块
  • Loading branch information
@loengjyu
loengjyu committed Jun 16, 2024
1 parent fb6c9b9 commit 1552756
Show file tree
Hide file tree
Showing 8 changed files with 485 additions and 21 deletions.
5 changes: 5 additions & 0 deletions apps/estimator/ekf2_fake/Kconfig
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,5 @@
menuconfig INS_USING_EKF2_FAKE
bool "ekf2_fake"
default n
---help---
Enable support for ekf2 fake
12 changes: 12 additions & 0 deletions apps/estimator/ekf2_fake/SConscript
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,12 @@
import os
import sys
from building import *

cwd = GetCurrentDir()

inc = []
src = Glob("*.cpp", exclude="*_test.cpp")

objs = DefineGroup("ins/ekf2_fake", src, depend=["INS_USING_EKF2_FAKE"])

Return("objs")
270 changes: 270 additions & 0 deletions apps/estimator/ekf2_fake/ekf2_fake.cpp
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,270 @@
// /*****************************************************************
// * _ __ __ ____ _ __ __
// * / | / /___ _ __ / /_ / __ \ (_)/ /____ / /_
// * / |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
// * / /| // __/_> < / /_ / ____// // // /_/ // /_
// * /_/ |_/ \___//_/|_| \__//_/ /_//_/ \____/ \__/
// *
// * Copyright All Reserved © 2015-2024 NextPilot Development Team
// ******************************************************************/

// #define LOG_TAG "ekf2_fake"
// #define LOG_LVL LOG_LVL_INFO

// #include "nextpilot.h"

// #define EKF2_FAKE_PERIOD_MS 5

// static bool _ekf2_fake_thread_switch = false;
// static bool _inject_gps_failsafe_flag = false;

// // 订阅主题
// static orb_subsc_t sensor_gyro_sub;
// static orb_subsc_t v_acceleration_sub;
// static orb_subsc_t v_angular_velocity_sub;
// static orb_subsc_t v_attitude_sub;
// static orb_subsc_t v_local_position_sub;
// static orb_subsc_t v_global_position_sub;

// int32_t _sys_hitl = 0;

// static rt_err_t ekf2_fake_init() {
// sensor_gyro_sub = orb_subscribe(ORB_ID(sensor_gyro));
// v_acceleration_sub = orb_subscribe(ORB_ID(sensor_accel));
// v_angular_velocity_sub = orb_subscribe(ORB_ID(vehicle_angular_velocity_groundtruth));
// v_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude_groundtruth));
// v_local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position_groundtruth));
// v_global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position_groundtruth));
// return RT_EOK;
// }

// static void ekf2_fake_step() {
// hrt_abstime now_us = hrt_absolute_time();
// if (_sys_hitl != 2) {
// return;
// }

// // 发布 vehicle_angular_acceleration_s 主题
// struct sensor_gyro_s sensor_gyro = {0}; // 订阅角速度主题，用于计算角加速度
// struct vehicle_angular_acceleration_s vehicle_angular_acceleration = {0}; // 需要发布的主题
// static struct sensor_gyro_s last_senseor_gyro = {0}; // 用于差分计算角加速度
// if (orb_update(sensor_gyro_sub, &sensor_gyro) == RT_EOK) {
// vehicle_angular_acceleration.timestamp = now_us;
// vehicle_angular_acceleration.timestamp_sample = 1;
// vehicle_angular_acceleration.xyz[0] = (sensor_gyro.x - last_senseor_gyro.x) / EKF2_FAKE_PERIOD_MS;
// vehicle_angular_acceleration.xyz[1] = (sensor_gyro.y - last_senseor_gyro.y) / EKF2_FAKE_PERIOD_MS;
// vehicle_angular_acceleration.xyz[2] = (sensor_gyro.z - last_senseor_gyro.z) / EKF2_FAKE_PERIOD_MS;
// last_senseor_gyro = sensor_gyro;
// orb_publish(ORB_ID(vehicle_angular_acceleration), NULL, &vehicle_angular_acceleration);
// }

// // 发布 vehicle_acceleration_s 主题
// struct vehicle_acceleration_s v_acceleration = {0};
// if (orb_update(v_acceleration_sub, &v_acceleration) == RT_EOK) {
// struct vehicle_acceleration_s vehicle_acceleration = {0};
// vehicle_acceleration.timestamp = now_us;
// vehicle_acceleration.timestamp_sample = v_acceleration.timestamp;
// vehicle_acceleration.xyz[0] = v_acceleration.xyz[0];
// vehicle_acceleration.xyz[1] = v_acceleration.xyz[1];
// vehicle_acceleration.xyz[2] = v_acceleration.xyz[2];
// orb_publish(ORB_ID(vehicle_acceleration), NULL, &vehicle_acceleration);
// }

// // 发布 vehicle_angular_velocity_s 主题
// struct vehicle_angular_velocity_s angular_velocity_groundtruth = {0};
// if (orb_update(v_angular_velocity_sub, &angular_velocity_groundtruth) == RT_EOK) {
// struct vehicle_angular_velocity_s vehicle_angular_velocity = {0};
// vehicle_angular_velocity.timestamp = now_us;
// vehicle_angular_velocity.timestamp_sample = now_us;
// vehicle_angular_velocity.xyz[0] = angular_velocity_groundtruth.xyz[0];
// vehicle_angular_velocity.xyz[1] = angular_velocity_groundtruth.xyz[1];
// vehicle_angular_velocity.xyz[2] = angular_velocity_groundtruth.xyz[2];
// orb_publish(ORB_ID(vehicle_angular_velocity), NULL, &vehicle_angular_velocity);
// }

// // 发布 vehicle_attitude_s 主题
// struct vehicle_attitude_s attitude_groundtruth = {0};
// if (orb_update(v_attitude_sub, &attitude_groundtruth) == RT_EOK) {
// struct vehicle_attitude_s attitude = {0};
// attitude.timestamp = attitude_groundtruth.timestamp;
// attitude.timestamp_sample = attitude_groundtruth.timestamp_sample;
// attitude.q[0] = attitude_groundtruth.q[0];
// attitude.q[1] = attitude_groundtruth.q[1];
// attitude.q[2] = attitude_groundtruth.q[2];
// attitude.q[3] = attitude_groundtruth.q[3];
// orb_publish(ORB_ID(vehicle_attitude), NULL, &attitude);
// }

// // 发布 local_position_s 主题
// struct vehicle_local_position_s local_position_groundtruth = {0};
// if (orb_update(v_local_position_sub, &local_position_groundtruth) == RT_EOK) {
// struct vehicle_local_position_s local_position = {0};
// if (!_inject_gps_failsafe_flag) {
// rt_memcpy(&local_position, &local_position_groundtruth, sizeof(struct vehicle_local_position_s));
// } else {
// rt_memcpy(&local_position, &local_position_groundtruth, sizeof(struct vehicle_local_position_s));
// local_position.eph = 20.0f;
// local_position.epv = 20.0f;
// local_position.evh = 20.0f;
// local_position.evv = 20.0f;
// local_position.xy_valid = false;
// local_position.v_xy_valid = false;
// local_position.xy_global = false;
// }
// orb_publish(ORB_ID(vehicle_local_position), NULL, &local_position);
// }

// // 发布 vehicle_global_position_s 主题
// struct vehicle_global_position_s global_position_groundtruth = {0};
// if (orb_update(v_global_position_sub, &global_position_groundtruth) == RT_EOK) {
// struct vehicle_global_position_s global_position = {0};
// if (!_inject_gps_failsafe_flag) {
// rt_memcpy(&global_position, &global_position_groundtruth, sizeof(struct vehicle_global_position_s));
// } else {
// rt_memcpy(&global_position, &global_position_groundtruth, sizeof(struct vehicle_global_position_s));
// global_position.eph = 20.0f;
// global_position.epv = 20.0f;
// global_position.dead_reckoning = true;
// }
// orb_publish(ORB_ID(vehicle_global_position), NULL, &global_position);
// }
// }

// /**
// * @brief 为了启用硬件在环仿真，暂时先加该函数，避免编译报错
// *
// * @return rt_err_t
// *
// */
// rt_err_t ins2fcs_ins_calibration(void) {
// return RT_EOK;
// }

// static void ekf2_fake_run(void *parameter) {
// ekf2_fake_init();

// while (_ekf2_fake_thread_switch) {
// ekf2_fake_step();
// rt_thread_mdelay(EKF2_FAKE_PERIOD_MS);
// }
// }

// static char _ekf2_fake_thread_stack[1536];
// static struct rt_thread _ekf2_fake_thread_handle;

// static int ekf2_fake_start(void) {
// param_get(param_find("SYS_HITL"), &_sys_hitl);
// if (_sys_hitl != 2) {
// return RT_EOK;
// }

// if (_ekf2_fake_thread_switch) {
// LOG_I("is running");
// return RT_EOK;
// }

// _ekf2_fake_thread_switch = true;

// rt_err_t result = rt_thread_init(&_ekf2_fake_thread_handle,
// "ekf2_fake",
// ekf2_fake_run,
// RT_NULL,
// &_ekf2_fake_thread_stack[0],
// sizeof(_ekf2_fake_thread_stack),
// RT_THREAD_PRIORITY_RATE_CONTROL,
// 5);

// if (result != RT_EOK) {
// LOG_E("thread init failed");
// return RT_ERROR;
// }

// result = rt_thread_startup(&_ekf2_fake_thread_handle);

// if (result != RT_EOK) {
// LOG_E("thread startup failed");
// return RT_ERROR;
// } else {
// LOG_I("start ok");
// return RT_EOK;
// }
// }

// INIT_APP_MODULE_EXPORT(ekf2_fake_start);

// /* Print useage */
// static void print_help(void) {
// LOG_I("optional arguments: [start] [stop] [help] [status] [publish] [gf] [cgf] [usage]");
// LOG_I("ekf2_fake start --start module thread");
// LOG_I(" stop --stop module thread");
// LOG_I(" help --show help information");
// LOG_I(" status --list module information status and statistics");
// LOG_I(" publish --list module publish topics information");
// LOG_I(" gf --inject gps failsafe");
// LOG_I(" cgf --cancel gps failsafe");
// LOG_I(" usage --list module usage");
// }

// static void ekf2_fake_stop(void) {
// if (!_ekf2_fake_thread_switch) {
// LOG_I("not running");

// } else {
// _ekf2_fake_thread_switch = false;
// LOG_I("stop the thread");
// LOG_I("_ekf2_fake_thread_switch: %d", _ekf2_fake_thread_switch);
// }
// }

// static void print_status(void) {
// LOG_I("To do");
// }

// static void print_publish(void) {
// LOG_I("To do");
// }

// static void inject_gps_failsafe(void) {
// _inject_gps_failsafe_flag = true;
// LOG_I("inject gps failsafe");
// }

// static void cancel_gps_failsafe(void) {
// _inject_gps_failsafe_flag = false;
// LOG_I("cancel gps failsafe");
// }

// static void print_usage(void) {
// LOG_I("This thread mainly creates ekf2 fake data for sitl simulation");
// }

// static int ekf2_fake_main(int argc, char *argv[]) {
// if (argc < 2) {
// print_help();
// return RT_EOK;
// }

// if (!rt_strcmp(argv[1], "start")) {
// ekf2_fake_start();
// } else if (!rt_strcmp(argv[1], "stop")) {
// ekf2_fake_stop();
// } else if (!rt_strcmp(argv[1], "help")) {
// print_help();
// } else if (!rt_strcmp(argv[1], "status")) {
// print_status();
// } else if (!rt_strcmp(argv[1], "publish")) {
// print_publish();
// } else if (!rt_strcmp(argv[1], "gf")) {
// inject_gps_failsafe();
// } else if (!rt_strcmp(argv[1], "cgf")) {
// cancel_gps_failsafe();
// } else if (!rt_strcmp(argv[1], "usage")) {
// print_usage();
// } else {
// print_help();
// }

// return RT_EOK;
// }

// MSH_CMD_EXPORT_ALIAS(ekf2_fake_main, sim, create ekf2 fake date useage);
68 changes: 68 additions & 0 deletions apps/libraries/controller/mixer_module/output_functions.hpp
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,68 @@

#pragma once

// This file is auto-generated by generate_function_header.py from C:\Users\yu\Desktop\nextpilot-flight-control\apps\libraries\controller\mixer_module\output_functions.yaml

#include <stdint.h>

enum class OutputFunction : int32_t {
Disabled = 0,
Constant_Min = 1,
Constant_Max = 2,
Motor1 = 101,
Motor2 = 102,
Motor3 = 103,
Motor4 = 104,
Motor5 = 105,
Motor6 = 106,
Motor7 = 107,
Motor8 = 108,
Motor9 = 109,
Motor10 = 110,
Motor11 = 111,
Motor12 = 112,
MotorMax = 112,

Servo1 = 201,
Servo2 = 202,
Servo3 = 203,
Servo4 = 204,
Servo5 = 205,
Servo6 = 206,
Servo7 = 207,
Servo8 = 208,
ServoMax = 208,

Offboard_Actuator_Set1 = 301,
Offboard_Actuator_Set2 = 302,
Offboard_Actuator_Set3 = 303,
Offboard_Actuator_Set4 = 304,
Offboard_Actuator_Set5 = 305,
Offboard_Actuator_Set6 = 306,
Offboard_Actuator_SetMax = 306,

Landing_Gear = 400,
Parachute = 401,
RC_Roll = 402,
RC_Pitch = 403,
RC_Throttle = 404,
RC_Yaw = 405,
RC_Flaps = 406,
RC_AUX1 = 407,
RC_AUX2 = 408,
RC_AUX3 = 409,
RC_AUX4 = 410,
RC_AUX5 = 411,
RC_AUX6 = 412,
RC_AUXMax = 412,

Gimbal_Roll = 420,
Gimbal_Pitch = 421,
Gimbal_Yaw = 422,
Gripper = 430,
Landing_Gear_Wheel = 440,
Camera_Trigger = 2000,
Camera_Capture = 2032,
PPS_Input = 2064,

};
Loading

0 comments on commit 1552756

Please sign in to comment.