gg/fts-can
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

mfts + afts basic geofence demo working ok

Browse Source
This commit is contained in:
neo 2024-06-08 18:58:04 +02:00
parent 7deb22de4c
commit 981936ab3f
13 changed files with 79 additions and 195 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ROMFS/px4fmu_common/init.d/airframes/0001_main_fts
View File

@ -24,3 +24,4 @@ param set-default FTS_SIM_MODE 0
param set-default MAV_TYPE 37
fts start
fts_controller start

7
ROMFS/px4fmu_common/init.d/airframes/0002_auto_fts
View File

@ -22,3 +22,10 @@
# param set-default FTS_SIM_MODE 0
param set-default MAV_TYPE 37
param set-default GF_ACTION 0
param set-default GF_ALTMODE 0
param set-default GF_COUNT -1
param set-default GF_MAX_HOR_DIST 0.0000
param set-default GF_MAX_VER_DIST 0.0000
param set-default GF_PREDICT 0
param set-default GF_SOURCE 1

3
boards/px4/fts-can/nuttx-config/nsh/defconfig
View File

@ -204,9 +204,6 @@ CONFIG_STM32H7_SAVE_CRASHDUMP=y
CONFIG_STM32H7_SDMMC2=y
CONFIG_STM32H7_SERIALBRK_BSDCOMPAT=y
CONFIG_STM32H7_SERIAL_DISABLE_REORDERING=y
CONFIG_STM32H7_SPI1=y
CONFIG_STM32H7_SPI1_DMA=y
CONFIG_STM32H7_SPI1_DMA_BUFFER=1024
CONFIG_STM32H7_SPI2=y
CONFIG_STM32H7_SPI2_DMA=y
CONFIG_STM32H7_SPI2_DMA_BUFFER=4096

16
boards/px4/fts-can/src/spi.cpp
View File

@ -32,36 +32,20 @@
****************************************************************************/
#include <px4_arch/spi_hw_description.h>
#include <drivers/drv_sensor.h>
#include <nuttx/spi/spi.h>
constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSIONS] = {
initSPIHWVersion(V6C00, {
initSPIBus(SPI::Bus::SPI1, {
initSPIDevice(DRV_GYR_DEVTYPE_BMI055, SPI::CS{GPIO::PortC, GPIO::Pin14}, SPI::DRDY{GPIO::PortE, GPIO::Pin5}),
initSPIDevice(DRV_ACC_DEVTYPE_BMI055, SPI::CS{GPIO::PortC, GPIO::Pin15}, SPI::DRDY{GPIO::PortE, GPIO::Pin4}),
initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortC, GPIO::Pin13}, SPI::DRDY{GPIO::PortE, GPIO::Pin6}),
}, {GPIO::PortB, GPIO::Pin2}),
initSPIBus(SPI::Bus::SPI2, {
initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortD, GPIO::Pin4})
}),
}),
initSPIHWVersion(V6C01, {
initSPIBus(SPI::Bus::SPI1, {
initSPIDevice(DRV_GYR_DEVTYPE_BMI055, SPI::CS{GPIO::PortC, GPIO::Pin14}, SPI::DRDY{GPIO::PortE, GPIO::Pin5}),
initSPIDevice(DRV_ACC_DEVTYPE_BMI055, SPI::CS{GPIO::PortC, GPIO::Pin15}, SPI::DRDY{GPIO::PortE, GPIO::Pin4}),
initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortC, GPIO::Pin13}, SPI::DRDY{GPIO::PortE, GPIO::Pin6}),
}, {GPIO::PortB, GPIO::Pin2}),
initSPIBus(SPI::Bus::SPI2, {
initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortD, GPIO::Pin4})
}),
}),
initSPIHWVersion(V6C21, {
initSPIBus(SPI::Bus::SPI1, {
initSPIDevice(DRV_GYR_DEVTYPE_BMI055, SPI::CS{GPIO::PortC, GPIO::Pin14}, SPI::DRDY{GPIO::PortE, GPIO::Pin5}),
initSPIDevice(DRV_ACC_DEVTYPE_BMI055, SPI::CS{GPIO::PortC, GPIO::Pin15}, SPI::DRDY{GPIO::PortE, GPIO::Pin4}),
initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortC, GPIO::Pin13}, SPI::DRDY{GPIO::PortE, GPIO::Pin6}),
}, {GPIO::PortB, GPIO::Pin2}),
initSPIBus(SPI::Bus::SPI2, {
initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortD, GPIO::Pin4})
}),

