void osd_update_values( void ) { switch (osd_phase) { case 0: { #if (OSD_LOC_ALTITUDE != OSD_LOC_DISABLED) osd_spi_write_location(OSD_LOC_ALTITUDE+1) ; osd_spi_write_number(IMUlocationz._.W1, 0, 0, NUM_FLAG_SIGNED, 0, 0) ; // Altitude #endif #if (OSD_LOC_CPU_LOAD != OSD_LOC_DISABLED) osd_spi_write_location(OSD_LOC_CPU_LOAD+1) ; osd_spi_write_number(udb_cpu_load(), 3, 0, 0, 0, 0) ; // CPU #endif #if (OSD_LOC_VARIO_NUM != OSD_LOC_DISABLED) osd_spi_write_location(OSD_LOC_VARIO_NUM) ; osd_spi_write_number(IMUvelocityz._.W1, 0, 0, NUM_FLAG_SIGNED, 0, 0) ; // Variometer #endif #if (OSD_LOC_VARIO_ARROW != OSD_LOC_DISABLED) osd_spi_write_location(OSD_LOC_VARIO_ARROW) ; if (IMUvelocityz._.W1 <= -VARIOMETER_HIGH) osd_spi_write(0x7, 0xD4) ; // Variometer down fast else if (IMUvelocityz._.W1 <= -VARIOMETER_LOW) osd_spi_write(0x7, 0xD2) ; // Variometer down slowly else if (IMUvelocityz._.W1 < VARIOMETER_LOW) osd_spi_write(0x7, 0x00) ; // Variometer flat (was 0xD0) else if (IMUvelocityz._.W1 < VARIOMETER_HIGH) osd_spi_write(0x7, 0xD1) ; // Variometer up slowly else if (IMUvelocityz._.W1 >= VARIOMETER_HIGH) osd_spi_write(0x7, 0xD3) ; // Variometer up fast #endif #if (OSD_LOC_AP_MODE != OSD_LOC_DISABLED) osd_spi_write_location(OSD_LOC_AP_MODE) ; if (!flags._.pitch_feedback) osd_spi_write(0x7, 0x97) ; // M : Manual Mode else if (!flags._.GPS_steering) osd_spi_write(0x7, 0x9D) ; // S : Stabilized Mode else if (udb_flags._.radio_on && !flags._.rtl_hold) osd_spi_write(0x7, 0xA1) ; // W : Waypoint Mode else if (flags._.rtl_hold && udb_flags._.radio_on) osd_spi_write(0x7, 0x92) ; // H : RTL Hold, has signal else osd_spi_write(0x7, 0x9C) ; // R : RTL Mode, lost signal #endif break ; } case 1: { signed char dir_to_goal ; int dist_to_goal ; struct relative2D curHeading ; curHeading.x = -rmat[1] ; curHeading.y = rmat[4] ; signed char earth_yaw = rect_to_polar(&curHeading) ;// 0-255 (0=East, ccw) if (flags._.GPS_steering) { dir_to_goal = desired_dir - earth_yaw ; dist_to_goal = abs(tofinish_line) ; } else { struct relative2D toGoal ; toGoal.x = 0 - IMUlocationx._.W1 ; toGoal.y = 0 - IMUlocationy._.W1 ; dir_to_goal = rect_to_polar ( &toGoal ) - earth_yaw ; dist_to_goal = toGoal.x ; } #if (OSD_LOC_DIST_TO_GOAL != OSD_LOC_DISABLED) osd_spi_write_location(OSD_LOC_DIST_TO_GOAL+1) ; osd_spi_write_number(dist_to_goal, 0, 0, 0, 0, 0) ; // Distance to wp/home #endif #if (OSD_LOC_ARROW_TO_GOAL != OSD_LOC_DISABLED) osd_spi_write_location(OSD_LOC_ARROW_TO_GOAL) ; osd_write_arrow(dir_to_goal) ; #endif #if (OSD_LOC_HEADING_NUM != OSD_LOC_DISABLED) osd_spi_write_location(OSD_LOC_HEADING_NUM+1) ; // earth_yaw // 0-255 (0=East, ccw) int angle = (earth_yaw * 180 + 64) >> 7 ; // 0-359 (0=East, ccw) angle = -angle + 90; // 0-359 (0=North, clockwise) if (angle < 0) angle += 360 ; // 0-359 (0=North, clockwise) osd_spi_write_number(angle, 3, 0, NUM_FLAG_ZERO_PADDED, 0, 0) ; // heading #endif #if (OSD_LOC_HEADING_CARDINAL != OSD_LOC_DISABLED) osd_spi_write_location(OSD_LOC_HEADING_CARDINAL) ; osd_spi_write_string(heading_strings[((unsigned char)(earth_yaw+8))>>4]) ; // heading #endif #if (OSD_LOC_VERTICAL_ANGLE_HOME != OSD_LOC_DISABLED) // Vertical angle from origin to plane int verticalAngle = 0 ; if (dist_to_goal != 0) { struct relative2D componentsToPlane ; componentsToPlane.x = dist_to_goal ; componentsToPlane.y = IMUlocationz._.W1 ; verticalAngle = rect_to_polar(&componentsToPlane) ; // binary angle (0 - 256 = 360 degrees) verticalAngle = (verticalAngle * BYTECIR_TO_DEGREE) >> 16 ; // switch polarity, convert to -180 - 180 degrees } osd_spi_write_location(OSD_LOC_VERTICAL_ANGLE_HOME) ; osd_spi_write_number(verticalAngle, 0, 0, NUM_FLAG_SIGNED, 0, 0x4D); // Footer: Degree symbol #endif #if (OSD_LOC_ROLL_RATE != OSD_LOC_DISABLED) osd_spi_write_location(OSD_LOC_ROLL_RATE) ; osd_spi_write_number(abs(omegagyro[1])/DEGPERSEC, 3, 0, 0, 0, 0) ; // roll rate in degrees/sec/sec #endif #if (OSD_LOC_PITCH_RATE != OSD_LOC_DISABLED) osd_spi_write_location(OSD_LOC_PITCH_RATE) ; osd_spi_write_number(abs(omegagyro[0])/DEGPERSEC, 3, 0, 0, 0, 0) ; // pitch rate in degrees/sec/sec #endif #if (OSD_LOC_YAW_RATE != OSD_LOC_DISABLED) osd_spi_write_location(OSD_LOC_YAW_RATE) ; osd_spi_write_number(abs(omegagyro[2])/DEGPERSEC, 3, 0, 0, 0, 0) ; // yaw rate in degrees/sec/sec #endif #if (ANALOG_CURRENT_INPUT_CHANNEL != CHANNEL_UNUSED) #if (OSD_LOC_BATT_CURRENT != OSD_LOC_DISABLED) osd_spi_write_location(OSD_LOC_BATT_CURRENT) ; osd_spi_write_number(battery_current._.W1, 3, 1, 0, 0, 0xB4) ; // tenths of Amps being used right now #endif #if (OSD_LOC_BATT_USED != OSD_LOC_DISABLED) osd_spi_write_location(OSD_LOC_BATT_USED) ; osd_spi_write_number(battery_mAh_used._.W1, 4, 0, 0, 0, 0xB7) ; // mAh used so far #endif #endif #if (ANALOG_VOLTAGE_INPUT_CHANNEL != CHANNEL_UNUSED) #if (OSD_LOC_BATT_VOLTAGE != OSD_LOC_DISABLED) osd_spi_write_location(OSD_LOC_BATT_VOLTAGE) ; osd_spi_write_number(battery_voltage._.W1, 3, 1, 0, 0, 0xA0) ; // tenths of Volts #endif #endif #if (ANALOG_RSSI_INPUT_CHANNEL != CHANNEL_UNUSED) #if (OSD_LOC_RSSI != OSD_LOC_DISABLED) osd_spi_write_location(OSD_LOC_RSSI) ; osd_spi_write_number(rc_signal_strength, 3, 0, 0, 0, 0xB3) ; // RC Receiver signal strength as 0-100% #endif #endif break ; } case 2: { #if (OSD_SHOW_HORIZON == 1) osd_update_horizon() ; #endif break ; } case 3: { #if (OSD_LOC_AIR_SPEED_M_S != OSD_LOC_DISABLED) osd_spi_write_location(OSD_LOC_AIR_SPEED_M_S) ; osd_spi_write_number(air_speed_3DIMU/100, 3, 0, 0, 0, 0) ; // speed in m/s #endif #if (OSD_LOC_AIR_SPEED_MI_HR != OSD_LOC_DISABLED) osd_spi_write_location(OSD_LOC_AIR_SPEED_MI_HR) ; osd_spi_write_number(air_speed_3DIMU/45, 3, 0, 0, 0, 0) ; // speed in mi/hr #endif #if (OSD_LOC_AIR_SPEED_KM_HR != OSD_LOC_DISABLED) osd_spi_write_location(OSD_LOC_AIR_SPEED_KM_HR) ; osd_spi_write_number(air_speed_3DIMU/28, 3, 0, 0, 0, 0) ; // speed in km/hr #endif #if (OSD_LOC_GROUND_SPEED_M_S != OSD_LOC_DISABLED || OSD_LOC_GROUND_SPEED_MI_HR != OSD_LOC_DISABLED || OSD_LOC_GROUND_SPEED_KM_HR != OSD_LOC_DISABLED) unsigned int ground_speed_3DIMU = vector3_mag ( IMUvelocityx._.W1 , IMUvelocityy._.W1 , IMUvelocityz._.W1 ) ; #endif #if (OSD_LOC_GROUND_SPEED_M_S != OSD_LOC_DISABLED) osd_spi_write_location(OSD_LOC_GROUND_SPEED_M_S) ; osd_spi_write_number(ground_speed_3DIMU/100, 3, 0, 0, 0, 0) ; // speed in m/s #endif #if (OSD_LOC_GROUND_SPEED_MI_HR != OSD_LOC_DISABLED) osd_spi_write_location(OSD_LOC_GROUND_SPEED_MI_HR) ; osd_spi_write_number(ground_speed_3DIMU/45, 3, 0, 0, 0, 0) ; // speed in mi/hr #endif #if (OSD_LOC_GROUND_SPEED_KM_HR != OSD_LOC_DISABLED) osd_spi_write_location(OSD_LOC_GROUND_SPEED_KM_HR) ; osd_spi_write_number(ground_speed_3DIMU/28, 3, 0, 0, 0, 0) ; // speed in km/hr #endif #if (OSD_LOC_VERTICAL_ACCEL != OSD_LOC_DISABLED) osd_spi_write_location(OSD_LOC_VERTICAL_ACCEL) ; union longww gravity_z ; gravity_z.WW = __builtin_mulss(GRAVITY, rmat[8]) << 2; osd_spi_write_number((ZACCEL_VALUE - gravity_z._.W1)/(100*ACCELSCALE), 3, 0, NUM_FLAG_SIGNED, 0, 0) ; // vertical acceleration rate in units of m/sec/sec #endif #if (OSD_LOC_VERTICAL_WIND_SPEED != OSD_LOC_DISABLED) osd_spi_write_location(OSD_LOC_VERTICAL_WIND_SPEED) ; osd_spi_write_number(estimatedWind[2]/10, 4, 1, NUM_FLAG_SIGNED, 0, 0) ; // vertical wind speed in m/s #endif #if (OSD_LOC_TOTAL_ENERGY != OSD_LOC_DISABLED) osd_spi_write_location(OSD_LOC_TOTAL_ENERGY) ; osd_spi_write_number(total_energy, 4, 0, NUM_FLAG_SIGNED, 0, 0) ; // total energy in meters above the origin #endif #if (OSD_AUTO_HIDE_GPS == 1) boolean showGPS = ( IMUlocationz._.W1 < 20 || ground_velocity_magnitudeXY < 150) ; #else boolean showGPS = 1 ; #endif #if (OSD_LOC_NUM_SATS != OSD_LOC_DISABLED) osd_spi_write_location(OSD_LOC_NUM_SATS) ; if (showGPS) { osd_spi_write_number(svs, 0, 0, 0, 0xEB, 0) ; // Num satelites locked, with SatDish icon header } else { osd_spi_erase_chars(3) ; } #endif #if (OSD_LOC_GPS_LAT != OSD_LOC_DISABLED) osd_spi_write_location(OSD_LOC_GPS_LAT) ; if (showGPS) { osd_spi_write_number(labs(lat_gps.WW/10), 8, 6, 0, 0, (lat_gps.WW >= 0) ? 0x98 : 0x9D) ; // Footer: N/S } else { osd_spi_erase_chars(9) ; } #endif #if (OSD_LOC_GPS_LONG != OSD_LOC_DISABLED) osd_spi_write_location(OSD_LOC_GPS_LONG) ; if (showGPS) { osd_spi_write_number(labs(long_gps.WW/10), 9, 6, 0, 0, (long_gps.WW >= 0) ? 0x8F : 0xA1) ; // Footer: E/W } else { osd_spi_erase_chars(10) ; } #endif break ; } } return ; }
void estLocation(void) { static int8_t cog_previous = 64; static int16_t sog_previous = 0; static int16_t climb_rate_previous = 0; static uint16_t velocity_previous = 0; static int16_t location_previous[] = { 0, 0, 0 }; union longbbbb accum; union longww accum_velocity; int8_t cog_circular; int8_t cog_delta; int16_t sog_delta; int16_t climb_rate_delta; #ifdef USE_EXTENDED_NAV int32_t location[3]; #else int16_t location[3]; #endif // USE_EXTENDED_NAV int16_t location_deltaZ; struct relative2D location_deltaXY; struct relative2D velocity_thru_air; int16_t velocity_thru_airz; location_plane(&location[0]); // convert GPS course of 360 degrees to a binary model with 256 accum.WW = __builtin_muluu(COURSEDEG_2_BYTECIR, cog_gps.BB) + 0x00008000; // re-orientate from compass (clockwise) to maths (anti-clockwise) with 0 degrees in East cog_circular = -accum.__.B2 + 64; // compensate for GPS reporting latency. // The dynamic model of the EM406 and uBlox is not well known. // However, it seems likely much of it is simply reporting latency. // This section of the code compensates for reporting latency. // markw: what is the latency? It doesn't appear numerically or as a comment // in the following code. Since this method is called at the GPS reporting rate // it must be assumed to be one reporting interval? if (dcm_flags._.gps_history_valid) { cog_delta = cog_circular - cog_previous; sog_delta = sog_gps.BB - sog_previous; climb_rate_delta = climb_gps.BB - climb_rate_previous; location_deltaXY.x = location[0] - location_previous[0]; location_deltaXY.y = location[1] - location_previous[1]; location_deltaZ = location[2] - location_previous[2]; } else { cog_delta = 0; sog_delta = 0; climb_rate_delta = 0; location_deltaXY.x = location_deltaXY.y = location_deltaZ = 0; } dcm_flags._.gps_history_valid = 1; actual_dir = cog_circular + cog_delta; cog_previous = cog_circular; // Note that all these velocities are in centimeters / second ground_velocity_magnitudeXY = sog_gps.BB + sog_delta; sog_previous = sog_gps.BB; GPSvelocity.z = climb_gps.BB + climb_rate_delta; climb_rate_previous = climb_gps.BB; accum_velocity.WW = (__builtin_mulss(cosine(actual_dir), ground_velocity_magnitudeXY) << 2) + 0x00008000; GPSvelocity.x = accum_velocity._.W1; accum_velocity.WW = (__builtin_mulss(sine(actual_dir), ground_velocity_magnitudeXY) << 2) + 0x00008000; GPSvelocity.y = accum_velocity._.W1; rotate_2D(&location_deltaXY, cog_delta); // this is a key step to account for rotation effects!! GPSlocation.x = location[0] + location_deltaXY.x; GPSlocation.y = location[1] + location_deltaXY.y; GPSlocation.z = location[2] + location_deltaZ; location_previous[0] = location[0]; location_previous[1] = location[1]; location_previous[2] = location[2]; velocity_thru_air.y = GPSvelocity.y - estimatedWind[1]; velocity_thru_air.x = GPSvelocity.x - estimatedWind[0]; velocity_thru_airz = GPSvelocity.z - estimatedWind[2]; #if (HILSIM == 1) air_speed_3DGPS = hilsim_airspeed.BB; // use Xplane as a pitot #else air_speed_3DGPS = vector3_mag(velocity_thru_air.x, velocity_thru_air.y, velocity_thru_airz); #endif calculated_heading = rect_to_polar(&velocity_thru_air); // veclocity_thru_air.x becomes XY air speed as a by product of CORDIC routine in rect_to_polar() air_speed_magnitudeXY = velocity_thru_air.x; // in cm / sec #if (GPS_RATE == 4) forward_acceleration = (air_speed_3DGPS - velocity_previous) << 2; // Ublox enters code 4 times per second #elif (GPS_RATE == 2) forward_acceleration = (air_speed_3DGPS - velocity_previous) << 1; // Ublox enters code 2 times per second #else forward_acceleration = (air_speed_3DGPS - velocity_previous); // EM406 standard GPS enters code once per second #endif velocity_previous = air_speed_3DGPS; }