From 079dfdf2097592312db0a85248c13c90bea2a654 Mon Sep 17 00:00:00 2001 From: chris1seto Date: Fri, 14 Oct 2022 07:54:14 -0700 Subject: [PATCH] drivers/osd/msp_osd: cleanup and small fixes/additions (#20399) - Fixed clearing arming word if flightmode unknown - Added power and cell voltage elements - New OSD layout - Fixed home direction/distance are now correct Co-authored-by: Chris Seto --- src/drivers/osd/msp_osd/module.yaml | 4 +-- src/drivers/osd/msp_osd/msp_osd.cpp | 42 ++++++++++++++++------ src/drivers/osd/msp_osd/uorb_to_msp.cpp | 47 ++++++++++++++++++------- 3 files changed, 69 insertions(+), 24 deletions(-) diff --git a/src/drivers/osd/msp_osd/module.yaml b/src/drivers/osd/msp_osd/module.yaml index 73c02b5c40..39ec327cd6 100644 --- a/src/drivers/osd/msp_osd/module.yaml +++ b/src/drivers/osd/msp_osd/module.yaml @@ -37,9 +37,9 @@ parameters: 16: (unused) PITCH_ANGLE 17: (unused) ROLL_ANGLE 18: (unused) CROSSHAIRS - 19: (unused) AVG_CELL_VOLTAGE + 19: AVG_CELL_VOLTAGE 20: (unused) HORIZON_SIDEBARS - 21: (unused) POWER + 21: POWER default: 16383 # OSD Log Level diff --git a/src/drivers/osd/msp_osd/msp_osd.cpp b/src/drivers/osd/msp_osd/msp_osd.cpp index 437d441c94..1bb62e5612 100644 --- a/src/drivers/osd/msp_osd/msp_osd.cpp +++ b/src/drivers/osd/msp_osd/msp_osd.cpp @@ -71,25 +71,44 @@ // Currently working elements positions (hardcoded) +/* center col + +Speed Power Alt +Rssi cell_voltage mah +craft name + +*/ + // Left const uint16_t osd_gps_lat_pos = 2048; const uint16_t osd_gps_lon_pos = 2080; const uint16_t osd_gps_sats_pos = 2112; -const uint16_t osd_rssi_value_pos = 2176; // Center -const uint16_t osd_home_dir_pos = 2093; -const uint16_t osd_craft_name_pos = 2543; +// Top const uint16_t osd_disarmed_pos = 2125; +const uint16_t osd_home_dir_pos = 2093; +const uint16_t osd_home_dist_pos = 2095; + +// Bottom row 1 +const uint16_t osd_gps_speed_pos = 2413; +const uint16_t osd_power_pos = 2415; +const uint16_t osd_altitude_pos = 2416; + +// Bottom Row 2 +const uint16_t osd_rssi_value_pos = 2445; +const uint16_t osd_avg_cell_voltage_pos = 2446; +const uint16_t osd_mah_drawn_pos = 2449; + +// Bottom Row 3 +const uint16_t osd_craft_name_pos = 2480; // Right const uint16_t osd_main_batt_voltage_pos = 2073; const uint16_t osd_current_draw_pos = 2103; -const uint16_t osd_mah_drawn_pos = 2138; -const uint16_t osd_altitude_pos = 2233; -const uint16_t osd_numerical_vario_pos = 2267; -const uint16_t osd_gps_speed_pos = 2299; -const uint16_t osd_home_dist_pos = 2331; + + +const uint16_t osd_numerical_vario_pos = LOCATION_HIDDEN; MspOsd::MspOsd(const char *device) : ModuleParams(nullptr), @@ -147,15 +166,18 @@ void MspOsd::SendConfig() msp_osd_config.osd_numerical_vario_pos = enabled(SymbolIndex::NUMERICAL_VARIO) ? osd_numerical_vario_pos : LOCATION_HIDDEN; + msp_osd_config.osd_power_pos = enabled(SymbolIndex::POWER) ? osd_power_pos : LOCATION_HIDDEN; + msp_osd_config.osd_avg_cell_voltage_pos = enabled(SymbolIndex::AVG_CELL_VOLTAGE) ? osd_avg_cell_voltage_pos : + LOCATION_HIDDEN; + // possibly available, but not currently used msp_osd_config.osd_flymode_pos = LOCATION_HIDDEN; msp_osd_config.osd_esc_tmp_pos = LOCATION_HIDDEN; msp_osd_config.osd_pitch_angle_pos = LOCATION_HIDDEN; msp_osd_config.osd_roll_angle_pos = LOCATION_HIDDEN; msp_osd_config.osd_crosshairs_pos = LOCATION_HIDDEN; - msp_osd_config.osd_avg_cell_voltage_pos = LOCATION_HIDDEN; + msp_osd_config.osd_horizon_sidebars_pos = LOCATION_HIDDEN; - msp_osd_config.osd_power_pos = LOCATION_HIDDEN; // Not implemented or not available msp_osd_config.osd_artificial_horizon_pos = LOCATION_HIDDEN; diff --git a/src/drivers/osd/msp_osd/uorb_to_msp.cpp b/src/drivers/osd/msp_osd/uorb_to_msp.cpp index d6b7032f7b..f30960ddc5 100644 --- a/src/drivers/osd/msp_osd/uorb_to_msp.cpp +++ b/src/drivers/osd/msp_osd/uorb_to_msp.cpp @@ -261,7 +261,7 @@ msp_status_BF_t construct_STATUS(const vehicle_status_s &vehicle_status) break; default: - status_BF.flight_mode_flags = 0; + status_BF.flight_mode_flags |= 0; break; } } @@ -277,10 +277,9 @@ msp_analog_t construct_ANALOG(const battery_status_s &battery_status, const inpu msp_analog_t analog {0}; analog.vbat = battery_status.voltage_v * 10; // bottom right... v * 10 - analog.rssi = (uint16_t)((input_rc.rssi * 1023.0f) / 100.0f); + analog.rssi = (uint16_t)((input_rc.link_quality * 1023.0f) / 100.0f); analog.amperage = battery_status.current_a * 100; // main amperage analog.mAhDrawn = battery_status.discharged_mah; // unused - return analog; } @@ -290,8 +289,8 @@ msp_battery_state_t construct_BATTERY_STATE(const battery_status_s &battery_stat msp_battery_state_t battery_state = {0}; // MSP_BATTERY_STATE - battery_state.amperage = battery_status.current_a; // not used? - battery_state.batteryVoltage = (uint16_t)(battery_status.voltage_v * 400.0f); // OK + battery_state.amperage = battery_status.current_a * 100.0f; // Used for power element + battery_state.batteryVoltage = (uint16_t)((battery_status.voltage_v / battery_status.cell_count) * 400.0f); // OK battery_state.mAhDrawn = battery_status.discharged_mah ; // OK battery_state.batteryCellCount = battery_status.cell_count; battery_state.batteryCapacity = battery_status.capacity; // not used? @@ -318,14 +317,24 @@ msp_raw_gps_t construct_RAW_GPS(const sensor_gps_s &vehicle_gps_position, raw_gps.lat = vehicle_gps_position.lat; raw_gps.lon = vehicle_gps_position.lon; raw_gps.alt = vehicle_gps_position.alt / 10; - //raw_gps.groundCourse = vehicle_gps_position_struct + + float course = math::degrees(vehicle_gps_position.cog_rad); + + if (course < 0) { + course += 360.0f; + } + + raw_gps.groundCourse = course * 100.0f; // centidegrees } else { raw_gps.lat = 0; raw_gps.lon = 0; raw_gps.alt = 0; + raw_gps.groundCourse = 0; // centidegrees } + raw_gps.groundCourse = 0; // centidegrees + if (vehicle_gps_position.fix_type == 0 || vehicle_gps_position.fix_type == 1) { raw_gps.fixType = MSP_GPS_NO_FIX; @@ -367,20 +376,24 @@ msp_comp_gps_t construct_COMP_GPS(const home_position_s &home_position, if (home_position.valid_hpos && home_position.valid_lpos && estimator_status.solution_status_flags & (1 << 4)) { - float bearing_to_home = get_bearing_to_next_waypoint(vehicle_global_position.lat, - vehicle_global_position.lon, - home_position.lat, home_position.lon); + float bearing_to_home = math::degrees(get_bearing_to_next_waypoint(vehicle_global_position.lat, + vehicle_global_position.lon, + home_position.lat, home_position.lon)); + + if (bearing_to_home < 0) { + bearing_to_home += 360.0f; + } float distance_to_home = get_distance_to_next_waypoint(vehicle_global_position.lat, vehicle_global_position.lon, home_position.lat, home_position.lon); comp_gps.distanceToHome = (int16_t)distance_to_home; // meters - comp_gps.directionToHome = bearing_to_home; // degrees + comp_gps.directionToHome = bearing_to_home; } else { comp_gps.distanceToHome = 0; // meters - comp_gps.directionToHome = 0; // degrees + comp_gps.directionToHome = 0; } comp_gps.heartbeat = heartbeat; @@ -396,7 +409,17 @@ msp_attitude_t construct_ATTITUDE(const vehicle_attitude_s &vehicle_attitude) matrix::Eulerf euler_attitude(matrix::Quatf(vehicle_attitude.q)); attitude.pitch = math::degrees(euler_attitude.theta()) * 10; attitude.roll = math::degrees(euler_attitude.phi()) * 10; - attitude.yaw = math::degrees(euler_attitude.psi()) * 10; + //attitude.yaw = math::degrees(euler_attitude.psi()) * 10; + + float yaw_fixed = math::degrees(euler_attitude.psi()); + + if (yaw_fixed < 0) { + yaw_fixed += 360.0f; + } + + attitude.yaw = yaw_fixed; + + //attitude.yaw = 360; return attitude; }