43
src/drivers/fts/fts.cpp
View File

@ -7,6 +7,7 @@
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/log.h>
#include <drivers/drv_hrt.h>
#include <drivers/fts/fts.h>
@ -73,26 +74,6 @@ px4_arch_configgpio(GPIO_FTS_CAN_OUTPUT);
px4_arch_configgpio(GPIO_FTS_CHUTE_OUTPUT);
}
void FTS::fts_arm()
{
FTS_ARM_NEGATED(false);
}
void FTS::fts_disarm()
{
FTS_ARM_NEGATED(true);
}
void FTS::fts_off()
{
FTS_TERMINATE_CAN(false);
}
void FTS::fts_on()
{
FTS_TERMINATE_CAN(true);
}
bool FTS::initialize_topics()
{
return true;
@ -171,3 +152,25 @@ extern "C" __EXPORT int fts_main(int argc, char *argv[])
{
return FTS::main(argc, argv);
}
void FTS::fts_arm()
{
FTS_ARM_NEGATED(false);
}
void FTS::fts_disarm()
{
FTS_ARM_NEGATED(true);
}
void FTS::fts_off()
{
FTS_TERMINATE_CAN(false);
FTS_TRIGGER_CHUTE(false);
}
void FTS::fts_on()
{
FTS_TERMINATE_CAN(true);
FTS_TRIGGER_CHUTE(true);
}

10
src/drivers/fts/fts.h
View File

@ -58,16 +58,16 @@ public:
*/
int start();
void fts_on();
void fts_off();
void fts_arm();
void fts_disarm();
private:
/** Disables the fts */
void disable_fts();
void fts_on();
void fts_off();
void fts_arm();
void fts_disarm();
void initialize();
/** Enables / configures the fts GPIO */

1
src/modules/fts_control/CMakeLists.txt
View File

@ -3,5 +3,4 @@ px4_add_module(
MAIN fts_controller
SRCS
fts_control.cpp
main_state_machine.cpp
)

29
src/modules/fts_control/fts_control.cpp
View File

@ -31,7 +31,6 @@ FTSController::init()
return false;
}
//_main_state_machine.init_state_machine(&_fts_ground_cmd_reply_pub, &_fts_control_cmd_pub, &_fts_state);
// run every 1s
ScheduleOnInterval(1000_ms);
return true;
@ -55,14 +54,18 @@ FTSController::Run()
// Update FTS comm status
update_status();
// Excecute the state machine
_main_state_machine.run_state_machine();
// Update AUTO-FTS status
if(_auto_fts_message_sub.updated()){
_auto_fts_message_sub.copy(&_auto_fts_message);
if (_auto_fts_message.geofence_violated!=0){
fire();
}else{
reset();
}
}
// Publish FTS controller state
_fts_state.timestamp = hrt_absolute_time();
_fts_state.afts_comm_timeout = 0;
_fts_state.main_fts_state = 0;
//_fts_state_pub.publish(_fts_state);
perf_end(_cycle_perf);
}
@ -72,11 +75,6 @@ void FTSController::update_status()
// Update FTS state
// Update GROUND-FTS communication state
_fts_state.gfts_comm_timeout = hrt_elapsed_time(&_last_fts_state_update_time) >= _fts_comm_timeout_threshold_us;
// Update AUTO-FTS communication state
_fts_state.afts_comm_timeout = hrt_elapsed_time(&_last_fts_state_update_time) >= _fts_comm_timeout_threshold_us;
}
void FTSController::send_fts_state()
@ -87,9 +85,7 @@ void FTSController::send_fts_state()
void
FTSController::update_topics()
{
if (_fts_state_sub.update()) {
_last_fts_state_update_time = _fts_state_sub.get().timestamp;
}
_fts_state_pub.publish(_fts_state);
}
@ -135,13 +131,13 @@ FTSController::task_spawn(int argc, char *argv[])
void
FTSController::fire()
{
px4_arch_gpiowrite(GPIO_OUTPUT|5, true);
fts_drv.fts_on();
}
void
FTSController::reset()
{
px4_arch_gpiowrite(GPIO_OUTPUT|5, false);
fts_drv.fts_off();
}
int
@ -166,7 +162,6 @@ FTSController::print_status()
perf_print_counter(_cycle_perf);
perf_print_counter(_interval_perf);
PX4_INFO("Current state machine state:%u", _main_state_machine.get_mainstatemachine_state());
PX4_INFO("Current FTS state:%u", _fts_state.main_fts_state);
return 0;

