Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

osd/atxxxx move to new WQ and uORB::Subscription #12173

Merged
merged 1 commit into from
Jul 29, 2019
Merged

osd/atxxxx move to new WQ and uORB::Subscription #12173

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 2 additions & 0 deletions src/drivers/osd/atxxxx/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -35,4 +35,6 @@ px4_add_module(
MAIN atxxxx
SRCS
atxxxx.cpp
DEPENDS
px4_work_queue
)
180 changes: 70 additions & 110 deletions src/drivers/osd/atxxxx/atxxxx.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -42,30 +42,24 @@
#include "atxxxx.h"
#include "symbols.h"

#include <uORB/topics/battery_status.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_status.h>

struct work_s OSDatxxxx::_work = {};
using namespace time_literals;

static constexpr uint32_t OSD_UPDATE_RATE{500_ms}; // 2 Hz

OSDatxxxx::OSDatxxxx(int bus) :
SPI("OSD", OSD_DEVICE_PATH, bus, PX4_MK_SPI_SEL(bus, OSD_SPIDEV), SPIDEV_MODE0, OSD_SPI_BUS_SPEED),
ModuleParams(nullptr)
SPI("OSD", nullptr, bus, PX4_MK_SPI_SEL(bus, OSD_SPIDEV), SPIDEV_MODE0, OSD_SPI_BUS_SPEED),
ModuleParams(nullptr),
ScheduledWorkItem(px4::device_bus_to_wq(get_device_id()))
{
_battery_sub = orb_subscribe(ORB_ID(battery_status));
_local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position));
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
}

OSDatxxxx::~OSDatxxxx()
{
orb_unsubscribe(_battery_sub);
orb_unsubscribe(_local_position_sub);
orb_unsubscribe(_vehicle_status_sub);
ScheduleClear();
}

int OSDatxxxx::task_spawn(int argc, char *argv[])
int
OSDatxxxx::task_spawn(int argc, char *argv[])
{
int ch;
int myoptind = 1;
Expand All @@ -80,23 +74,25 @@ int OSDatxxxx::task_spawn(int argc, char *argv[])
}
}

int ret = work_queue(LPWORK, &_work, (worker_t)&OSDatxxxx::initialize_trampoline, (void *)(long)spi_bus, 0);
OSDatxxxx *osd = new OSDatxxxx(spi_bus);

if (ret < 0) {
return ret;
if (!osd) {
PX4_ERR("alloc failed");
return PX4_ERROR;
}

ret = wait_until_running();

if (ret < 0) {
return ret;
if (osd->init() != PX4_OK) {
delete osd;
return PX4_ERROR;
}

_object.store(osd);
_task_id = task_id_is_work_queue;

return 0;
}
osd->start();

return PX4_OK;
}

int
OSDatxxxx::init()
Expand All @@ -108,16 +104,20 @@ OSDatxxxx::init()
return ret;
}

