yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

heater move to new WQ and uORB::Subscription

Browse Source
This commit is contained in:
Daniel Agar
2019-06-04 12:59:14 -04:00
parent 4c42cac380
commit ca5651bd8b
2 changed files with 28 additions and 113 deletions
Download Patch File Download Diff File Expand all files Collapse all files

104
src/drivers/heater/heater.cpp
View File

@@ -49,14 +49,12 @@
#error "To use the heater driver, the board_config.h must define and initialize GPIO_HEATER_INPUT and GPIO_HEATER_OUTPUT"
#endif
struct work_s Heater::_work = {};
Heater::Heater() :
ModuleParams(nullptr)
ModuleParams(nullptr),
ScheduledWorkItem(px4::wq_configurations::lp_default)
{
px4_arch_configgpio(GPIO_HEATER_OUTPUT);
px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, 0);
_params_sub = orb_subscribe(ORB_ID(parameter_update));
}
Heater::~Heater()
@@ -70,13 +68,6 @@ Heater::~Heater()
px4_arch_configgpio(GPIO_HEATER_OUTPUT);
px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, 0);
}
// Unsubscribe from uORB topics.
orb_unsubscribe(_params_sub);
if (_sensor_accel_sub >= 0) {
orb_unsubscribe(_sensor_accel_sub);
}
}
int Heater::controller_period(char *argv[])
@@ -128,7 +119,7 @@ int Heater::custom_command(int argc, char *argv[])
return PX4_OK;
}
void Heater::cycle()
void Heater::Run()
{
if (should_exit()) {
exit_and_cleanup();
@@ -150,7 +141,7 @@ void Heater::cycle()
} else {
update_params(false);
orb_update(ORB_ID(sensor_accel), _sensor_accel_sub, &_sensor_accel);
_sensor_accel_sub.update(&_sensor_accel);
// Obtain the current IMU sensor temperature.
_sensor_temperature = _sensor_accel.temperature;
@@ -181,45 +172,33 @@ void Heater::cycle()
// Schedule the next cycle.
if (_heater_on) {
work_queue(LPWORK, &_work, (worker_t)&Heater::cycle_trampoline, this,
USEC2TICK(_controller_time_on_usec));
ScheduleDelayed(_controller_time_on_usec);
} else {
work_queue(LPWORK, &_work, (worker_t)&Heater::cycle_trampoline, this,
USEC2TICK(_controller_period_usec - _controller_time_on_usec));
ScheduleDelayed(_controller_period_usec - _controller_time_on_usec);
}
}
void Heater::cycle_trampoline(void *argv)
{
Heater *obj = reinterpret_cast<Heater *>(argv);
obj->cycle();
}
void Heater::initialize_topics()
{
// Get the total number of accelerometer instances.
size_t number_of_imus = orb_group_count(ORB_ID(sensor_accel));
uint8_t number_of_imus = orb_group_count(ORB_ID(sensor_accel));
// Check each instance for the correct ID.
for (size_t x = 0; x < number_of_imus; x++) {
_sensor_accel_sub = orb_subscribe_multi(ORB_ID(sensor_accel), (int)x);
for (uint8_t x = 0; x < number_of_imus; x++) {
_sensor_accel_sub = uORB::Subscription{ORB_ID(sensor_accel), x};
if (_sensor_accel_sub < 0) {
if (!_sensor_accel_sub.published()) {
continue;
}
while (orb_update(ORB_ID(sensor_accel), _sensor_accel_sub, &_sensor_accel) != PX4_OK) {
usleep(200000);
}
_sensor_accel_sub.copy(&_sensor_accel);
// If the correct ID is found, exit the for-loop with _sensor_accel_sub pointing to the correct instance.
if (_sensor_accel.device_id == (uint32_t)_param_sens_temp_id.get()) {
PX4_INFO("IMU sensor identified.");
break;
}
orb_unsubscribe(_sensor_accel_sub);
}
PX4_INFO("Device ID: %d", _sensor_accel.device_id);
@@ -231,19 +210,6 @@ void Heater::initialize_topics()
}
}
void Heater::initialize_trampoline(void *argv)
{
Heater *heater = new Heater();
if (!heater) {
PX4_ERR("driver allocation failed");
return;
}
_object.store(heater);
heater->start();
}
float Heater::integrator(char *argv[])
{
if (argv[1]) {
@@ -254,26 +220,6 @@ float Heater::integrator(char *argv[])
return _param_sens_imu_temp_i.get();
}
int Heater::orb_update(const struct orb_metadata *meta, int handle, void *buffer)
{
bool newData = false;
// Check if there is new data to obtain.
if (orb_check(handle, &newData) != OK) {
return PX4_ERROR;
}
if (!newData) {
return PX4_ERROR;
}
if (orb_copy(meta, handle, buffer) != OK) {
return PX4_ERROR;
}
return PX4_OK;
}
int Heater::print_status()
{
PX4_INFO("Temperature: %3.3fC - Setpoint: %3.2fC - Heater State: %s",
@@ -339,8 +285,7 @@ int Heater::start()
update_params(true);
initialize_topics();
// Kick off the cycling. We can call it directly because we're already in the work queue context
cycle();
ScheduleNow();
PX4_INFO("Driver started successfully.");
@@ -349,19 +294,18 @@ int Heater::start()
int Heater::task_spawn(int argc, char *argv[])
{
int ret = work_queue(LPWORK, &_work, (worker_t)&Heater::initialize_trampoline, nullptr, 0);
Heater *heater = new Heater();
if (ret < 0) {
return ret;
}
ret = wait_until_running();
if (ret < 0) {
return ret;
if (!heater) {
PX4_ERR("driver allocation failed");
return PX4_ERROR;
}
_object.store(heater);
_task_id = task_id_is_work_queue;
heater->start();
return 0;
}
@@ -377,18 +321,12 @@ float Heater::temperature_setpoint(char *argv[])
void Heater::update_params(const bool force)
{
bool updated;
parameter_update_s param_update;
orb_check(_params_sub, &updated);
if (updated || force) {
if (_params_sub.update(&param_update) || force) {
ModuleParams::updateParams();
orb_copy(ORB_ID(parameter_update), _params_sub, &param_update);
}
}
/**
* Main entry point for the heater driver module
*/

37
src/drivers/heater/heater.h
View File

@@ -41,13 +41,13 @@
#pragma once
#include <px4_workqueue.h>
#include <px4_work_queue/ScheduledWorkItem.hpp>
#include <px4_module.h>
#include <px4_module_params.h>
#include <px4_config.h>
#include <px4_getopt.h>
#include <uORB/uORB.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_accel.h>
@@ -62,7 +62,7 @@
extern "C" __EXPORT int heater_main(int argc, char *argv[]);
class Heater : public ModuleBase<Heater>, public ModuleParams
class Heater : public ModuleBase<Heater>, public ModuleParams, public px4::ScheduledWorkItem
{
public:
Heater();
@@ -149,36 +149,14 @@ protected:
*/
void initialize_topics();
/**
* @see ModuleBase::initialize_trampoline().
* @brief Trampoline initialization.
* @param argv Pointer to the task startup arguments.
*/
static void initialize_trampoline(void *argv);
private:
/**
* @brief Trampoline for the work queue.
* @param argv Pointer to the task startup arguments.
*/
static void cycle_trampoline(void *argv);
/**
* @brief Calculates the heater element on/off time, carries out
* closed loop feedback and feedforward temperature control,
* and schedules the next cycle.
*/
void cycle();
/**
* @brief Updates the uORB topics for local subscribers.
* @param meta The uORB metadata to copy.
* @param handle The uORB handle to obtain data from.
* @param buffer The data buffer to copy data into.
* @return Returns true iff update was successful.
*/
int orb_update(const struct orb_metadata *meta, int handle, void *buffer);
void Run() override;
/**
* @brief Updates and checks for updated uORB parameters.
@@ -197,13 +175,12 @@ private:
float _integrator_value = 0.0f;
int _params_sub = 0;
uORB::Subscription _params_sub{ORB_ID(parameter_update)};
float _proportional_value = 0.0f;
struct sensor_accel_s _sensor_accel = {};
int _sensor_accel_sub = -1;
uORB::Subscription _sensor_accel_sub{ORB_ID(sensor_accel)};
sensor_accel_s _sensor_accel{};
float _sensor_temperature = 0.0f;