15
src/modules/fts_control/fts_control.hpp
View File

@ -14,12 +14,11 @@
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/fts_state.h>
#include <uORB/topics/auto_fts_message.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/vehicle_local_position.h>
#include "main_state_machine.hpp"
#include <drivers/fts/fts.h>
using namespace time_literals;
class FTSController : public ModuleBase<FTSController>, public ModuleParams, public px4::ScheduledWorkItem
@ -56,15 +55,17 @@ private:
perf_counter_t _interval_perf;
uORB::SubscriptionData<vehicle_status_s> _vehicle_status_sub{ORB_ID(vehicle_status)};
uORB::SubscriptionData<fts_state_s> _fts_state_sub{ORB_ID(fts_state)};
uORB::SubscriptionData<auto_fts_message_s> _auto_fts_message_sub{ORB_ID(auto_fts_message)};
uORB::SubscriptionData<home_position_s> _home_position_sub{ORB_ID(home_position)};
uORB::SubscriptionData<vehicle_local_position_s> _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
uORB::Publication<fts_state_s> _fts_state_pub{ORB_ID(fts_state)};
hrt_abstime _last_fts_state_update_time{0};
FTS fts_drv;
hrt_abstime _last_fts_state_update_time{0};
MainStateMachine _main_state_machine;
MainStateMachine::fts_state_s _fts_state{};
fts_state_s _fts_state{};
auto_fts_message_s _auto_fts_message{};
uint64_t _fts_comm_timeout_threshold_us{10_s}; // Currently the box emits state at 3 Hz
};

63
src/modules/fts_control/main_state_machine.cpp
View File

@ -1,63 +0,0 @@
#include "main_state_machine.hpp"
MainStateMachine::MainStateMachine()
{
}
uint8_t MainStateMachine::get_mainstatemachine_state()
{
return 0;
}
void MainStateMachine::init_state_machine(uORB::Publication<fts_state_s> *fts_state_pub,
MainStateMachine::fts_state_s *FTS_state)
{
}
void MainStateMachine::send_cmd_ack(uint8_t cmd_result)
{
//ground_fts_reply_s msg{};
//msg.timestamp = hrt_absolute_time();
//msg.cmd_result = cmd_result;
//_ground_cmd_reply_pub->publish(msg);
}
bool MainStateMachine::send_fts_cmd(uint8_t cmd)
{
return true;
}
bool MainStateMachine::check_ground_fts_timeout()
{
if (hrt_elapsed_time(&_last_mfts_ping_time) > FTS_PING_TIMEOUT) {
return true;
}
return false;
}
bool MainStateMachine::check_auto_fts_timeout()
{
if (hrt_elapsed_time(&_last_afts_ping_time) > FTS_PING_TIMEOUT) {
return true;
}
return false;
}
void MainStateMachine::run_state_machine()
{
switch (0) {
case 0:
break;
case 1:
break;
case 2:
break;
}
}

47
src/modules/fts_control/main_state_machine.hpp
View File

@ -1,47 +0,0 @@
#ifndef PX4_MAIN_STATE_MACHINE_HPP
#define PX4_MAIN_STATE_MACHINE_HPP
#define FTS_PING_TIMEOUT 1000
#include <px4_platform_common/module.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/fts_state.h>
#include <uORB/Publication.hpp>
using namespace time_literals;
class MainStateMachine : public ModuleBase<MainStateMachine>
{
public:
struct fts_state_s {
uint64_t timestamp{0};
bool gfts_comm_timeout{false};
bool afts_comm_timeout{false};
uint8_t ground_fts_state{0};
uint8_t auto_fts_state{0};
uint8_t main_fts_state{0};
};
MainStateMachine();
void init_state_machine(uORB::Publication<fts_state_s> *fts_state_pub,
MainStateMachine::fts_state_s *FTS_state);
void run_state_machine();
uint8_t get_mainstatemachine_state();
void send_cmd_ack(uint8_t cmd_result);
bool send_fts_cmd(uint8_t cmd);
bool check_ground_fts_timeout();
bool check_auto_fts_timeout();
private:
hrt_abstime _last_mfts_ping_time{0};
hrt_abstime _last_afts_ping_time{0};
uORB::Publication<fts_state_s> *_fts_state_pub{nullptr};
static constexpr uint64_t FTS_STATE_SEND_INTERVAL = 200_ms; // Limit the publication of the FTS state at 5 Hz
};
#endif //PX4_MAIN_STATE_MACHINE_HPP