if ((ret = reset()) != PX4_OK) {
ret = reset();

if (ret != PX4_OK) {
return ret;
}

if ((ret = init_osd()) != PX4_OK) {
ret = init_osd();

if (ret != PX4_OK) {
return ret;
}

// clear the screen
int num_rows = _param_osd_atxxxx_cfg.get() == 1 ? OSD_NUM_ROWS_NTSC : OSD_NUM_ROWS_PAL;
int num_rows = (_param_osd_atxxxx_cfg.get() == 1 ? OSD_NUM_ROWS_NTSC : OSD_NUM_ROWS_PAL);

for (int i = 0; i < OSD_CHARS_PER_ROW; i++) {
for (int j = 0; j < num_rows; j++) {
Expand All @@ -128,41 +128,14 @@ OSDatxxxx::init()
return ret;
}

int OSDatxxxx::start()
{
if (is_running()) {
return 0;
}

init();

// Kick off the cycling. We can call it directly because we're already in the work queue context.
cycle();

return 0;
}

void OSDatxxxx::initialize_trampoline(void *arg)
{
OSDatxxxx *osd = new OSDatxxxx((long)arg);

if (!osd) {
PX4_ERR("alloc failed");
return;
}

osd->start();
_object.store(osd);
}

void OSDatxxxx::cycle_trampoline(void *arg)
int
OSDatxxxx::start()
{
OSDatxxxx *obj = reinterpret_cast<OSDatxxxx *>(arg);
ScheduleOnInterval(OSD_UPDATE_RATE, 10000);

obj->cycle();
return PX4_OK;
}


int
OSDatxxxx::probe()
{
Expand Down Expand Up @@ -200,52 +173,46 @@ OSDatxxxx::init_osd()
int
OSDatxxxx::readRegister(unsigned reg, uint8_t *data, unsigned count)
{
uint8_t cmd[5]; // read up to 4 bytes
int ret;
uint8_t cmd[5] {}; // read up to 4 bytes

cmd[0] = DIR_READ(reg);

ret = transfer(&cmd[0], &cmd[0], count + 1);
int ret = transfer(&cmd[0], &cmd[0], count + 1);

if (OK != ret) {
if (ret != PX4_OK) {
DEVICE_LOG("spi::transfer returned %d", ret);
return ret;
}

memcpy(&data[0], &cmd[1], count);

return ret;

}


int
OSDatxxxx::writeRegister(unsigned reg, uint8_t data)
{
uint8_t cmd[2]; // write 1 byte
int ret;
uint8_t cmd[2] {}; // write 1 byte

cmd[0] = DIR_WRITE(reg);
cmd[1] = data;

ret = transfer(&cmd[0], nullptr, 2);
int ret = transfer(&cmd[0], nullptr, 2);

if (OK != ret) {
DEVICE_LOG("spi::transfer returned %d", ret);
return ret;
}

return ret;

}

int
OSDatxxxx::add_character_to_screen(char c, uint8_t pos_x, uint8_t pos_y)
{

uint16_t position = (OSD_CHARS_PER_ROW * pos_y) + pos_x;
uint8_t position_lsb;
int ret;
uint8_t position_lsb = 0;
int ret = PX4_ERROR;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This change is unnecessary.

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I know, but I'd rather initialize everything as a general rule.


if (position > 0xFF) {
position_lsb = static_cast<uint8_t>(position) - 0xFF;
Expand All @@ -256,17 +223,23 @@ OSDatxxxx::add_character_to_screen(char c, uint8_t pos_x, uint8_t pos_y)
ret = writeRegister(0x05, 0x00); //DMAH
}

if (ret != 0) { return ret; }
if (ret != 0) {
return ret;
}

ret = writeRegister(0x06, position_lsb); //DMAL

if (ret != 0) { return ret; }
if (ret != 0) {
return ret;
}

ret = writeRegister(0x07, c);

return ret;
}

void OSDatxxxx::add_string_to_screen_centered(const char *str, uint8_t pos_y, int max_length)
void
OSDatxxxx::add_string_to_screen_centered(const char *str, uint8_t pos_y, int max_length)
{
int len = strlen(str);

Expand All @@ -290,7 +263,8 @@ void OSDatxxxx::add_string_to_screen_centered(const char *str, uint8_t pos_y, in
}
}

void OSDatxxxx::clear_line(uint8_t pos_x, uint8_t pos_y, int length)
void
OSDatxxxx::clear_line(uint8_t pos_x, uint8_t pos_y, int length)
{
for (int i = 0; i < length; ++i) {
add_character_to_screen(' ', pos_x + i, pos_y);
Expand Down Expand Up @@ -363,7 +337,7 @@ OSDatxxxx::add_flighttime(float flight_time, uint8_t pos_x, uint8_t pos_y)
int
OSDatxxxx::enable_screen()
{
uint8_t data;
uint8_t data = 0;
int ret = PX4_OK;

ret |= readRegister(0x00, &data, 1);
Expand All @@ -375,7 +349,7 @@ OSDatxxxx::enable_screen()
int
OSDatxxxx::disable_screen()
{
uint8_t data;
uint8_t data = 0;
int ret = PX4_OK;

ret |= readRegister(0x00, &data, 1);
Expand All @@ -384,18 +358,13 @@ OSDatxxxx::disable_screen()
return ret;
}


int
OSDatxxxx::update_topics()
{
bool updated = false;

/* update battery subscription */
orb_check(_battery_sub, &updated);

if (updated) {
battery_status_s battery;
orb_copy(ORB_ID(battery_status), _battery_sub, &battery);
if (_battery_sub.updated()) {
battery_status_s battery{};
_battery_sub.copy(&battery);

if (battery.connected) {
_battery_voltage_filtered_v = battery.voltage_filtered_v;
Expand All @@ -408,23 +377,21 @@ OSDatxxxx::update_topics()
}

/* update vehicle local position subscription */
orb_check(_local_position_sub, &updated);
if (_local_position_sub.updated()) {
vehicle_local_position_s local_position{};
_local_position_sub.copy(&local_position);

if (updated) {
vehicle_local_position_s local_position;
orb_copy(ORB_ID(vehicle_local_position), _local_position_sub, &local_position);
_local_position_valid = local_position.z_valid;

if ((_local_position_valid = local_position.z_valid)) {
if (_local_position_valid) {
_local_position_z = -local_position.z;
}
}

/* update vehicle status subscription */
orb_check(_vehicle_status_sub, &updated);

if (updated) {
vehicle_status_s vehicle_status;
orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &vehicle_status);
if (_vehicle_status_sub.updated()) {
vehicle_status_s vehicle_status{};
_vehicle_status_sub.copy(&vehicle_status);

if (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED &&
_arming_state != vehicle_status_s::ARMING_STATE_ARMED) {
Expand All @@ -437,14 +404,14 @@ OSDatxxxx::update_topics()
}

_arming_state = vehicle_status.arming_state;

_nav_state = vehicle_status.nav_state;
}

return PX4_OK;
}

const char *OSDatxxxx::get_flight_mode(uint8_t nav_state)
const char *
OSDatxxxx::get_flight_mode(uint8_t nav_state)
{
const char *flight_mode = "UNKNOWN";

Expand Down Expand Up @@ -509,7 +476,6 @@ const char *OSDatxxxx::get_flight_mode(uint8_t nav_state)
return flight_mode;
}


int
OSDatxxxx::update_screen()
{
Expand Down Expand Up @@ -543,7 +509,6 @@ OSDatxxxx::update_screen()
add_string_to_screen_centered(flight_mode, 12, 10);

return ret;

}

int
Expand All @@ -556,7 +521,7 @@ OSDatxxxx::reset()
}

void
OSDatxxxx::cycle()
OSDatxxxx::Run()
{
if (should_exit()) {
exit_and_cleanup();
Expand All @@ -566,17 +531,10 @@ OSDatxxxx::cycle()
update_topics();

update_screen();

/* schedule a fresh cycle call when the measurement is done */
work_queue(LPWORK,
&_work,
(worker_t)&OSDatxxxx::cycle_trampoline,
this,
USEC2TICK(OSD_UPDATE_RATE));

}

int OSDatxxxx::print_usage(const char *reason)
int
OSDatxxxx::print_usage(const char *reason)
{
if (reason) {
printf("%s\n\n", reason);
Expand All @@ -598,12 +556,14 @@ It can be enabled with the OSD_ATXXXX_CFG parameter.
return 0;
}

int OSDatxxxx::custom_command(int argc, char *argv[])
int
OSDatxxxx::custom_command(int argc, char *argv[])
{
return print_usage("unrecognized command");
}

int atxxxx_main(int argc, char *argv[])
int
atxxxx_main(int argc, char *argv[])
{
return OSDatxxxx::main(argc, argv);
}
Loading