diff --git a/NAV_Algorithms/data_structures.h b/NAV_Algorithms/data_structures.h index 80bdb24..fd143c7 100644 --- a/NAV_Algorithms/data_structures.h +++ b/NAV_Algorithms/data_structures.h @@ -90,7 +90,7 @@ typedef struct float integrator_vario; float3vector wind; float3vector wind_average; - uint32_t circle_mode; + uint32_t flight_mode; quaternion q; eulerangle euler; float effective_vertical_acceleration; diff --git a/NAV_Algorithms/navigator.cpp b/NAV_Algorithms/navigator.cpp index 39e0eed..cf7ae6f 100644 --- a/NAV_Algorithms/navigator.cpp +++ b/NAV_Algorithms/navigator.cpp @@ -143,7 +143,11 @@ void navigator_t::report_data( output_data_t &d) d.effective_vertical_acceleration = variometer.get_effective_vertical_acceleration(); - d.circle_mode = ahrs.get_circling_state(); + if( airborne_detector.is_airborne()) + d.flight_mode = ahrs.get_circling_state(); + else + d.flight_mode = 3; // = ON_GROUND + d.turn_rate = ahrs.get_turn_rate(); d.slip_angle = ahrs.getSlipAngle(); d.pitch_angle = ahrs.getPitchAngle(); diff --git a/Output_Formatter/CAN_output.cpp b/Output_Formatter/CAN_output.cpp index 7213072..8226615 100644 --- a/Output_Formatter/CAN_output.cpp +++ b/Output_Formatter/CAN_output.cpp @@ -44,6 +44,7 @@ enum CAN_ID_SENSOR CAN_Id_SlipPitch = 0x128, //!< float slip angle from body-acc, float pitch angle from body-acc CAN_Id_Voltage_Circle = 0x129, //!< float supply voltage, uint8_t circle-mode CAN_Id_SystemState = 0x12a, //!< u32 system_state, u32 git_tag dec + CAN_Id_Sensor_Health = 0x12b, //!< float magnetic disturbance CAN_Id_GPS_Date_Time = 0x140, //!< uint8_t year-2000, month, day, hour, mins, secs CAN_Id_GPS_Lat = 0x141, //!< double latitude @@ -121,7 +122,12 @@ void CAN_output ( const output_data_t &x, bool horizon_activated) p.id=CAN_Id_Voltage_Circle; p.dlc=5; p.data_f[0] = x.m.supply_voltage; - p.data_b[4] = (uint8_t)(x.circle_mode); + p.data_b[4] = (uint8_t)(x.flight_mode); + CAN_send(p, 1); + + p.id=CAN_Id_Sensor_Health; + p.dlc=4; + p.data_f[0] = x.magnetic_disturbance; CAN_send(p, 1); p.id=CAN_Id_GPS_Date_Time; diff --git a/Output_Formatter/CAN_output.h b/Output_Formatter/CAN_output.h index 0b698f1..9520888 100644 --- a/Output_Formatter/CAN_output.h +++ b/Output_Formatter/CAN_output.h @@ -58,6 +58,7 @@ #define CMD_MEASURE_LEVEL 0x3002 #define CMD_CALCULATE 0x3003 #define CMD_TUNE 0x3004 +#define CMD_RESET_SENSOR 0x3005 void CAN_output ( const output_data_t &x, bool horizon_activated); void CAN_heartbeat( void); diff --git a/Output_Formatter/system_state.h b/Output_Formatter/system_state.h index 16e4f7d..99697a6 100644 --- a/Output_Formatter/system_state.h +++ b/Output_Formatter/system_state.h @@ -24,7 +24,9 @@ enum availability_bits USB_OUTPUT_ACTIVE = 0x1000, BLUEZ_OUTPUT_ACTIVE = 0x2000, CAN_OUTPUT_ACTIVE = 0x4000, - USART_2_OUTPUT_ACTIVE = 0x8000 + USART_2_OUTPUT_ACTIVE = 0x8000, + + HORIZON_NOT_AVAILABLE = 0x10000 }; extern uint32_t system_state; //!< bits collected from availability_bits