12
src/modules/mavlink/streams/AUTO_FTS_MESSAGE.hpp
View File

@ -37,7 +37,12 @@ public:
const char *get_name() const override { return get_name_static(); }
uint16_t get_id() override { return get_id_static(); }
unsigned get_size() override
uORB::Subscription _geofence_sub{ORB_ID(geofence_result)};
hrt_abstime _last_reset_time{0};
hrt_abstime _last_update_time{0};
unsigned get_size() override
{
return MAVLINK_MSG_ID_AUTO_FTS_MESSAGE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
}
@ -96,11 +101,6 @@ private:
msg.geofence_violated = 0;
}
uORB::Subscription _geofence_sub{ORB_ID(geofence_result)};
hrt_abstime _last_reset_time{0};
hrt_abstime _last_update_time{0};
};
#endif

25
src/modules/navigator/navigator_main.cpp
View File

@ -923,27 +923,27 @@ void Navigator::geofence_breach_check(bool &have_geofence_position_data)
matrix::Vector2<double> fence_violation_test_point;
geofence_violation_type_u gf_violation_type{};
float test_point_bearing;
float test_point_distance;
float test_point_bearing = 0;
float test_point_distance = 0;
float vertical_test_point_distance;
if (_vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
test_point_bearing = atan2f(_local_pos.vy, _local_pos.vx);
//test_point_bearing = atan2f(_local_pos.vy, _local_pos.vx);
const float velocity_hor_abs = sqrtf(_local_pos.vx * _local_pos.vx + _local_pos.vy * _local_pos.vy);
_gf_breach_avoidance.setHorizontalVelocity(velocity_hor_abs);
_gf_breach_avoidance.setClimbRate(-_local_pos.vz);
test_point_distance = _gf_breach_avoidance.computeBrakingDistanceMultirotor();
//test_point_distance = _gf_breach_avoidance.computeBrakingDistanceMultirotor();
vertical_test_point_distance = _gf_breach_avoidance.computeVerticalBrakingDistanceMultirotor();
} else {
test_point_distance = 2.0f * get_loiter_radius();
//test_point_distance = 2.0f * get_loiter_radius();
vertical_test_point_distance = 5.0f;
if (hrt_absolute_time() - pos_ctrl_status.timestamp < 100000 && PX4_ISFINITE(pos_ctrl_status.nav_bearing)) {
test_point_bearing = pos_ctrl_status.nav_bearing;
//test_point_bearing = pos_ctrl_status.nav_bearing;
} else {
test_point_bearing = atan2f(_local_pos.vy, _local_pos.vx);
//test_point_bearing = atan2f(_local_pos.vy, _local_pos.vx);
}
}
@ -962,8 +962,15 @@ void Navigator::geofence_breach_check(bool &have_geofence_position_data)
fence_violation_test_point = _gf_breach_avoidance.getFenceViolationTestPoint();
} else {
fence_violation_test_point = matrix::Vector2d(_global_pos.lat, _global_pos.lon);
vertical_test_point_distance = 0;
//fence_violation_test_point = matrix::Vector2d(_global_pos.lat, _global_pos.lon);
if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS) {
fence_violation_test_point = matrix::Vector2d(_gps_pos.lat, _gps_pos.lon);
} else {
fence_violation_test_point = matrix::Vector2d(_gps_pos.lat * 1.0e-7, _gps_pos.lon * 1.0e-7);
}
vertical_test_point_distance = 0;
}
gf_violation_type.flags.dist_to_home_exceeded = !_geofence.isCloserThanMaxDistToHome(fence_violation_test_point(0),