diff --git a/src/drivers/osd/atxxxx/atxxxx.cpp b/src/drivers/osd/atxxxx/atxxxx.cpp index 2d5c7a34c7..97f16d72f6 100644 --- a/src/drivers/osd/atxxxx/atxxxx.cpp +++ b/src/drivers/osd/atxxxx/atxxxx.cpp @@ -36,10 +36,15 @@ * @author Daniele Pettenuzzo * @author Beat Küng * - * Driver for the ATXXXX chip on the omnibus fcu connected via SPI. + * Driver for the ATXXXX chip (e.g. MAX7456) on the omnibus f4 fcu connected via SPI. */ #include "atxxxx.h" +#include "symbols.h" + +#include +#include +#include struct work_s OSDatxxxx::_work = {}; @@ -111,8 +116,11 @@ OSDatxxxx::init() return ret; } + // clear the screen + int num_rows = _param_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 < 17; j++) { + for (int j = 0; j < num_rows; j++) { add_character_to_screen(' ', i, j); } } @@ -165,7 +173,7 @@ OSDatxxxx::probe() ret |= readRegister(0x00, &data, 1); if (data != 1 || ret != PX4_OK) { - printf("probe error\n"); + PX4_ERR("probe failed (%i %i)", ret, data); } return ret; @@ -186,18 +194,13 @@ OSDatxxxx::init_osd() enable_screen(); - // writeRegister(0x00, 0x48) != PX4_OK) { //DMM set to 0 - // goto fail; - // } - return ret; - } int OSDatxxxx::readRegister(unsigned reg, uint8_t *data, unsigned count) { - uint8_t cmd[5]; // read up to 4 bytes + uint8_t cmd[5]; // read up to 4 bytes int ret; cmd[0] = DIR_READ(reg); @@ -219,7 +222,7 @@ OSDatxxxx::readRegister(unsigned reg, uint8_t *data, unsigned count) int OSDatxxxx::writeRegister(unsigned reg, uint8_t data) { - uint8_t cmd[2]; // write 1 byte + uint8_t cmd[2]; // write 1 byte int ret; cmd[0] = DIR_WRITE(reg); @@ -242,94 +245,115 @@ 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 = PX4_OK; + int ret; if (position > 0xFF) { position_lsb = static_cast(position) - 0xFF; - ret |= writeRegister(0x05, 0x01); //DMAH + ret = writeRegister(0x05, 0x01); //DMAH } else { position_lsb = static_cast(position); - ret |= writeRegister(0x05, 0x00); //DMAH + ret = writeRegister(0x05, 0x00); //DMAH } - ret |= writeRegister(0x06, position_lsb); //DMAL - ret |= writeRegister(0x07, c); + if (ret != 0) { return ret; } + + ret = writeRegister(0x06, position_lsb); //DMAL + if (ret != 0) { return ret; } + + ret = writeRegister(0x07, c); return ret; } -int -OSDatxxxx::add_battery_symbol(uint8_t pos_x, uint8_t pos_y) +void OSDatxxxx::add_string_to_screen_centered(const char *str, uint8_t pos_y, int max_length) { - return add_character_to_screen(146, pos_x, pos_y); + int len = strlen(str); + + if (len > max_length) { + len = max_length; + } + + int pos = (OSD_CHARS_PER_ROW - max_length) / 2; + int before = (max_length - len) / 2; + + for (int i = 0; i < before; ++i) { + add_character_to_screen(' ', pos++, pos_y); + } + + for (int i = 0; i < len; ++i) { + add_character_to_screen(str[i], pos++, pos_y); + } + + while (pos < (OSD_CHARS_PER_ROW + max_length) / 2) { + add_character_to_screen(' ', pos++, pos_y); + } +} + +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); + } } int OSDatxxxx::add_battery_info(uint8_t pos_x, uint8_t pos_y) { - char buf[5]; + char buf[10]; int ret = PX4_OK; - sprintf(buf, "%4.2f", (double)_battery_voltage_filtered_v); + // TODO: show battery symbol based on battery fill level + snprintf(buf, sizeof(buf), "%c%5.2f", OSD_SYMBOL_BATT_3, (double)_battery_voltage_filtered_v); + buf[sizeof(buf) - 1] = '\0'; - for (int i = 0; i < 5; i++) { + for (int i = 0; buf[i] != '\0'; i++) { ret |= add_character_to_screen(buf[i], pos_x + i, pos_y); } ret |= add_character_to_screen('V', pos_x + 5, pos_y); pos_y++; + pos_x++; - sprintf(buf, "%4d", (int)_battery_discharge_mah); + snprintf(buf, sizeof(buf), "%5d", (int)_battery_discharge_mah); + buf[sizeof(buf) - 1] = '\0'; - for (int i = 0; i < 5; i++) { + for (int i = 0; buf[i] != '\0'; i++) { ret |= add_character_to_screen(buf[i], pos_x + i, pos_y); } - ret |= add_character_to_screen(7, pos_x + 5, pos_y); // mAh symbol + ret |= add_character_to_screen(OSD_SYMBOL_MAH, pos_x + 5, pos_y); return ret; } -int -OSDatxxxx::add_altitude_symbol(uint8_t pos_x, uint8_t pos_y) -{ - return add_character_to_screen(154, pos_x, pos_y); -} - int OSDatxxxx::add_altitude(uint8_t pos_x, uint8_t pos_y) { - char buf[5]; + char buf[16]; int ret = PX4_OK; - sprintf(buf, "%4.2f", (double)_local_position_z); + snprintf(buf, sizeof(buf), "%c%5.2f%c", OSD_SYMBOL_ARROW_NORTH, (double)_local_position_z, OSD_SYMBOL_M); + buf[sizeof(buf) - 1] = '\0'; - for (int i = 0; i < 5; i++) { + for (int i = 0; buf[i] != '\0'; i++) { ret |= add_character_to_screen(buf[i], pos_x + i, pos_y); } - ret |= add_character_to_screen('m', pos_x + 5, pos_y); - return ret; } -int -OSDatxxxx::add_flighttime_symbol(uint8_t pos_x, uint8_t pos_y) -{ - return add_character_to_screen(112, pos_x, pos_y); -} - int OSDatxxxx::add_flighttime(float flight_time, uint8_t pos_x, uint8_t pos_y) { - char buf[6]; + char buf[10]; int ret = PX4_OK; - sprintf(buf, "%5.1f", (double)flight_time); + snprintf(buf, sizeof(buf), "%c%5.1f", OSD_SYMBOL_FLIGHT_TIME, (double)flight_time); + buf[sizeof(buf) - 1] = '\0'; - for (int i = 0; i < 6; i++) { + for (int i = 0; buf[i] != '\0'; i++) { ret |= add_character_to_screen(buf[i], pos_x + i, pos_y); } @@ -362,18 +386,15 @@ OSDatxxxx::disable_screen() int -OSDatxxxx::update_topics()//TODO have an argument to choose what to update and return validity +OSDatxxxx::update_topics() { - struct battery_status_s battery = {}; - // struct vehicle_local_position_s local_position = {}; - struct vehicle_status_s vehicle_status = {}; - 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.connected) { @@ -387,61 +408,107 @@ OSDatxxxx::update_topics()//TODO have an argument to choose what to update and r } /* update vehicle local position subscription */ - // orb_check(_local_position_sub, &updated); + orb_check(_local_position_sub, &updated); - // if (updated) { - // if (local_position.z_valid) { - // _local_position_z = -local_position.z; - // _local_position_valid = true; + if (updated) { + vehicle_local_position_s local_position; + orb_copy(ORB_ID(vehicle_local_position), _local_position_sub, &local_position); - // } else { - // _local_position_valid = false; - // } - // } + if ((_local_position_valid = local_position.z_valid)) { + _local_position_z = -local_position.z; + } + } /* update vehicle status subscription */ orb_check(_vehicle_status_sub, &updated); if (updated) { - if (vehicle_status.arming_state == 2 && _arming_state == 1) { + vehicle_status_s vehicle_status; + orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &vehicle_status); + + if (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED && + _arming_state != vehicle_status_s::ARMING_STATE_ARMED) { + // arming _arming_timestamp = hrt_absolute_time(); - _arming_state = 2; - } else if (vehicle_status.arming_state == 1 && _arming_state == 2) { - _arming_state = 1; + } else if (vehicle_status.arming_state != vehicle_status_s::ARMING_STATE_ARMED && + _arming_state == vehicle_status_s::ARMING_STATE_ARMED) { + // disarming } + _arming_state = vehicle_status.arming_state; - if (vehicle_status.nav_state == vehicle_status.NAVIGATION_STATE_ACRO) { - add_character_to_screen('A', 1, 7); - add_character_to_screen('C', 2, 7); - add_character_to_screen('R', 3, 7); - add_character_to_screen('O', 4, 7); - - } else if (vehicle_status.nav_state == vehicle_status.NAVIGATION_STATE_STAB) { - add_character_to_screen('S', 1, 8); - add_character_to_screen('T', 2, 8); - add_character_to_screen('A', 3, 8); - add_character_to_screen('B', 4, 8); - add_character_to_screen('I', 5, 8); - add_character_to_screen('L', 6, 8); - add_character_to_screen('I', 7, 8); - add_character_to_screen('Z', 8, 8); - add_character_to_screen('E', 9, 8); - - } else if (vehicle_status.nav_state == vehicle_status.NAVIGATION_STATE_MANUAL) { - // add_character_to_screen('A', uint8_t pos_x, uint8_t pos_y) - // add_character_to_screen('C', uint8_t pos_x, uint8_t pos_y) - // add_character_to_screen('R', uint8_t pos_x, uint8_t pos_y) - // add_character_to_screen('O', uint8_t pos_x, uint8_t pos_y) - } - - // _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 *flight_mode = "UNKNOWN"; + + switch (nav_state) { + case vehicle_status_s::NAVIGATION_STATE_MANUAL: + flight_mode = "MANUAL"; + break; + + case vehicle_status_s::NAVIGATION_STATE_ALTCTL: + flight_mode = "ALTITUDE"; + break; + + case vehicle_status_s::NAVIGATION_STATE_POSCTL: + flight_mode = "POSITION"; + break; + + case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: + flight_mode = "RETURN"; + break; + + case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: + flight_mode = "MISSION"; + break; + + case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER: + case vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER: + case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS: + case vehicle_status_s::NAVIGATION_STATE_DESCEND: + case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF: + case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND: + case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET: + case vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND: + flight_mode = "AUTO"; + break; + + case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL: + case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL: + flight_mode = "FAILURE"; + break; + + case vehicle_status_s::NAVIGATION_STATE_ACRO: + flight_mode = "ACRO"; + break; + + case vehicle_status_s::NAVIGATION_STATE_TERMINATION: + flight_mode = "TERMINATE"; + break; + + case vehicle_status_s::NAVIGATION_STATE_OFFBOARD: + flight_mode = "OFFBOARD"; + break; + + case vehicle_status_s::NAVIGATION_STATE_STAB: + flight_mode = "STABILIZED"; + break; + + case vehicle_status_s::NAVIGATION_STATE_RATTITUDE: + flight_mode = "RATTITUDE"; + break; + } + + return flight_mode; +} + int OSDatxxxx::update_screen() @@ -449,22 +516,31 @@ OSDatxxxx::update_screen() int ret = PX4_OK; if (_battery_valid) { - ret |= add_battery_symbol(1, 1); - ret |= add_battery_info(2, 1); + ret |= add_battery_info(1, 1); + + } else { + clear_line(1, 1, 10); + clear_line(1, 2, 10); } if (_local_position_valid) { - ret |= add_altitude_symbol(1, 3); - ret |= add_altitude(2, 3); + ret |= add_altitude(1, 3); + + } else { + clear_line(1, 3, 10); } - // if (_arming_state == 2) { - float flight_time_sec = static_cast((hrt_absolute_time() - _arming_timestamp) / (1.0e6)); - ret |= add_flighttime_symbol(1, 5); - ret |= add_flighttime(flight_time_sec, 2, 5); - // } + const char *flight_mode = ""; - // enable_screen(); + if (_arming_state == vehicle_status_s::ARMING_STATE_ARMED) { + float flight_time_sec = static_cast((hrt_absolute_time() - _arming_timestamp) / (1e6f)); + ret |= add_flighttime(flight_time_sec, 1, 14); + + } else { + flight_mode = get_flight_mode(_nav_state); + } + + add_string_to_screen_centered(flight_mode, 12, 10); return ret; @@ -489,9 +565,7 @@ OSDatxxxx::cycle() update_topics(); - if (_battery_valid || _local_position_valid || _arming_state > 1) { - update_screen(); - } + update_screen(); /* schedule a fresh cycle call when the measurement is done */ work_queue(LPWORK, @@ -512,6 +586,8 @@ int OSDatxxxx::print_usage(const char *reason) R"DESCR_STR( ### Description OSD driver for the ATXXXX chip that is mounted on the OmnibusF4SD board for example. + +It can be enabled with the OSD_ATXXXX_CFG parameter. )DESCR_STR"); PRINT_MODULE_USAGE_NAME("atxxxx", "driver"); diff --git a/src/drivers/osd/atxxxx/atxxxx.h b/src/drivers/osd/atxxxx/atxxxx.h index ca15b85f61..55b42fe5cd 100644 --- a/src/drivers/osd/atxxxx/atxxxx.h +++ b/src/drivers/osd/atxxxx/atxxxx.h @@ -55,11 +55,6 @@ #include #include -#include -#include -#include -#include - /* Configuration Constants */ #ifdef PX4_SPI_BUS_OSD #define OSD_BUS PX4_SPI_BUS_OSD @@ -80,13 +75,15 @@ #define OSD_DEVICE_PATH "/dev/osd" -#define OSD_US 1000 /* 1 ms */ #define OSD_UPDATE_RATE 500000 /* 2 Hz */ #define OSD_CHARS_PER_ROW 30 +#define OSD_NUM_ROWS_PAL 16 +#define OSD_NUM_ROWS_NTSC 13 #define OSD_ZERO_BYTE 0x00 #define OSD_PAL_TX_MODE 0x40 /* OSD Registers addresses */ +// TODO #ifndef CONFIG_SCHED_WORKQUEUE @@ -141,18 +138,20 @@ private: int writeRegister(unsigned reg, uint8_t data); int add_character_to_screen(char c, uint8_t pos_x, uint8_t pos_y); - int add_battery_symbol(uint8_t pos_x, uint8_t pos_y); + void add_string_to_screen_centered(const char *str, uint8_t pos_y, int max_length); + void clear_line(uint8_t pos_x, uint8_t pos_y, int length); + int add_battery_info(uint8_t pos_x, uint8_t pos_y); - int add_altitude_symbol(uint8_t pos_x, uint8_t pos_y); int add_altitude(uint8_t pos_x, uint8_t pos_y); - int add_flighttime_symbol(uint8_t pos_x, uint8_t pos_y); int add_flighttime(float flight_time, uint8_t pos_x, uint8_t pos_y); + static const char *get_flight_mode(uint8_t nav_state); + int enable_screen(); int disable_screen(); int update_topics(); - int update_screen(); + int update_screen(); static work_s _work; @@ -173,6 +172,9 @@ private: uint8_t _arming_state{0}; uint64_t _arming_timestamp{0}; + // flight mode + uint8_t _nav_state{0}; + DEFINE_PARAMETERS( (ParamInt) _param_atxxxx_cfg ) diff --git a/src/drivers/osd/atxxxx/params.c b/src/drivers/osd/atxxxx/params.c index 9e67a7f58f..1836522c94 100644 --- a/src/drivers/osd/atxxxx/params.c +++ b/src/drivers/osd/atxxxx/params.c @@ -42,6 +42,7 @@ * @value 2 PAL * * @reboot_required true +* @group OSD * */ PARAM_DEFINE_INT32(OSD_ATXXXX_CFG, 0); diff --git a/src/drivers/osd/atxxxx/symbols.h b/src/drivers/osd/atxxxx/symbols.h new file mode 100644 index 0000000000..52d5289e99 --- /dev/null +++ b/src/drivers/osd/atxxxx/symbols.h @@ -0,0 +1,119 @@ +/**************************************************************************** + * + * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +/* special symbols (the chip comes preflashed with these) */ + +#define OSD_SYMBOL_RSSI 0x01 +// Throttle Position (%) +#define OSD_SYMBOL_THR 0x04 +#define OSD_SYMBOL_THR1 0x05 +// Map mode +#define OSD_SYMBOL_HOME 0x04 +#define OSD_SYMBOL_AIRCRAFT 0x05 +// Unit Icon (Metric) +#define OSD_SYMBOL_M 0x0C +// Unit Icon (Imperial) +#define OSD_SYMBOL_FT 0x0F +// Heading Graphics +#define OSD_SYMBOL_HEADING_N 0x18 +#define OSD_SYMBOL_HEADING_S 0x19 +#define OSD_SYMBOL_HEADING_E 0x1A +#define OSD_SYMBOL_HEADING_W 0x1B +#define OSD_SYMBOL_HEADING_DIVIDED_LINE 0x1C +#define OSD_SYMBOL_HEADING_LINE 0x1D +// Artificial Horizon Center screen Graphics +#define OSD_SYMBOL_AH_CENTER_LINE 0x26 +#define OSD_SYMBOL_AH_CENTER_LINE_RIGHT 0x27 +#define OSD_SYMBOL_AH_CENTER 0x7E +#define OSD_SYMBOL_AH_RIGHT 0x02 +#define OSD_SYMBOL_AH_LEFT 0x03 +#define OSD_SYMBOL_AH_DECORATION 0x13 +// Satellite Graphics +#define OSD_SYMBOL_SAT_L 0x1E +#define OSD_SYMBOL_SAT_R 0x1F +// Direction arrows +#define OSD_SYMBOL_ARROW_SOUTH 0x60 +#define OSD_SYMBOL_ARROW_2 0x61 +#define OSD_SYMBOL_ARROW_3 0x62 +#define OSD_SYMBOL_ARROW_4 0x63 +#define OSD_SYMBOL_ARROW_EAST 0x64 +#define OSD_SYMBOL_ARROW_6 0x65 +#define OSD_SYMBOL_ARROW_7 0x66 +#define OSD_SYMBOL_ARROW_8 0x67 +#define OSD_SYMBOL_ARROW_NORTH 0x68 +#define OSD_SYMBOL_ARROW_10 0x69 +#define OSD_SYMBOL_ARROW_11 0x6A +#define OSD_SYMBOL_ARROW_12 0x6B +#define OSD_SYMBOL_ARROW_WEST 0x6C +#define OSD_SYMBOL_ARROW_14 0x6D +#define OSD_SYMBOL_ARROW_15 0x6E +#define OSD_SYMBOL_ARROW_16 0x6F +// Artifical Horizon Bars +#define OSD_SYMBOL_AH_BAR9_0 0x80 +#define OSD_SYMBOL_AH_BAR9_1 0x81 +#define OSD_SYMBOL_AH_BAR9_2 0x82 +#define OSD_SYMBOL_AH_BAR9_3 0x83 +#define OSD_SYMBOL_AH_BAR9_4 0x84 +#define OSD_SYMBOL_AH_BAR9_5 0x85 +#define OSD_SYMBOL_AH_BAR9_6 0x86 +#define OSD_SYMBOL_AH_BAR9_7 0x87 +#define OSD_SYMBOL_AH_BAR9_8 0x88 +// Progress bar +#define OSD_SYMBOL_PB_START 0x8A +#define OSD_SYMBOL_PB_FULL 0x8B +#define OSD_SYMBOL_PB_HALF 0x8C +#define OSD_SYMBOL_PB_EMPTY 0x8D +#define OSD_SYMBOL_PB_END 0x8E +#define OSD_SYMBOL_PB_CLOSE 0x8F +// Batt evolution +#define OSD_SYMBOL_BATT_FULL 0x90 +#define OSD_SYMBOL_BATT_5 0x91 +#define OSD_SYMBOL_BATT_4 0x92 +#define OSD_SYMBOL_BATT_3 0x93 +#define OSD_SYMBOL_BATT_2 0x94 +#define OSD_SYMBOL_BATT_1 0x95 +#define OSD_SYMBOL_BATT_EMPTY 0x96 +// Batt Icon +#define OSD_SYMBOL_MAIN_BATT 0x97 +// Voltage and amperage +#define OSD_SYMBOL_VOLT 0x06 +#define OSD_SYMBOL_AMP 0x9A +#define OSD_SYMBOL_MAH 0x07 +#define OSD_SYMBOL_WATT 0x57 +// Time +#define OSD_SYMBOL_ON_M 0x9B +#define OSD_SYMBOL_FLY_M 0x9C +#define OSD_SYMBOL_FLIGHT_TIME 0x70 +